diff --git a/.dockerignore b/.dockerignore new file mode 100644 index 0000000..a63d738 --- /dev/null +++ b/.dockerignore @@ -0,0 +1,3 @@ +.venv +**/__pycache__ +**/*.pyc diff --git a/.github/workflows/gui-build.yml b/.github/workflows/gui-build.yml new file mode 100644 index 0000000..e706977 --- /dev/null +++ b/.github/workflows/gui-build.yml @@ -0,0 +1,30 @@ +name: GUI build + +on: + push: + paths: + - ".github/workflows/gui-build.yml" + - "docker/base/gui-deps.dockerfile" + - "docker/gui.dockerfile" + - "docker-compose.yml" + - "Makefile" + pull_request: + paths: + - ".github/workflows/gui-build.yml" + - "docker/base/gui-deps.dockerfile" + - "docker/gui.dockerfile" + - "docker-compose.yml" + - "Makefile" + +# NOTE: This is config validation only, not an image build. A real `make gui` +# build requires the published Noetic core-deps image as a base and a display +# for the wx smoke test. Upgrade this to an actual layer build (e.g. build +# gui-deps against a published core-deps) when CI infrastructure allows. +jobs: + validate-compose: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v4 + + - name: Validate gui compose config + run: docker compose --profile gui config diff --git a/.gitignore b/.gitignore index 82b8e5c..5cb3a6d 100644 --- a/.gitignore +++ b/.gitignore @@ -1,8 +1,11 @@ **__pycache__ **.swp **.pyc +**.DS_Store artifacts build devel provision/ansible/.password .catkin_tools +.venv +kamera.egg-info diff --git a/CALIBRATION.md b/CALIBRATION.md new file mode 100644 index 0000000..f7064bd --- /dev/null +++ b/CALIBRATION.md @@ -0,0 +1,293 @@ +# Camera Rig Calibration + +Calibrate a KAMERA camera rig from a raw calibration flight to +per-camera models. For every camera it recovers: + +- **intrinsics** — focal length, principal point, OpenCV distortion + (from COLMAP bundle adjustment); and +- **the mount** — orientation and lever arm relative to the aircraft + INS, from a single *boresight* solve per modality group. + +Outputs: a per-camera `*.yaml`, one self-contained +`___rig.json`, and registration QC gifs. +`TO26Su1_RicesWhale_calibration/fl004` is the running example. + +```mermaid +flowchart LR + raw["Raw flight
center/left/right_view
+ meta.json"] + img0["images0/
colmap_rgb · colmap_ir"] + out["kamera_models/
*.yaml · rig.json · gifs"] + raw -->|"stage (automatic)"| img0 -->|"calibrate_rig.py"| out +``` + + +![Reconstruction overview](assets/calibration/reconstruction_overview.jpg) +*The prior-mapped SfM model — camera poses and 3-D points in the INS ENU frame.* + +--- + +## Installation + +Same native post-processing setup as the main +[README](README.md#installation): GDAL and pycolmap from conda-forge, +[uv](https://docs.astral.sh/uv/) for the rest. Requires +[conda](https://conda-forge.org/download/); works on Windows and Linux. + +```bash +git clone https://github.com/Kitware/kamera.git && cd kamera +conda env create -f environment.yml +conda activate kamera +make install && source .venv/bin/activate # Linux/macOS +# Windows: pip install -e . +``` + +Afterwards `conda activate kamera` is all you need. Conda picks the CUDA +pycolmap build with NVIDIA driver 575+, else CPU. **A GPU only matters +here** — feature extraction/matching over a full flight is slow on CPU. + +> The raw flight must contain the INS `*_meta.json` files (one per +> image); everything reads the INS from those, not the `.dat`. + +--- + +## TL;DR + +```bash +conda activate kamera +FLIGHT=/Volumes/extreme2tb/TO26Su1_RicesWhale_calibration/fl004 + +# stage (first run, automatic), build database, map with INS priors, +# boresight, export +python kamera/postflight/scripts/calibrate_rig.py $FLIGHT +``` + +Outputs land in `$FLIGHT/kamera_models/`. The raw imagery dir is +auto-detected under the flight dir; pass `--raw-dir` if a flight holds +several candidates. + +--- + +## Input: the raw flight + +``` +fl004/ +├── images_21deg_N56RF/ # one folder per camera view +│ ├── center_view/ # *_C_*_.jpg + *_C_*_meta.json +│ ├── left_view/ # *_L_*_.jpg + ... +│ └── right_view/ # *_R_*_.jpg + ... +├── ins_raw/ins_raw_*.dat +└── ..._fl004_log.txt +``` + +Each view folder holds every image that camera took, in whatever +modalities were flown (`rgb`, `uv`, `ir`, or a mix), each with a +`_meta.json` carrying the INS state at exposure. All cameras are +hardware-synchronized — at each trigger they share one timestamp, which +is how images group into frames. Names encode provenance: + +``` +TO26Su1_RicesWhale_calibration_fl004_C_20260704_193924.627932_rgb.jpg +└──────────── effort ────────────┘ └flt┘└ch┘└─ date ─┘└── time ──┘└mod┘ +``` + +--- + +## Step 1 — staging into `images0` (automatic) + +The pipeline reads a per-*camera* layout, not the raw per-*view* one. +On its first run against a flight, `calibrate_rig.py` reorganizes it +(by symlink; nothing is copied) into per-modality groups so SIFT never +matches across the EO/IR gap. The same staging is available standalone +— useful to inspect the frame selection before committing to a +calibration run: + +```bash +python kamera/postflight/scripts/prepare_flight.py +``` + +``` +fl004/ +├── colmap_rgb/images0/ # EO group: rgb + uv +│ ├── 21deg_N56RF_center_rgb/ # + *_uv folders if flown +│ ├── 21deg_N56RF_left_rgb/ +│ └── 21deg_N56RF_right_rgb/ +└── colmap_ir/images0/ # IR group, only if ir was flown +``` + +A group with no imagery simply isn't created (fl004 is RGB, so only +`colmap_rgb`). + +Every trigger with a nav time is staged (`--copy` copies instead of +symlinking). Frame density and overlap are set during the flight, not +pared here — SfM needs enough overlap to triangulate, and the binding +constraint is the *lowest-altitude* pass (smallest footprint). + + +![fl004 trajectory](assets/calibration/fl004_trajectory.png) +*fl004 — three figure-8 blocks at ~565/435/275 m. All calibration, no transit.* + +--- + +## Step 2 — `calibrate_rig.py`: calibrate + +```bash +python kamera/postflight/scripts/calibrate_rig.py +``` + +Per modality group, the map fans out to two independent solves that +rejoin at export: + +```mermaid +flowchart TD + db["Build database
SIFT + spatial match"] --> priors["Write INS
pose priors"] + priors --> map["Prior-position map
→ one ENU model"] + map --> bore["Boresight
rig → INS"] + map --> sfr["Derive
sensor_from_rig"] + bore --> export["Export"] + sfr --> export + export --> yaml["*.yaml"] + export --> rig["rig.json"] + export --> gif["registration gifs"] +``` + +- **Build the database** (if none) — SIFT extraction, one OPENCV camera + per folder, then **spatial matching** on INS priors (spatial neighbors, + not every pair). +- **Priors + map** — each image's ENU position at exposure seeds a + prior-position reconstruction, directly in the INS ENU frame (no + separate alignment) and resistant to fragmenting. +- **Boresight + `sensor_from_rig`** — a single robust rig-to-INS rotation + and each camera's rig pose, both recovered from that one model. +- **Export** — a yaml per camera, mount = `ins_from_rig ∘ rig_from_sensor`. + +Flags: + +| flag | effect | +|------|--------| +| `--reuse-aligned` | reuse an existing `aligned/` model instead of re-mapping (fast) | +| `--save-dir DIR` | output dir (default `/kamera_models`) | +| `--prior-std M` | INS position prior std, meters (default 2.0) | +| `--groups rgb:colmap_rgb ir:colmap_ir` | override which workspaces run | +| `--no-gifs` / `--num-gifs N` | skip / set QC gifs per camera | +| `--fuse` | cross-modal fusion — see the next section | + +A group runs only if its workspace exists, so a flight calibrates +whatever modalities it carries with no extra flags. + +--- + +## Optional — `--fuse`: one multimodal model + +By default EO and IR mounts agree only *through the INS*: each group's +boresight resolves to the same physical INS frame, so their relative +mount inherits the INS attitude noise (validated to ~0.01–0.27 deg). +`--fuse` replaces that relay with direct image evidence, using +[MINIMA-LoFTR](https://github.com/LSXI7/MINIMA) — a modality-invariant +deep matcher — to register every IR image straight into the EO model: + +```bash +uv sync --group fusion # one-time: torch + vismatch (weights + # auto-download on first run) +python kamera/postflight/scripts/calibrate_rig.py $FLIGHT --fuse +``` + +Per IR image, its co-located same-trigger EO image is warped into the +IR view (a ground-plane homography from the just-computed per-group +mounts, so the matcher only faces the modality gap, not the ~20x scale +gap), matched, and each matched EO pixel is lifted to 3-D at the +interpolated depth of that EO image's triangulated points. Per-image +PnP only *filters* these correspondences — over near-planar terrain a +single image's pose has a strong tilt/translation ambiguity — and all +surviving matches (tens of thousands) jointly solve **one Sim3** +aligning the IR reconstruction to the EO reconstruction. Every fused IR +pose therefore keeps the IR model's own multi-view relative geometry, +globally registered by the cross-modal matches. The boresight, +`sensor_from_rig`, and export then re-run once on the fused model, so +IR extrinsics are measured against the EO reference camera directly. + +The per-group solve still runs first — it supplies the IR intrinsics +and the warp initialization — and any IR camera that fuses too few +frames falls back to its two-boresight calibration with a warning. +Outputs are the same yamls/rig.json (the rig JSON gains a fused +`rgb+ir` group and a `fusion` provenance block) plus a +`fusion_report.json` with per-image match/inlier stats, the model +alignment (rotation, translation, scale, reprojection), and each IR +camera's mount delta vs the two-boresight solution — expect that delta +within the INS-noise band above; a large outlier on one camera flags a +bad fusion. + +Tuning flags (defaults are sensible): `--fuse-matcher` (any vismatch +name, e.g. `minima-roma`), `--fuse-pairs-per-ir` (add neighbor-trigger +EO partners), `--fuse-max-dt`, `--fuse-snap-px`, `--fuse-ransac-px`, +`--fuse-min-inliers`, `--fuse-max-images` (smoke runs), `--fuse-ba` / +`--fuse-refine-ir-intrinsics` (optional fused bundle adjustment). A GPU +(CUDA or Apple MPS) makes matching ~6x faster; CPU works. + +--- + +## Outputs + +Under `/kamera_models/`: + +``` +kamera_models/ +├── 21deg_N56RF_center_rgb.yaml # one per physical camera +├── ... # (nine for a 3-station EO+UV+IR rig) +├── fl004_20260704_21deg_N56RF_rig.json # complete rig model +└── registration_gifs/*.gif # QC +``` + +**Per-camera yaml** — the runtime model: image size, `fx/fy/cx/cy`, +distortion, `camera_quaternion` (camera→INS mount), `camera_position`. + +**Rig JSON** — the authoritative, self-contained mount description: +provenance (flight, date, pycolmap version, git commit); reference frame +(INS body, ENU origin, quaternion convention); per group the boresight, +lever arm, and residual stats; per camera the intrinsics, INS mount, rig +extrinsics (`sensor_from_rig`), reprojection error, and image count. + +**Registration gifs** — each non-reference camera flipped against its +colocated reference image warped into its view. Features hold still when +calibrated; jitter means misregistration. The visual acceptance check. + + +![Registration gif](assets/calibration/registration_example.gif) +*UV flipped against RGB warped into its view — ground features stay locked.* + +--- + +## How it works + +Each camera's mount composes down a chain of frames: + +```mermaid +flowchart LR + cam["Camera
pixels / rays"] -->|"sensor_from_rig"| rig["Rig
= reference camera"] + rig -->|"ins_from_rig
(boresight)"| ins["INS body"] + ins -->|"nav state at t"| enu["ENU world"] +``` + +The cameras are rigidly mounted and synchronized, so the flight maps +onto COLMAP's rig model. Rather than force the rig onto the mapper +(rig-constrained mapping from scratch fragments badly), we map once with +INS priors into one ENU model, then recover the two rig transforms above +in closed form — `sensor_from_rig` by robust averaging over synchronized +frames, and one rig-to-INS **boresight** per group. Because every +group's boresight resolves to the *same physical INS frame*, EO and IR +come out mutually consistent with no cross-modal matching — which is why +this replaces the old per-camera search and IR transfer/keypoint steps. +`--fuse` tightens that INS-relayed link into a measured one by matching +IR images directly into the EO model (see the fusion section above). + +--- + +## Troubleshooting + +- **Check the gifs first** — the fastest read on calibration quality. +- **Reprojection error** (rig JSON) is INS-noise-limited (tens of px at + long EO focals); a health signal, not the accuracy metric. The + boresight residual (fractions of a degree) and the gifs are. +- **Fragmented model / few images** → the flight was too sparse (not + enough overlap, especially on the lowest-altitude pass); a + calibration flight needs dense, overlapping coverage by design. +- **Iterating on boresight/export** without re-mapping → `--reuse-aligned`. diff --git a/Makefile b/Makefile index f64b5a6..be05684 100644 --- a/Makefile +++ b/Makefile @@ -1,45 +1,63 @@ -CMD ?= bash -runtime ?= runc -network ?= host -name ?= kamera -DAQDEV ?= mcc_daq -REPO_DIR ?= $(shell realpath ${PWD}) -REPO_DIR_BASEDIR ?= $(shell dirname $(REPO_DIR)) -ROS_MASTER_URI ?= not_set_your_config_is_bad -## Docker buildkit - disable by setting to 0 if you have issues - -DAQPATH = $(shell readlink -f "/dev/$(DAQDEV)") -PROJ_DIR=/root/kamera -WS_DIR=/root/kamera -ROS_DISTRO=noetic -BRANCH=latest - -.PHONY: info -info: - @echo REPO_DIR_BASEDIR=$(REPO_DIR_BASEDIR) - @echo ROS_MASTER_URI=$(ROS_MASTER_URI) - @echo REDIS_HOST=$(REDIS_HOST) - -.PHONY: build +ROS_DISTRO ?= noetic + +.PHONY: install build core viame gui postflight follower leader all clean + +# Build .venv on top of an activated conda env from environment.yml. +# The interpreter is taken from the active env, so any python version +# the env provides works (3.10, 3.11, ...). +install: + @test -n "$$CONDA_PREFIX" || { echo "No active conda env — run 'micromamba activate kamera' first."; exit 1; } + @echo "🚀 Creating virtual environment using uv ($$("$$CONDA_PREFIX/bin/python" --version) from $$CONDA_PREFIX)" + @uv venv --system-site-packages --python="$$CONDA_PREFIX/bin/python" + @uv sync --frozen --no-cache + build: docker compose build -.PHONY: nuvo -nuvo: - docker compose --profile nuvo build +## --- individual layer targets --- + +core: + docker compose --profile core build core-ros + docker compose --profile core build core-deps + docker compose --profile core build -.PHONY: viame viame: + docker compose --profile viame build viame-base docker compose --profile viame build -.PHONY: gui gui: - ROS_DISTRO=kinetic docker compose --profile gui build + docker compose --profile gui build core-ros + docker compose --profile gui build core-deps + docker compose --profile gui build gui-deps + docker compose --profile gui build gui -.PHONY: postflight postflight: docker compose --profile pf build -.PHONY: clean +## --- deployment-target targets --- + +# Follower node: headless sensor node — core + VIAME + postproc +follower: + docker compose --profile follower build core-ros + docker compose --profile follower build core-deps + docker compose --profile follower build viame-base + docker compose --profile follower build + +# Leader node: operator workstation — GUI + VIAME + postproc +leader: + docker compose --profile leader build core-ros + docker compose --profile leader build core-deps + docker compose --profile leader build viame-base + docker compose --profile leader build gui-deps + docker compose --profile leader build + +# Everything +all: + docker compose --profile all build core-ros + docker compose --profile all build core-deps + docker compose --profile all build viame-base + docker compose --profile all build gui-deps + docker compose --profile all build + clean: rm -rf .ros .catkin_tools .cmake .config build* devel* logs* diff --git a/NOTICE b/NOTICE index 6774501..ed27a5f 100644 --- a/NOTICE +++ b/NOTICE @@ -1,4 +1,4 @@ -Copyright 2024 Kitware, Inc. +KAMERA ============================================================= Licensed under the Apache License, Version 2.0 (the "License"); diff --git a/README.md b/README.md index 09aa463..ff3afa5 100644 --- a/README.md +++ b/README.md @@ -22,11 +22,41 @@ KAMERA, or the **K**nowledge-guided Image **A**cquisition **M**anag**ER** and ** ## Installation +### Post-processing (native, Windows or Linux) + +GDAL and pycolmap come from conda-forge; [uv](https://docs.astral.sh/uv/) +installs the rest into `.venv`. Requires +[conda](https://conda-forge.org/download/). + +Linux/macOS: + ```bash git clone https://github.com/Kitware/kamera.git cd kamera -# For the pure post-processing and generating flight summary, you can install -# the requirements in requirements.txt, or use the provided dockerfile +conda env create -f environment.yml +conda activate kamera +make install +source .venv/bin/activate +``` + +Windows (PowerShell or Anaconda Prompt): + +```powershell +git clone https://github.com/Kitware/kamera.git +cd kamera +conda env create -f environment.yml +conda activate kamera +pip install -e . +``` + +Afterwards, `conda activate kamera` is all you need. Conda installs the CUDA +build of pycolmap automatically with NVIDIA driver 575+ (CUDA 12.9), +otherwise the CPU build; GPU only matters for full camera model calibration. + +### Docker images + +```bash +# post-processing / flight summary image make postflight # Builds the core docker images for use in the onboard sytems make nuvo @@ -38,9 +68,7 @@ make gui Note that these images take up a large amount of disk space, especially the VIAME image which is 30Gb, and it can take several hours to builds. The core images are faster and lighter weight. ## Partners and Acknowledgements -KAMERA was developed in collaboration with: - - NOAA Marine Mammal Laboratory - - University of Washington +KAMERA was developed in collaboration with the NOAA Marine Mammal Laboratory and the University of Washington. We thank all contributors who have helped in developing KAMERA, with special thanks to Mike McDermott and Matt Brown who created the core system back in 2018. ## License diff --git a/assets/calibration/README.md b/assets/calibration/README.md new file mode 100644 index 0000000..419cf57 --- /dev/null +++ b/assets/calibration/README.md @@ -0,0 +1,12 @@ +# Calibration doc images + +Images referenced by [`CALIBRATION.md`](../../CALIBRATION.md). Drop the +real files here (paths must match). + +| file | what it shows | +|------|---------------| +| `reconstruction_overview.jpg` | The prior-mapped SfM model — camera poses + 3-D points in the ENU frame. A COLMAP viewer screenshot works. | +| `fl004_trajectory.png` | The flight path colored by altitude, showing the three figure-8 bands. | +| `registration_example.gif` | A registration flip gif (e.g. `kamera_models/registration_gifs/*_uv_vs_*_rgb_*.gif`). | + +The frame-chain diagram is inline Mermaid in the doc — no image needed. diff --git a/compose/.env b/compose/.env new file mode 100644 index 0000000..cc98b32 --- /dev/null +++ b/compose/.env @@ -0,0 +1,5 @@ +KAMERA_CORE_IMAGE=kitware/kamera:core +KAMERA_KAMERAD_IMAGE=kitware/kamera:kamerad +KAMERA_VIAME_IMAGE=kitware/kamera:viame +KAMERA_POSTPROC_IMAGE=kitware/kamera:postproc +KAMERA_GUI_IMAGE=kitware/kamera:gui diff --git a/compose/cam_ir.yml b/compose/cam_ir.yml index 83ce7f1..9d7bcb1 100644 --- a/compose/cam_ir.yml +++ b/compose/cam_ir.yml @@ -1,10 +1,10 @@ --- ## Auxiliary node to attach to master -version: '3.7' services: cam_ir: container_name: "cam-ir-${CAM_FOV}" - image: kitware/kamera:core + image: ${KAMERA_CORE_IMAGE} + init: true tty: true environment: ROS_MASTER_URI: "${ROS_MASTER_URI}" diff --git a/compose/cam_param_monitor.yml b/compose/cam_param_monitor.yml index ddb60f5..b975e0f 100644 --- a/compose/cam_param_monitor.yml +++ b/compose/cam_param_monitor.yml @@ -1,10 +1,10 @@ --- ## Core processes -version: '3.7' services: cam_param_monitor: container_name: cam_param_monitor - image: kitware/kamera:core + image: ${KAMERA_CORE_IMAGE} + init: true tty: true environment: ROS_MASTER_URI: "${ROS_MASTER_URI}" diff --git a/compose/cam_rgb.yml b/compose/cam_rgb.yml index 83f9a69..474401d 100644 --- a/compose/cam_rgb.yml +++ b/compose/cam_rgb.yml @@ -1,11 +1,11 @@ --- ## Auxiliary node to attach to master -version: '3.7' services: ## =========================== headless nodes ============================= cam_rgb: container_name: "cam-rgb-${CAM_FOV}" - image: kitware/kamera:core + image: ${KAMERA_CORE_IMAGE} + init: true tty: true environment: ROS_MASTER_URI: "${ROS_MASTER_URI}" diff --git a/compose/cam_uv.yml b/compose/cam_uv.yml index 18732cc..52b7478 100644 --- a/compose/cam_uv.yml +++ b/compose/cam_uv.yml @@ -1,11 +1,11 @@ --- ## Auxiliary node to attach to master -version: '3.7' services: ## =========================== headless nodes ============================= cam_uv: container_name: "cam-uv-${CAM_FOV}" - image: kitware/kamera:core + image: ${KAMERA_CORE_IMAGE} + init: true tty: true environment: ROS_MASTER_URI: "${ROS_MASTER_URI}" diff --git a/compose/daq.yml b/compose/daq.yml index 3dc3fee..7d0633b 100644 --- a/compose/daq.yml +++ b/compose/daq.yml @@ -1,10 +1,10 @@ --- ## Bring up the core nodes (aside from roscore) -version: '3.7' services: daq: container_name: daq - image: kitware/kamera:core + image: ${KAMERA_CORE_IMAGE} + init: true tty: true devices: - "${MCC_DAQ}:${MCC_DAQ}" diff --git a/compose/detections.yml b/compose/detections.yml index a8e1fb1..22f973c 100644 --- a/compose/detections.yml +++ b/compose/detections.yml @@ -1,10 +1,10 @@ --- ## Core processes -version: '3.7' services: ## =========================== headless nodes ============================= detections: - image: kitware/kamera:postproc + image: ${KAMERA_POSTPROC_IMAGE} + init: true network_mode: host environment: REDIS_HOST: "${REDIS_HOST}" diff --git a/compose/detector.yml b/compose/detector.yml index af61eab..d2cae26 100644 --- a/compose/detector.yml +++ b/compose/detector.yml @@ -1,13 +1,13 @@ --- ## container configuration for viame interactive building # use compose v2 to use runtime config -version: '2.4' services: ## ========================== viame ============================= detector: container_name: "viame-${CAM_FOV}" - image: kitware/kamera:viame + image: ${KAMERA_VIAME_IMAGE} + init: true tty: true environment: REDIS_HOST: "${REDIS_HOST}" diff --git a/compose/flight_summary.yml b/compose/flight_summary.yml index d86d0ae..2d7efcc 100644 --- a/compose/flight_summary.yml +++ b/compose/flight_summary.yml @@ -1,10 +1,10 @@ --- ## Core processes -version: '3.7' services: ## =========================== headless nodes ============================= flight_summary: - image: kitware/kamera:postproc + image: ${KAMERA_POSTPROC_IMAGE} + init: true network_mode: host environment: REDIS_HOST: "${REDIS_HOST}" diff --git a/compose/fps_monitor.yml b/compose/fps_monitor.yml index 74e7aa3..53fb05b 100644 --- a/compose/fps_monitor.yml +++ b/compose/fps_monitor.yml @@ -1,10 +1,10 @@ --- ## Core processes -version: '3.7' services: fps_monitor: container_name: fps_monitor - image: kitware/kamera:core + image: ${KAMERA_CORE_IMAGE} + init: true tty: true environment: ROS_MASTER_URI: "${ROS_MASTER_URI}" diff --git a/compose/gui.yml b/compose/gui.yml index 8f37ae6..005122d 100644 --- a/compose/gui.yml +++ b/compose/gui.yml @@ -1,11 +1,11 @@ --- ## container configuration for GUI -version: '3.7' services: ## =========================== gui ============================= gui: container_name: gui - image: kitware/kamera:gui + image: ${KAMERA_GUI_IMAGE} + init: true tty: true environment: REDIS_HOST: "${REDIS_HOST}" @@ -23,17 +23,15 @@ services: dns_search: - kamera.systems network_mode: host + ipc: host # fixes bad X Windows startups volumes: - "/mnt:/mnt" - "/tmp/.X11-unix:/tmp/.X11-unix" - - "/home/user/kw/kamera/scripts:/home/user/kamera_ws/scripts" - - "${PWD}/src:/home/user/kamera_ws/src:ro" - - "${PWD}/kamera:/home/user/kamera_ws/kamera:ro" - - "${PWD}/assets:/home/user/noaa_kamera/assets:ro" + - "/home/user/kw/kamera/scripts:/home/user/kamera/scripts" + - "${PWD}/src:/home/user/kamera/src:ro" + - "${PWD}/assets:/home/user/kamera/assets:ro" - "save-gui:/home/user/.config/kamera/gui" - - "${HOME}/.ssh:/home/user/.ssh" - entrypoint: ["/bin/tini", "--"] - command: ["/entry/gui.sh"] + entrypoint: ["/entry/gui.sh"] #command: ["bash"] volumes: diff --git a/compose/homography.yml b/compose/homography.yml index a10da6a..cb9fc3a 100644 --- a/compose/homography.yml +++ b/compose/homography.yml @@ -1,10 +1,10 @@ --- ## Core processes -version: '3.7' services: ## =========================== headless nodes ============================= homography: - image: kitware/kamera:postproc + image: ${KAMERA_POSTPROC_IMAGE} + init: true network_mode: host environment: REDIS_HOST: "${REDIS_HOST}" diff --git a/compose/image_manager.yml b/compose/image_manager.yml index 23b156f..8437bb1 100644 --- a/compose/image_manager.yml +++ b/compose/image_manager.yml @@ -1,11 +1,11 @@ --- ## Auxiliary node to attach to master -version: '3.7' services: ## =========================== headless nodes ============================= image_manager: container_name: "image_manager" - image: kitware/kamera:core + image: ${KAMERA_CORE_IMAGE} + init: true tty: true environment: REDIS_HOST: "${REDIS_HOST}" diff --git a/compose/imageview.yml b/compose/imageview.yml index e4d5246..78d824c 100644 --- a/compose/imageview.yml +++ b/compose/imageview.yml @@ -1,9 +1,9 @@ --- -version: '3.7' services: imageview: container_name: "imageview-${CAM_FOV}" - image: kitware/kamera:core + image: ${KAMERA_CORE_IMAGE} + init: true tty: true environment: REDIS_HOST: "${REDIS_HOST}" diff --git a/compose/ins.yml b/compose/ins.yml index 5d01c04..ffab818 100644 --- a/compose/ins.yml +++ b/compose/ins.yml @@ -1,10 +1,10 @@ --- ## Bring up the core nodes (aside from roscore) -version: '3.7' services: ins: container_name: ins - image: kitware/kamera:core + image: ${KAMERA_CORE_IMAGE} + init: true tty: true devices: - "${PULSE_TTY}:${PULSE_TTY}" diff --git a/compose/kamerad.yml b/compose/kamerad.yml index 56cde0d..3b6e7d0 100644 --- a/compose/kamerad.yml +++ b/compose/kamerad.yml @@ -1,14 +1,17 @@ --- ## Core processes -version: '3.7' services: ## =========================== headless nodes ============================= kamerad: - image: kitware/kamera:kamerad + image: ${KAMERA_KAMERAD_IMAGE} + init: true network_mode: host build: ../src/core/kamerad tty: true user: root + environment: + NODE_HOSTNAME: "${NODE_HOSTNAME}" + SYSTEM_NAME: "${SYSTEM_NAME}" volumes: - "/mnt:/mnt:ro,rshared" restart: unless-stopped diff --git a/compose/nodelist.yml b/compose/nodelist.yml index 36f71b4..9a0eb69 100644 --- a/compose/nodelist.yml +++ b/compose/nodelist.yml @@ -4,7 +4,8 @@ services: ## =========================== headless nodes ============================= nodelist: container_name: nodelist - image: kitware/kamera:core + image: ${KAMERA_CORE_IMAGE} + init: true tty: true network_mode: host environment: diff --git a/compose/postproc.yml b/compose/postproc.yml index 9cfe2e7..23ccebb 100644 --- a/compose/postproc.yml +++ b/compose/postproc.yml @@ -1,10 +1,10 @@ --- ## Core processes -version: '3.7' services: ## =========================== headless nodes ============================= postproc: - image: kitware/kamera:postproc + image: ${KAMERA_POSTPROC_IMAGE} + init: true network_mode: host environment: REDIS_HOST: "${REDIS_HOST}" diff --git a/compose/registry.yml b/compose/registry.yml index ccbb7cf..14f4f6a 100644 --- a/compose/registry.yml +++ b/compose/registry.yml @@ -1,8 +1,8 @@ -version: "3" services: registry: restart: always image: registry:2 + init: true ports: - 5000:5000 volumes: diff --git a/compose/roscore.yml b/compose/roscore.yml index b427a95..5b2e379 100644 --- a/compose/roscore.yml +++ b/compose/roscore.yml @@ -1,11 +1,11 @@ --- ## Bring up the core nodes -version: '3.7' services: ## =========================== headless nodes ============================= roscore: container_name: roscore - image: kitware/kamera:core + image: ${KAMERA_CORE_IMAGE} + init: true tty: true network_mode: host environment: diff --git a/compose/shapefile_monitor.yml b/compose/shapefile_monitor.yml index 2814580..2184930 100644 --- a/compose/shapefile_monitor.yml +++ b/compose/shapefile_monitor.yml @@ -1,10 +1,10 @@ --- ## Core processes -version: '3.7' services: shapefile_monitor: container_name: shapefile_monitor - image: kitware/kamera:core + image: ${KAMERA_CORE_IMAGE} + init: true tty: true environment: REDIS_HOST: "${REDIS_HOST}" diff --git a/compose/spoof_events.yml b/compose/spoof_events.yml index 831d424..0272cc6 100644 --- a/compose/spoof_events.yml +++ b/compose/spoof_events.yml @@ -1,10 +1,10 @@ --- ## Bring up the core nodes (aside from roscore) -version: '3.7' services: spoof_events: container_name: spoof_events - image: kitware/kamera:core + image: ${KAMERA_CORE_IMAGE} + init: true tty: true devices: - "${PULSE_TTY}:${PULSE_TTY}" diff --git a/compose/sync_msg_publisher.yml b/compose/sync_msg_publisher.yml index 4f6124a..5d6f18a 100644 --- a/compose/sync_msg_publisher.yml +++ b/compose/sync_msg_publisher.yml @@ -1,10 +1,10 @@ --- -version: "3.7" ## =======================image sync publisher=================== services: sync-publisher: container_name: "sync-publisher-${CAM_FOV}" - image: kitware/kamera:viame + image: ${KAMERA_VIAME_IMAGE} + init: true tty: true environment: ROS_MASTER_URI: "${ROS_MASTER_URI}" diff --git a/docker-compose.yml b/docker-compose.yml index 21d57fc..b0424b2 100644 --- a/docker-compose.yml +++ b/docker-compose.yml @@ -5,85 +5,75 @@ ## provide permissions with xhost, usually `xhost +local:root` (not the safest) ## You also have to volume mount `/tmp/.X11-unix`, blank out $DISPLAY, and ## disable X11 shared memory `QT_X11_NO_MITSHM=1` +## +## Deployment profiles: +## follower -- headless sensor node: core + VIAME + postproc +## leader -- operator workstation: GUI + VIAME + postproc +## all -- everything +## +## Individual layer profiles: +## core -- Noetic base images only (no VIAME, no GUI) +## viame -- VIAME detector images only +## gui -- Noetic GUI images only +## pf -- postproc / postflight only -version: '3.7' services: -## ======================= base containers & bases ======================== - # Docker file for kamerad service +## ======================= Noetic core chain ======================== + + # Lightweight Python service; independent of ROS distro. kamerad: build: context: ./src/core/kamerad image: "kitware/kamera:kamerad" profiles: - - nuvo + - core + - follower + - leader - all - # The core ROS image with CUDA and basic APT packages, no KAMERA code. + # CUDA + ROS Noetic base with utility packages. No KAMERA source. core-ros: build: context: . dockerfile: docker/base/core-ros.dockerfile image: "kamera/base/core-ros:${TAG_KAMERA_DEPS:-latest}" profiles: - - nuvo - - gui + - core + - follower - all + - gui + - leader -## =========================== main (full dependencies) ======================= - - ## This is the main image with all dependendencies required for KAMERA, built on - # the basic ROS image. + # System dependencies (drivers, C++ libs, ROS packages). No source copy. core-deps: build: context: . dockerfile: docker/base/core-deps.dockerfile image: "kamera/base/core-deps:${TAG_KAMERA_DEPS:-latest}" - depends_on: - - core-ros profiles: - - nuvo - - gui + - core + - follower - all - - core-gui-deps: - build: - context: . - dockerfile: docker/base/gui-deps.dockerfile - image: "kamera/base/core-gui-deps:${TAG_KAMERA_GUI_DEPS:-latest}" - depends_on: - - core-deps - profiles: - gui + - leader - # Just for the catkin rebuild, if we want to save the static image. - core-final: + # KAMERA source + catkin build on top of core-deps. + core: build: context: . dockerfile: docker/core.dockerfile image: "kitware/kamera:core" tty: true network_mode: host - depends_on: - - core-deps volumes: - - "/tmp/.X11-unix:/tmp/.X11-unix" - - "${PWD}/src:/root/kamera/src" + - "/tmp/.X11-unix:/tmp/.X11-unix" + - "${PWD}/src:/root/kamera/src" profiles: - - nuvo + - core + - follower + - leader - all -## =========================== gui ============================= - gui: - container_name: gui - build: - context: . - dockerfile: docker/gui.dockerfile - image: "kitware/kamera:gui" - depends_on: - - core-gui-deps - profiles: - - gui - postproc: build: context: . @@ -94,13 +84,14 @@ services: network_mode: host tty: true profiles: - - nuvo + - core + - follower + - leader - all - pf +## ========================== VIAME detector chain ============================= - -## ========================== viame ============================= viame-base: build: context: . @@ -111,6 +102,8 @@ services: image: "kamera/base/viame:${VIAME_BRANCH:-latest}" profiles: - viame + - follower + - leader - all viame: @@ -124,10 +117,36 @@ services: tty: true network_mode: host command: ["bash"] - depends_on: - - viame-base profiles: - viame + - follower + - leader + - all + +## ========================== GUI chain ======================================== + + # GUI deps layered on the Noetic core-deps image. + gui-deps: + build: + context: . + dockerfile: docker/base/gui-deps.dockerfile + image: "kamera/base/gui-deps:${TAG_KAMERA_GUI_DEPS:-latest}" + profiles: + - gui + - leader + - all + + gui: + container_name: gui + build: + context: . + dockerfile: docker/gui.dockerfile + args: + GUI_DEPS_IMAGE: "kamera/base/gui-deps:${TAG_KAMERA_GUI_DEPS:-latest}" + image: "kitware/kamera:gui" + profiles: + - gui + - leader - all ## ==================================================================== diff --git a/docker/base/core-deps.dockerfile b/docker/base/core-deps.dockerfile index a6a5f64..18df1ef 100644 --- a/docker/base/core-deps.dockerfile +++ b/docker/base/core-deps.dockerfile @@ -102,30 +102,3 @@ RUN cd /src/DALSA &&\ > GigeV/bin/temp && mv GigeV/bin/temp GigeV/bin/install.gigev &&\ chmod +x GigeV/bin/install.gigev && ./corinstall install -## === === === === === === Project specifics === === === === === === === === -ENV WS_DIR=/root/kamera -WORKDIR /root/kamera - -# Copy products into container -COPY . $WS_DIR - -RUN rm -rf /entry &&\ - ln -sf $WS_DIR/src/run_scripts/entry /entry &&\ - printf "\nsource /entry/project.sh\n" >> /root/.bashrc &&\ - touch $WS_DIR/.catkin_workspace &&\ - ln -sf $WS_DIR/src/run_scripts/aliases.sh /aliases.sh &&\ - printf "\nsource /aliases.sh\n" >> /root/.bashrc - -# Making useful links and copies -RUN ln -sf $WS_DIR/scripts/activate_ros.bash $WS_DIR/activate_ros.bash -RUN ln -sf $WS_DIR/src/cfg /cfg -RUN mkdir -p /root/.config/kamera && \ - ln -sf $WS_DIR/.dir /root/.config/kamera/repo_dir.bash - -# Need to build phase one first to generate SRV -RUN ln -sv /usr/bin/python3 /usr/bin/python || true -RUN [ "/bin/bash", "-c", "source ${WS_DIR}/activate_ros.bash && catkin build phase_one"] -RUN [ "/bin/bash", "-c", "source /entry/project.sh && catkin build backend"] - -ENTRYPOINT ["/entry/project.sh"] -CMD ["bash"] diff --git a/docker/base/core-ros.dockerfile b/docker/base/core-ros.dockerfile index 186a7c6..7fc2389 100644 --- a/docker/base/core-ros.dockerfile +++ b/docker/base/core-ros.dockerfile @@ -1,6 +1,6 @@ # This image contains the base of the ROS/CUDA for the system, plus # a bunch of utility packages -FROM nvidia/cuda:12.6.2-devel-ubuntu20.04 as base_cuda_ubuntu +FROM nvidia/cuda:12.6.2-devel-ubuntu20.04 AS base_cuda_ubuntu WORKDIR /root # setup environment @@ -80,7 +80,6 @@ RUN pip install --upgrade --no-cache-dir pip \ && pip install --no-cache-dir \ ipython \ ipdb \ - python-benedict \ pyserial \ typing \ pathlib \ diff --git a/docker/base/detector-viame-deps.dockerfile b/docker/base/detector-viame-deps.dockerfile index 2caa84c..1267513 100644 --- a/docker/base/detector-viame-deps.dockerfile +++ b/docker/base/detector-viame-deps.dockerfile @@ -1,5 +1,5 @@ # Build off the public VIAME docker build (with ITK support) -FROM kitware/viame:gpu-algorithms-seal as vb +FROM kitware/viame:gpu-algorithms-seal AS vb WORKDIR /root # setup environment @@ -37,10 +37,6 @@ RUN apt-get update && apt-get install -y --no-install-recommends \ # Build tools necessary for catkin and roskv -# Add Tini to handle signals -RUN curl -sSL https://github.com/krallin/tini/releases/download/v0.18.0/tini -o /tini && \ - chmod +x /tini - # Add yq to make config query work RUN curl -sL https://github.com/mikefarah/yq/releases/download/2.4.0/yq_linux_amd64 \ -o /usr/local/bin/yq && \ diff --git a/docker/base/gui-deps.dockerfile b/docker/base/gui-deps.dockerfile index c2e4b33..fbf5533 100644 --- a/docker/base/gui-deps.dockerfile +++ b/docker/base/gui-deps.dockerfile @@ -1,30 +1,36 @@ -## this is currently ONLY supported on kinetic! -FROM kamera/base/core-deps-kinetic:latest +## GUI deps layered on the Noetic core-deps chain. +FROM kamera/base/core-deps:latest -RUN apt-get update && apt-get install -y \ - gdal-bin \ - python-gdal \ - python-tk \ - libgl1-mesa-glx \ - libqt5x11extras5 \ - openssh-server \ - && rm -rf /var/lib/apt/lists/* +RUN apt-get update && apt-get install -y --no-install-recommends \ + gdal-bin \ + python3-gdal \ + python3-tk \ + python3-wxgtk4.0 \ + libgl1-mesa-glx \ + libqt5x11extras5 \ + locales \ + && rm -rf /var/lib/apt/lists/* -RUN pip install --upgrade \ - pip \ - Pillow +# wxPython's wx.App init sets the en_US locale at startup; generate it so the +# GUI doesn't fail with "locale en_US cannot be set". +RUN locale-gen en_US.UTF-8 && update-locale LANG=en_US.UTF-8 +ENV LANG=en_US.UTF-8 \ + LANGUAGE=en_US:en \ + LC_ALL=en_US.UTF-8 +RUN pip install --upgrade \ + pip \ + Pillow -RUN pip install --no-cache-dir \ - 'PyGeodesy<19.12' \ - 'IPython==5.0' \ - exifread \ - numpy \ - shapely \ - pyshp \ - scipy\ - simplekml \ - wxPython \ - typing +# numpy/scipy/shapely/pyshp come from core-deps; only GUI-unique deps here. +# TODO: validate the unpinned pygeodesy already installed by core-deps and drop +# this <19.12 downgrade (legacy Py2-era pin, untested on Py3). +RUN pip install --no-cache-dir \ + 'PyGeodesy<19.12' \ + exifread \ + ipython \ + psutil \ + simplekml -RUN pip install 'src/core/roskv[redis]' +COPY src/core/roskv /src/roskv +RUN pip install --no-cache-dir '/src/roskv[redis]' diff --git a/docker/base/gui-ros.dockerfile b/docker/base/gui-ros.dockerfile deleted file mode 100644 index 003c7aa..0000000 --- a/docker/base/gui-ros.dockerfile +++ /dev/null @@ -1,50 +0,0 @@ -FROM ros:noetic-robot as noetic-robot -ENV PYTHON=python3 \ - ROS_PKG_VERS=1.5.0-1 - -## Necessary, followed by unessential but useful packages -RUN apt-key adv --keyserver hkps://keyserver.ubuntu.com --refresh-keys -RUN apt-get update -q && apt-get install --no-install-recommends -y \ - curl \ - git \ - iputils-ping \ - iproute2 \ - net-tools \ - dnsutils \ - jq \ - redis-tools \ - sqlite3 \ - python3-pip \ - python3-rosdep \ - python3-rosinstall \ - python3-vcstools \ - python3-catkin-tools \ - unzip \ - && apt-get update -q && apt-get install --no-install-recommends -y \ - autoconf \ - automake\ - build-essential \ - dirmngr \ - pkg-config \ - sudo \ - nano \ - vim \ - inetutils-traceroute \ - tmux \ - python3-dev \ - && rm -rf /var/lib/apt/lists/* - - -## ipython isn't strictly required (like most things in is kitchen sink image) but it's extremely useful for debugging -RUN pip install --upgrade --no-cache-dir pip \ - && pip install --no-cache-dir \ - ipython \ - ipdb \ - python-benedict \ - pyserial \ - typing \ - pathlib \ - profilehooks \ - redis \ - osrf-pycommon \ - six diff --git a/docker/core.dockerfile b/docker/core.dockerfile index 0a95cef..7dc471b 100644 --- a/docker/core.dockerfile +++ b/docker/core.dockerfile @@ -2,15 +2,27 @@ FROM kamera/base/core-deps:latest -COPY . /root/kamera -WORKDIR /root/kamera +ENV REPO_DIR=/root/kamera +WORKDIR $REPO_DIR -# use the exec form of run because we need bash syntax -RUN [ "/bin/bash", "-c", "source /entry/project.sh && catkin build -s backend"] -# python package is currently only supported for python3.10+, not on ubuntu20.04 -# RUN [ "/bin/bash", "-c", "pip install ."] +COPY . $REPO_DIR + +RUN rm -rf /entry \ + && ln -sf $REPO_DIR/src/run_scripts/entry /entry \ + && printf "\nsource /entry/project.sh\n" >> /root/.bashrc \ + && touch $REPO_DIR/.catkin_workspace \ + && ln -sf $REPO_DIR/src/run_scripts/aliases.sh /aliases.sh \ + && printf "\nsource /aliases.sh\n" >> /root/.bashrc +RUN ln -sf $REPO_DIR/scripts/activate_ros.bash $REPO_DIR/activate_ros.bash +RUN ln -sf $REPO_DIR/src/cfg /cfg +RUN mkdir -p /root/.config/kamera && \ + ln -sf $REPO_DIR/.dir /root/.config/kamera/repo_dir.bash +# Need to build phase_one first to generate SRV, then build backend +RUN ln -sv /usr/bin/python3 /usr/bin/python || true +RUN [ "/bin/bash", "-c", "source ${REPO_DIR}/activate_ros.bash && catkin build phase_one"] +RUN [ "/bin/bash", "-c", "source /entry/project.sh && catkin build -s backend"] ENTRYPOINT ["/entry/project.sh"] CMD ["bash"] diff --git a/docker/detector.dockerfile b/docker/detector.dockerfile index 2cc0c8c..250f056 100644 --- a/docker/detector.dockerfile +++ b/docker/detector.dockerfile @@ -5,8 +5,7 @@ COPY . /root/kamera RUN ln -sf /root/kamera/src/run_scripts/entry /entry WORKDIR /root/kamera -ENV REPO_DIR=/root/kamera \ - WS_DIR=/root/kamera +ENV REPO_DIR=/root/kamera RUN ["/bin/bash", "-c", "source /entry/project.sh && \ source src/run_scripts/setup/setup_viame_build.sh && \ diff --git a/docker/gui.dockerfile b/docker/gui.dockerfile index 02be9e3..fdbf0e6 100644 --- a/docker/gui.dockerfile +++ b/docker/gui.dockerfile @@ -1,5 +1,6 @@ ARG BRANCH=latest -FROM kamera/base/kamera-gui-deps:latest +ARG GUI_DEPS_IMAGE=kamera/base/gui-deps:latest +FROM ${GUI_DEPS_IMAGE} # Create a non-root user and switch to it. Running X11 applications as root does @@ -9,37 +10,42 @@ RUN useradd -m --uid=1000 user \ && useradd --uid=7777 share \ && usermod -aG share user -# Source code goes into ~/noaa_kamera, live dir goes into ~/kamera_ws +# Source code goes into ~/kamera ENV HOME_DIR=/home/user \ - REPO_DIR=/home/user/noaa_kamera \ - WS_DIR=/home/user/kamera_ws \ + REPO_DIR=/home/user/kamera \ GUI_CFG_DIR=/home/user/.config/kamera/gui # trying to speed up chown operation RUN mkdir -p /home/user && chown -R user:user /home/user -COPY --chown=user:user src/kitware-ros-pkg/wxpython_gui/config $GUI_CFG_DIR/config -COPY --chown=user:user repo_dir.bash $REPO_DIR/repo_dir.bash -COPY --chown=user:user src $REPO_DIR/src -COPY --chown=user:user activate_ros.bash $WS_DIR/activate_ros.bash -RUN ln -sf $REPO_DIR/src $WS_DIR/src &&\ - rm -rf /entry &&\ - ln -sf $REPO_DIR/src/run_scripts/entry /entry &&\ - printf "\nsource /entry/project.sh\n" >> /home/user/.bashrc &&\ - touch $WS_DIR/.catkin_workspace &&\ - ln -sf $REPO_DIR/src/run_scripts/aliases.sh /aliases.sh &&\ - printf "\nsource /aliases.sh\n" >> /root/.bashrc &&\ - ln -sf $REPO_DIR/src/cfg /cfg &&\ - HOME=/home/user $REPO_DIR/src/run_scripts/setup/install_links.sh - -WORKDIR $WS_DIR +WORKDIR $REPO_DIR +COPY --chown=user:user . $REPO_DIR + +RUN rm -rf /entry \ + && ln -sf $REPO_DIR/src/run_scripts/entry /entry \ + && printf "\nsource /entry/project.sh\n" >> /home/user/.bashrc \ + && touch $REPO_DIR/.catkin_workspace \ + && ln -sf $REPO_DIR/src/run_scripts/aliases.sh /aliases.sh \ + && printf "\nsource /aliases.sh\n" >> /home/user/.bashrc + +RUN ln -sf $REPO_DIR/scripts/activate_ros.bash $REPO_DIR/activate_ros.bash +RUN ln -sf $REPO_DIR/src/cfg /cfg +RUN mkdir -p /home/user/.config/kamera && \ + ln -sf $REPO_DIR/.dir /home/user/.config/kamera/repo_dir.bash + +RUN ln -sv /usr/bin/python3 /usr/bin/python || true RUN find /home/user -not -user user -execdir chown user {} \+ +# Install kamera for wxpython_gui imports. --no-deps: deps come from the base +# image (a full install trips on ROS's distutils PyYAML). +# --ignore-requires-python: ROS Noetic pins python 3.8, below our 3.10 floor. +RUN pip install --no-cache-dir matplotlib \ + && pip install --no-cache-dir --no-deps --ignore-requires-python -e $REPO_DIR + # use the exec form of run because we need bash syntax USER user RUN [ "/bin/bash", "-c", "source /entry/project.sh && catkin build wxpython_gui "] RUN [ "/bin/bash", "-c", "source /entry/project.sh && catkin build ins_driver "] -RUN [ "/bin/bash", "-c", "pip install -e ."] USER root RUN find /home/user -not -user user -execdir chown user {} \+ USER user diff --git a/docker/kamerapy.dockerfile b/docker/kamerapy.dockerfile index e00f1bf..65469fe 100644 --- a/docker/kamerapy.dockerfile +++ b/docker/kamerapy.dockerfile @@ -1,20 +1,41 @@ -FROM python:3.10.15-bookworm +FROM debian:bookworm-slim + +SHELL ["/bin/bash", "-c"] RUN apt-get update && apt-get install -yq \ - libgdal-dev \ - python3-gdal \ - libgl1-mesa-glx \ + curl \ + bzip2 \ + ca-certificates \ + make \ + libgl1 \ + libglib2.0-0 \ libsm6 \ libxext6 \ redis \ dnsutils \ - gdal-bin + && rm -rf /var/lib/apt/lists/* + +# Install micromamba +ARG MAMBA_VERSION=2.3.3 +RUN curl -Ls https://micro.mamba.pm/api/micromamba/linux-64/${MAMBA_VERSION} \ + | tar -xvj -C /usr/local/bin --strip-components=1 bin/micromamba +ENV MAMBA_ROOT_PREFIX=/opt/conda + +# Conda env supplies python + GDAL + uv; make install layers .venv on top +COPY environment.yml /tmp/environment.yml +RUN micromamba create -y -n kamera -f /tmp/environment.yml \ + && micromamba clean --all -y -RUN pip install --upgrade pip -RUN pip install setuptools==57.0.0 +ENV UV_COMPILE_BYTECODE=1 \ + UV_LINK_MODE=copy COPY ./ /src/kamera WORKDIR /src/kamera -RUN pip install -e . + +RUN eval "$(micromamba shell hook --shell bash)" \ + && micromamba activate kamera \ + && make install + +ENV PATH="/src/kamera/.venv/bin:/opt/conda/envs/kamera/bin:$PATH" ENTRYPOINT ["bash"] diff --git a/environment.yml b/environment.yml new file mode 100644 index 0000000..ebd077a --- /dev/null +++ b/environment.yml @@ -0,0 +1,11 @@ +# Binary deps (GDAL, pycolmap) with no reliable cross-platform wheels. +# `make install` layers the rest on top; see README.md for setup. +name: kamera +channels: + - conda-forge +dependencies: + - python>=3.10,<3.14 # make install builds .venv on whichever version the env has + - gdal>=3.10 + - pycolmap>=4.1 # COLMAP 3.14 bindings; postflight calibration is tested against 4.1 + - pip + - uv diff --git a/kamera/colmap_processing/calibration.py b/kamera/colmap_processing/calibration.py deleted file mode 100644 index b73c4cd..0000000 --- a/kamera/colmap_processing/calibration.py +++ /dev/null @@ -1,460 +0,0 @@ -#!/usr/bin/env python -""" -ckwg +31 -Copyright 2020 by Kitware, Inc. -All rights reserved. - -Redistribution and use in source and binary forms, with or without -modification, are permitted provided that the following conditions are met: - - * Redistributions of source code must retain the above copyright notice, - this list of conditions and the following disclaimer. - - * Redistributions in binary form must reproduce the above copyright notice, - this list of conditions and the following disclaimer in the documentation - and/or other materials provided with the distribution. - - * Neither name of Kitware, Inc. nor the names of any contributors may be used - to endorse or promote products derived from this software without specific - prior written permission. - -THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS ``AS IS'' -AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHORS OR CONTRIBUTORS BE LIABLE FOR -ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - -============================================================================== - -Library handling projection operations of a standard camera model. - -Note: the image coordiante system has its origin at the center of the top left -pixel. - -""" -from __future__ import division, print_function, absolute_import -import numpy as np -import os -from numpy import pi -import cv2 -import time -import yaml -from scipy.interpolate import interp1d, RectBivariateSpline -from scipy.optimize import fmin, fminbound, minimize -import copy -import pickle -import PIL - -try: - import matplotlib.pyplot as plt -except ImportError: - pass - -# Repository imports. -from colmap_processing.image_renderer import stitch_images -from colmap_processing.platform_pose import PlatformPoseFixed -from colmap_processing.geo_conversions import enu_to_llh, llh_to_enu -import colmap_processing.dp as dp -from colmap_processing.camera_models import ray_intersect_plane - - -def horn(P, Q, fit_scale=True, fit_translation=True): - """Method of Horn. - - :param P: Initial point cloud. - :type P: num_dim x N - - :param Q: Destination point cloud. - :type Q: num_dim x N - - :return: Scale, rotation matrix, and translation to be applied in that - order that minimizes the difference between Q and (R*s*P + t). - :rtype: - - Example: - - s = 213.5 - S = np.diag([s, s, s, 1]) - R = np.identity(4) - R[:3, :3] = np.array([[0.411982,-0.833738,-0.367630], - [-0.058727,-0.426918,0.902382], - [-0.909297,-0.350175,-0.224845]]) - t = [1, 2, 3] - T = np.identity(4) - T[:3, 3] = t - - H = np.dot(np.dot(T, R), S)[:3] - - xyz1 = np.random.rand(4, 100) - xyz1[3, :] = 1 - xyz2 = np.dot(H, xyz1) - - # Scale, rotation, translation - s2, R2, t2 = horn(xyz1[:3], xyz2, fit_scale=True, fit_translation=True) - print(s, s2) - print(R[:3, :3], '\n', R2) - print(t, t2) - print('Max error:', np.max(np.abs(np.dot(R[:3, :3], s*xyz1[:3, :]).T + t - xyz2.T))) - - # Only rotation - xyz2 = np.dot(R[:3, :3], xyz1[:3]) - R2 = horn(xyz1[:3], xyz2, fit_scale=False, fit_translation=False)[1] - print(R[:3, :3], '\n', R2) - - """ - if P.shape != Q.shape: - print("Matrices P and Q must be of the same dimensionality") - - if fit_translation: - P0 = np.mean(P, axis=1) - Q0 = np.mean(Q, axis=1) - A = P - np.outer(P0, np.ones(P.shape[1])) - B = Q - np.outer(Q0, np.ones(Q.shape[1])) - else: - A = P - B = Q - - if fit_scale: - s = np.sqrt(np.mean(B.ravel()**2)) / np.sqrt(np.mean(A.ravel()**2)) - - # Apply scale. - A = s*A - - if fit_translation: - P0 = P0*s - else: - s = 1 - - C = np.dot(A, B.transpose()) - U, S, V = np.linalg.svd(C) - R = np.dot(V.transpose(), U.transpose()) - L = np.eye(3) - if np.linalg.det(R) < 0: - L[2][2] *= -1 - - R = np.dot(V.transpose(), np.dot(L, U.transpose())) - - if fit_translation: - t = np.dot(-R, P0) + Q0 - else: - t = np.zeros(len(P)) - - return s, R, t - - -def fit_plane(xyz): - """Check whether results from cv2.calibrateCamera are valid. - - :param xyz: 3-D coordinates. - :type xyz: N x 2 - - """ - plane_point = np.mean(xyz, 0) - x = xyz - np.atleast_2d(plane_point) - M = np.dot(x.T, x) - plane_normal = np.linalg.svd(M)[0][:,-1] - plane_normal *= np.sign(plane_normal[-1]) - return plane_point, plane_normal - - -def check_valid_camera(width, height, K, dist, rvec, tvec): - if K[0,0] < 0 or K[1,1] < 1: - return False - - if K[0, 2] < 0 or K[0, 2] > width: - return False - - if K[1, 2] < 0 or K[1, 2] > height: - return False - - return True - - -def cam_depth_map_plane(camera_model, t, plane_point, plane_normal): - """Calculate depth map for camera assuming it is viewing plane. - - """ - height = camera_model.height - width = camera_model.width - X, Y = np.meshgrid(np.linspace(0.5, width - 0.5, width), - np.linspace(0.5, height - 0.5, height)) - im_pts = np.vstack([X.ravel(), Y.ravel()]) - ray_pos, ray_dir = camera_model.unproject(im_pts, t, - normalize_ray_dir=False) - - ip = ray_intersect_plane(plane_point, plane_normal, ray_pos, ray_dir, - epsilon=1e-6) - - # depth*ray_dir = ip - ray_pos - depth = np.sqrt(np.sum((ip - ray_pos)**2, axis=0)/np.sum(ray_dir**2, axis=0)) - - depth_map = np.reshape(depth, X.shape) - return depth_map - - -def get_rvec_btw_times(cm, t1, t2): - """Return rotation vector defined in the camera coordinate system. - - Calculate the rotation vector corresponding to the rotation of the camera - frame from time t1 to t2. The rotation is defined within the coordinate - system of the camera at the first time. - - :param cm: Needs method `get_camera_pose` accepting the time at which the - pose is desired. - :type cm: - - :param t1: First time (seconds). - - :param t2: Second time (seconds). - :type t2: float - """ - # R1 takes a world vector and moves it into the coordinate system of the camera - # at t=image_times[i]. - R1 = cm.get_camera_pose(t=t1)[:, :3] - # R2 takes a world vector and moves it into the coordinate system of the camera - # at t=image_times[i + 1]. - R2 = cm.get_camera_pose(t=t2)[:, :3] - - # R2 = R1*R1_2 - R1_2 = np.dot(R1.T, R2) - return cv2.Rodrigues(R1_2.T)[0].ravel() - - -def calibrate_camera_to_xyz(im_pts, wrld_pts, height, width, - fix_aspect_ratio=True, fix_principal_point=True, - fix_k1=True, fix_k2=True, fix_k3=True, fix_k4=True, - fix_k5=True, fix_k6=True, plot_results=False, - ref_image=None): - """ - :im_pts: Image coordinates associated with (x, y, z) coordinates. - :type im_pts: num_pts x 2 - - :param wrld_pts: World (x, y, z) coordinates associated with image - coordinates. - :type wrld_pts: num_pts x 3 - - :param fix_aspect_ratio: Fix the aspect ratio during optimization. - :type fix_aspect_ratio: - - :param fix_principal_point: Fix the principal point to during optimization. - :type fix_principal_point: - - :param fix_k1: Fix the 1st distortion coefficient during optimization. - :type fix_k1: bool - - :param fix_k2: Fix the 2nd distortion coefficient during optimization. - :type fix_k2: bool - - :param fix_k3: Fix the 3rd distortion coefficient during optimization - :type fix_k3: bool - - :param fix_k4: Fix the 4rth distortion coefficient during optimization - :type fix_k4: bool - - :param fix_k5: Fix the 5th distortion coefficient during optimization - :type fix_k5: bool - - :param fix_k6: Fix the 6th distortion coefficient during optimization - :type fix_k6: bool - - :param plot_results: Generate Matplotlib figures displaying results. - :type plot_results: bool - - :param ref_image: Reference image to show during plotting of results. - :type ref_image: Numpy image - - """ - def monte_carlo_calibrateCamera(min_focal_length=1, - max_focal_length=100000, - fix_aspect_ratio=True, - fix_principal_point=True, fix_k1=True, - fix_k2=True, fix_k3=True, fix_k4=True, - fix_k5=True, fix_k6=True, max_runtime=20, - best_ret=None): - - flags = cv2.CALIB_ZERO_TANGENT_DIST - flags = flags | cv2.CALIB_USE_INTRINSIC_GUESS - - if fix_principal_point: - flags = flags | cv2.CALIB_FIX_PRINCIPAL_POINT - - if fix_aspect_ratio: - flags = flags | cv2.CALIB_FIX_ASPECT_RATIO - - if fix_k1: - flags = flags | cv2.CALIB_FIX_K1 - - if fix_k2: - flags = flags | cv2.CALIB_FIX_K2 - - if fix_k3: - flags = flags | cv2.CALIB_FIX_K3 - - if fix_k4: - flags = flags | cv2.CALIB_FIX_K4 - - if fix_k5: - flags = flags | cv2.CALIB_FIX_K5 - - if fix_k6: - flags = flags | cv2.CALIB_FIX_K6 - - criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30000, - 0.0000001) - - min_flog = np.log10(min_focal_length) - max_flog = np.log10(max_focal_length) - - if best_ret is None: - best_err = np.inf - else: - best_err = best_ret[0] - - cutoff_time = time.time() + max_runtime - while time.time() < cutoff_time: - # Monte Carlo starting parameters. - - # Draw focal length logarithmic uniformly over exponents. - n = float(np.random.rand(1))*(max_flog - min_flog) + min_flog - f = 10**n - K = np.identity(3) - K[0, 2] = width/2 - K[1, 2] = height/2 - K[0, 0] = K[1, 1] = f - - ret = cv2.calibrateCamera([wrld_pts.astype(np.float32)], - [im_pts.astype(np.float32)], - (width, height), cameraMatrix=K, - distCoeffs=np.zeros(5), flags=flags, - criteria=criteria) - - err, K, dist, rvecs, tvecs = ret - rvec, tvec = rvecs[0], tvecs[0] - ret = err, K, dist, rvec, tvec - - if not check_valid_camera(width, height, K, dist, rvec, tvec): - continue - - im_pts2 = cv2.projectPoints(wrld_pts, rvec, tvec, K, dist) - err_ = np.sqrt(np.sum((np.squeeze(im_pts2[0]) - im_pts)**2, axis=1)) - - # Check that the world points are actually in front of the camera. - R = cv2.Rodrigues(rvec)[0] - cam_pos = -np.dot(R.T, tvec).ravel() - ray_dir0 = (wrld_pts - cam_pos).T - ray_dir0 /= np.sqrt(np.sum(ray_dir0**2, 0)) - ray_dir0 = np.dot(R, ray_dir0) - - ray_dir = np.ones((3, len(im_pts)), dtype=np.float) - ray_dir[:2] = np.squeeze(cv2.undistortPoints(im_pts, K, dist, R=None), 1).T - ray_dir /= np.sqrt(np.sum(ray_dir**2, 0)) - - if np.any(np.sum(ray_dir * ray_dir0, axis=0) < 0.999): - continue - - # Ignore correspondences that are 10X the mean error. - ind = err < 10*err_.mean() - - ret = cv2.calibrateCamera([wrld_pts[ind].astype(np.float32)], - [im_pts[ind].astype(np.float32)], - (width, height), cameraMatrix=K, - distCoeffs=np.zeros(5), flags=flags, - criteria=criteria) - - err, K, dist, rvecs, tvecs = ret - ret = err, K, dist, rvecs[0], tvecs[0] - - if not check_valid_camera(width, height, K, dist, rvec, tvec): - continue - - if err < best_err: - best_err = err - best_ret = ret - print('Current reprojection error:', best_err) - - return best_ret - - print('First Pass') - best_ret = monte_carlo_calibrateCamera(min_focal_length=1, - max_focal_length=100000, - fix_aspect_ratio=True, - fix_principal_point=True, - fix_k1=True, fix_k2=True, - fix_k3=True, fix_k4=True, - fix_k5=True, fix_k6=True, - max_runtime=20, best_ret=None) - - err, K, dist, rvec, tvec = best_ret - - if not fix_aspect_ratio or not fix_principal_point: - print('Second Pass') - best_ret = monte_carlo_calibrateCamera(min_focal_length=min(K[0, 0], - K[1, 1])/2, - max_focal_length=max(K[0, 0], - K[1, 1])*2, - fix_aspect_ratio=fix_aspect_ratio, - fix_principal_point=True, - fix_k1=fix_k1, fix_k2=True, - fix_k3=True, fix_k4=True, - fix_k5=True, fix_k6=True, - max_runtime=10, - best_ret=best_ret) - - err, K, dist, rvec, tvec = best_ret - - if not np.all([fix_k1, fix_k2, fix_k3, fix_k4, fix_k5, fix_k6]): - print('Final Pass') - best_ret = monte_carlo_calibrateCamera(min_focal_length=min(K[0, 0], - K[1, 1])*0.9, - max_focal_length=max(K[0, 0], - K[1, 1])/0.9, - fix_aspect_ratio=fix_aspect_ratio, - fix_principal_point=True, - fix_k1=fix_k1, fix_k2=fix_k2, - fix_k3=fix_k3, fix_k4=fix_k4, - fix_k5=fix_k5, fix_k6=fix_k6, - max_runtime=10, - best_ret=best_ret) - - err, K, dist, rvec, tvec = best_ret - - if not fix_principal_point: - best_ret = monte_carlo_calibrateCamera(min_focal_length=min(K[0, 0], - K[1, 1]), - max_focal_length=max(K[0, 0], - K[1, 1]), - fix_aspect_ratio=fix_aspect_ratio, - fix_principal_point=fix_principal_point, - fix_k1=fix_k1, fix_k2=fix_k2, - fix_k3=fix_k3, fix_k4=fix_k4, - fix_k5=fix_k5, fix_k6=fix_k6, - max_runtime=0.01, - best_ret=best_ret) - - err, K, dist, rvec, tvec = best_ret - - def plot_proj_err(im_pts, wrld_pts, K, dist, rvec, tvec): - im_pts2 = cv2.projectPoints(wrld_pts, rvec, tvec, K, dist) - im_pts2 = np.squeeze(im_pts2[0]) - plt.imshow(ref_image) - for i in range(len(im_pts)): - plt.plot(im_pts[i, 0], im_pts[i, 1], 'bo') - plt.plot(im_pts2[i, 0], im_pts2[i, 1], 'ro') - plt.plot([im_pts[i, 0], im_pts2[i, 0]], - [im_pts[i, 1], im_pts2[i, 1]], 'k--') - - if plot_results: - plt.figure() - plot_proj_err(im_pts, wrld_pts, K, dist, rvec, tvec) - - plt.figure() - plt.imshow(cv2.undistort(ref_image, K, dist)) - plt.axis('off') - plt.title('Undistorted', fontsize=20) - - return K, dist, rvec, tvec \ No newline at end of file diff --git a/kamera/colmap_processing/camera_models.py b/kamera/colmap_processing/camera_models.py index c1211f8..dc745ae 100644 --- a/kamera/colmap_processing/camera_models.py +++ b/kamera/colmap_processing/camera_models.py @@ -4,37 +4,28 @@ import cv2 import time import yaml -from scipy.interpolate import interp1d, RectBivariateSpline -from scipy.optimize import fmin, fminbound, minimize +from scipy.interpolate import RectBivariateSpline +from scipy.spatial.transform import Rotation import PIL from math import sqrt -import matplotlib.pyplot as plt - # Repository imports. -from kamera.colmap_processing.image_renderer import stitch_images from kamera.colmap_processing.platform_pose import PlatformPoseFixed from kamera.colmap_processing.geo_conversions import enu_to_llh, llh_to_enu from kamera.colmap_processing.rotations import ( - euler_from_quaternion, - quaternion_multiply, - quaternion_matrix, - quaternion_from_euler, - quaternion_inverse, - quaternion_from_matrix, - ) -import kamera.colmap_processing.dp as dp + quaternion_matrix, + quaternion_inverse, + quaternion_from_matrix, +) def to_str(v): - """Convert numerical values (scalar or float) to string for saving to yaml - - """ + """Convert numerical values (scalar or float) to string for saving to yaml""" if isinstance(v, np.ndarray): v = v.tolist() else: return str(v) - if isinstance(v, list): + if isinstance(v, list): if len(v) > 1: return repr(list(v)) else: @@ -57,13 +48,18 @@ class CamToCamTform(object): the view such that we can ignore parallax during transformation. """ + def __init__(self, src_cm, dst_cm): - if src_cm.platform_pose_provider != dst_cm.platform_pose_provider and \ - not isinstance(src_cm.platform_pose_provider, PlatformPoseFixed) and \ - not isinstance(dst_cm.platform_pose_provider, PlatformPoseFixed): - raise Exception('src_cm and dst_cm must have the same ' - 'platform_pose_provider indicating that the cameras ' - 'are rigidly mounted to the same platform') + if ( + src_cm.platform_pose_provider != dst_cm.platform_pose_provider + and not isinstance(src_cm.platform_pose_provider, PlatformPoseFixed) + and not isinstance(dst_cm.platform_pose_provider, PlatformPoseFixed) + ): + raise Exception( + "src_cm and dst_cm must have the same " + "platform_pose_provider indicating that the cameras " + "are rigidly mounted to the same platform" + ) self._src_cm = src_cm self._dst_cm = dst_cm @@ -84,11 +80,11 @@ def fit(self, tol=0.1, k=1): # the number of tiles. N = 10 while True: - dx = np.sqrt(w*h/N) - x = np.linspace(0, w, int(np.ceil(w/dx))) - y = np.linspace(0, h, int(np.ceil(h/dx))) - X,Y = np.meshgrid(x, y) - points = np.vstack([X.ravel(),Y.ravel()]) + dx = np.sqrt(w * h / N) + x = np.linspace(0, w, int(np.ceil(w / dx))) + y = np.linspace(0, h, int(np.ceil(h / dx))) + X, Y = np.meshgrid(x, y) + points = np.vstack([X.ravel(), Y.ravel()]) out_points = self.tform_rigorous(points) @@ -99,14 +95,14 @@ def fit(self, tol=0.1, k=1): self._model_y = RectBivariateSpline(x, y, out_y.T, kx=k, ky=k) # Test - x = np.linspace(0, w, int(np.ceil(w/dx))*2) - y = np.linspace(0, h, int(np.ceil(h/dx))*2) - X,Y = np.meshgrid(x, y) - points = np.vstack([X.ravel(),Y.ravel()]) + x = np.linspace(0, w, int(np.ceil(w / dx)) * 2) + y = np.linspace(0, h, int(np.ceil(h / dx)) * 2) + X, Y = np.meshgrid(x, y) + points = np.vstack([X.ravel(), Y.ravel()]) points_out = self.tform(points) points_out_truth = self.tform_rigorous(points) - err = np.sqrt(np.sum((points_out_truth - points_out)**2, 0)) + err = np.sqrt(np.sum((points_out_truth - points_out) ** 2, 0)) if np.max(err) < tol or N > 2000: break @@ -123,8 +119,8 @@ def tform(self, points): :rtype: numpy.ndarray of size (2,n) """ - if not hasattr(self, '_model_x'): - raise Exception('Must call \'fit\' before calling \'tform\'') + if not hasattr(self, "_model_x"): + raise Exception("Must call 'fit' before calling 'tform'") out_points = np.zeros_like(points) out_points[0] = self._model_x.ev(points[0], points[1]) @@ -148,7 +144,7 @@ def tform_rigorous(self, points): # We don't have a world model to intersect with, so we send it out # to "infinity". - point = (ray_pos + ray_dir*1e5) + point = ray_pos + ray_dir * 1e5 return self._dst_cm.project(point, -np.inf) @@ -182,35 +178,32 @@ def rt_from_quat_pos(position, quaternion): # system. So, we invert each quaternion. quaternion = quaternion_inverse(quaternion) - p = quaternion_matrix(quaternion) # R - p[:3,3] = -np.dot(p[:3,:3], position) # T + p = quaternion_matrix(quaternion) # R + p[:3, 3] = -np.dot(p[:3, :3], position) # T return p def load_from_file(filename, platform_pose_provider=None): - """Load from configuration yaml for any of the Camera subclasses. - - """ - with open(filename, 'r') as f: + """Load from configuration yaml for any of the Camera subclasses.""" + with open(filename, "r") as f: calib = yaml.safe_load(f) - if calib['model_type'] == 'standard': + if calib["model_type"] == "standard": return StandardCamera.load_from_file(filename, platform_pose_provider) - if calib['model_type'] == 'rolling_shutter': + if calib["model_type"] == "rolling_shutter": return RollingShutterCamera.load_from_file(filename, platform_pose_provider) - if calib['model_type'] == 'depth': + if calib["model_type"] == "depth": return DepthCamera.load_from_file(filename, platform_pose_provider) - if calib['model_type'] == 'static': + if calib["model_type"] == "static": return GeoStaticCamera.load_from_file(filename, platform_pose_provider) raise Exception() -def ray_intersect_plane(plane_point, plane_normal, ray_pos, ray_dir, - epsilon=1e-6): +def ray_intersect_plane(plane_point, plane_normal, ray_pos, ray_dir, epsilon=1e-6): """From https://rosettacode.org/wiki/Find_the_intersection_of_a_line_with_a_plane#Python :param ray_pos: Ray starting positions. @@ -260,6 +253,7 @@ class Camera(object): any time-varying parameters (e.g., navigation coordinate system state). """ + def __init__(self, width, height, platform_pose_provider=None): """ :param width: Width of the image provided by the imaging sensor, @@ -285,6 +279,7 @@ def __init__(self, width, height, platform_pose_provider=None): self._platform_pose_provider = platform_pose_provider self._depth_map = None + self.model_type = "standard" @property def width(self): @@ -312,35 +307,35 @@ def depth_map(self, value): @property def platform_pose_provider(self): - """Instance of a subclass of NavStateProvider - - """ + """Instance of a subclass of NavStateProvider""" return self._platform_pose_provider @platform_pose_provider.setter def platform_pose_provider(self, value): - """Instance of a subclass of NavStateProvider - - """ + """Instance of a subclass of NavStateProvider""" self._platform_pose_provider = value def __str__(self): - string = [''.join(['image_width: ',repr(self._width),'\n'])] - string.append(''.join(['image_height: ',repr(self._height),'\n'])) - string.append(''.join(['platform_pose_provider: ', - repr(self._platform_pose_provider)])) + string = ["".join(["image_width: ", repr(self._width), "\n"])] + string.append("".join(["image_height: ", repr(self._height), "\n"])) + string.append( + "".join(["platform_pose_provider: ", repr(self._platform_pose_provider)]) + ) try: # Some time-dependent cameras may not have a queue of values. - string.append(''.join(['\nifov: ', - '({:.6g},{:.6g})'.format(*self.ifov(np.inf)), - '\n'])) - string.append(''.join(['fov: ', - '({:.6},{:.6},{:.6})'.format(*self.fov(np.inf))])) + string.append( + "".join( + ["\nifov: ", "({:.6g},{:.6g})".format(*self.ifov(np.inf)), "\n"] + ) + ) + string.append( + "".join(["fov: ", "({:.6},{:.6},{:.6})".format(*self.fov(np.inf))]) + ) except: pass - return ''.join(string) + return "".join(string) def __repr__(self): return self.__str__() @@ -364,7 +359,7 @@ def get_param_array(self, param_list): """ params = np.zeros(0) for param in param_list: - params = np.hstack([params,getattr(self, param)]) + params = np.hstack([params, getattr(self, param)]) return params @@ -381,8 +376,8 @@ def set_param_array(self, param_list, params): ind = 0 for param in param_list: p0 = getattr(self, param) - if hasattr(p0, '__len__') and len(p0) > 1: - setattr(self, param, params[ind:ind+len(p0)]) + if hasattr(p0, "__len__") and len(p0) > 1: + setattr(self, param, params[ind : ind + len(p0)]) ind += len(p0) else: setattr(self, param, params[ind]) @@ -483,8 +478,10 @@ def unproject_to_llh(self, points, t=None, cov=None): h0 = self.platform_pose_provider.h0 if lat0 is None or lon0 is None or h0 is None: - raise Exception('\'platform_pose_provider\' must have \'lat0\', ' - '\'lon0\', and \'ho\' defined.') + raise Exception( + "'platform_pose_provider' must have 'lat0', " + "'lon0', and 'ho' defined." + ) points = np.array(points) if points.ndim == 1: @@ -493,13 +490,13 @@ def unproject_to_llh(self, points, t=None, cov=None): else: was_1d = False points = np.array(points) - points = np.reshape(points, (2,-1)) + points = np.reshape(points, (2, -1)) llh = [] geo_cov = [] for i in range(points.shape[1]): - xyz = self.unproject_to_depth(points[:,i], t) + xyz = self.unproject_to_depth(points[:, i], t) if np.all(np.isfinite(xyz)): llh.append(enu_to_llh(xyz[0], xyz[1], xyz[2], lat0, lon0, h0)) else: @@ -508,16 +505,14 @@ def unproject_to_llh(self, points, t=None, cov=None): if cov is not None: # Sample 10 random points and project each into enu coordinate # system - rpoints = np.random.multivariate_normal(points[:,i], cov[i], - 20) + rpoints = np.random.multivariate_normal(points[:, i], cov[i], 20) # Points must be inside image. - ind = np.logical_and(rpoints[:,0] > 0, rpoints[:,1] > 0) - ind = np.logical_and(ind, rpoints[:,0] < self.width) - ind = np.logical_and(ind, rpoints[:,1] < self.height) + ind = np.logical_and(rpoints[:, 0] > 0, rpoints[:, 1] > 0) + ind = np.logical_and(ind, rpoints[:, 0] < self.width) + ind = np.logical_and(ind, rpoints[:, 1] < self.height) rpoints = rpoints[ind] - enu_pts = ([self.unproject_to_depth(_, t).ravel() - for _ in rpoints]) + enu_pts = [self.unproject_to_depth(_, t).ravel() for _ in rpoints] enu_pts = [_ for _ in enu_pts if np.all(np.isfinite(enu_pts))] @@ -527,7 +522,7 @@ def unproject_to_llh(self, points, t=None, cov=None): enu_pts = np.array(enu_pts) - if xyz[0]**2 + xyz[1]**2 > 6250000: + if xyz[0] ** 2 + xyz[1] ** 2 > 6250000: # If the point is further than 2.5km from the camera, we # want the covariance defined in an east/north/up # coordinate system centered at xyz, the most-likely @@ -538,12 +533,17 @@ def unproject_to_llh(self, points, t=None, cov=None): llh0 = enu_to_llh(xyz[0], xyz[1], xyz[2], lat0, lon0, h0) for i in range(len(enu_pts)): - llhi = enu_to_llh(enu_pts[i,0], enu_pts[i,1], - enu_pts[i,2], llh0[0], llh0[1], - llh0[2]) - enu_pts[i,:] = llh_to_enu(llhi[0], llhi[1], llhi[2], - llh0[0], llh0[1], llh0[2]) - + llhi = enu_to_llh( + enu_pts[i, 0], + enu_pts[i, 1], + enu_pts[i, 2], + llh0[0], + llh0[1], + llh0[2], + ) + enu_pts[i, :] = llh_to_enu( + llhi[0], llhi[1], llhi[2], llh0[0], llh0[1], llh0[2] + ) geo_cov.append(np.cov(enu_pts.T)) @@ -553,7 +553,7 @@ def unproject_to_llh(self, points, t=None, cov=None): llh = np.array(llh).T if cov is not None: - return llh,geo_cov + return llh, geo_cov else: return llh @@ -568,25 +568,34 @@ def points_along_image_border(self, num_points=4): :rtype: numpy.ndarry with shape (3,n) """ - perimeter = 2*(self.height + self.width) - ds = num_points/float(perimeter) - xn = np.max([2,int(ds*self.width)]) - yn = np.max([2,int(ds*self.height)]) + perimeter = 2 * (self.height + self.width) + ds = num_points / float(perimeter) + xn = np.max([2, int(ds * self.width)]) + yn = np.max([2, int(ds * self.height)]) x = np.linspace(0, self.width, xn) y = np.linspace(0, self.height, yn)[1:-1] - pts = np.vstack([np.hstack([x, - np.full(len(y), self.width, - dtype=np.float64), - x[::-1], - np.zeros(len(y))]), - np.hstack([np.zeros(xn), - y, - np.full(xn, self.height, - dtype=np.float64), - y[::-1]])]) + pts = np.vstack( + [ + np.hstack( + [ + x, + np.full(len(y), self.width, dtype=np.float64), + x[::-1], + np.zeros(len(y)), + ] + ), + np.hstack( + [ + np.zeros(xn), + y, + np.full(xn, self.height, dtype=np.float64), + y[::-1], + ] + ), + ] + ) return pts - def ifov(self, t=None): """Instantaneous field of view (ifov) at the image center. @@ -603,13 +612,13 @@ def ifov(self, t=None): if t is None: t = time.time() - cx = self.width/2 - cy = self.height/2 - ray1 = self.unproject([cx,cy], t)[1] + cx = self.width / 2 + cy = self.height / 2 + ray1 = self.unproject([cx, cy], t)[1] ray1 /= np.sqrt(np.sum(ray1**2, 0)) - ray2 = self.unproject([cx,cy+1], t)[1] + ray2 = self.unproject([cx, cy + 1], t)[1] ray2 /= np.sqrt(np.sum(ray2**2, 0)) - ray3 = self.unproject([cx+1,cy], t)[1] + ray3 = self.unproject([cx + 1, cy], t)[1] ray3 /= np.sqrt(np.sum(ray3**2, 0)) ifovx = np.arccos(np.dot(ray1.ravel(), ray3.ravel())) @@ -632,33 +641,31 @@ def fov(self, t=None): if t is None: t = time.time() - cx = self.width/2 - cy = self.height/2 + cx = self.width / 2 + cy = self.height / 2 - ray1 = self.unproject([cx,0], t)[1] + ray1 = self.unproject([cx, 0], t)[1] ray1 /= np.sqrt(np.sum(ray1**2, 0)) - ray2 = self.unproject([cx,self.height], t)[1] + ray2 = self.unproject([cx, self.height], t)[1] ray2 /= np.sqrt(np.sum(ray2**2, 0)) - fov_v = np.arccos(np.dot(ray1.ravel(), ray2.ravel()))*180/np.pi + fov_v = np.arccos(np.dot(ray1.ravel(), ray2.ravel())) * 180 / np.pi - ray1 = self.unproject([0,cy], t)[1] + ray1 = self.unproject([0, cy], t)[1] ray1 /= np.sqrt(np.sum(ray1**2, 0)) ray2 = self.unproject([self.width, cy], t)[1] ray2 /= np.sqrt(np.sum(ray2**2, 0)) - fov_h = np.arccos(np.dot(ray1.ravel(), ray2.ravel()))*180/np.pi + fov_h = np.arccos(np.dot(ray1.ravel(), ray2.ravel())) * 180 / np.pi - ray1 = self.unproject([0,0], t)[1] + ray1 = self.unproject([0, 0], t)[1] ray1 /= np.sqrt(np.sum(ray1**2, 0)) - ray2 = self.unproject([self.width,self.height], t)[1] + ray2 = self.unproject([self.width, self.height], t)[1] ray2 /= np.sqrt(np.sum(ray2**2, 0)) - fov_d = np.arccos(np.dot(ray1.ravel(), ray2.ravel()))*180/np.pi + fov_d = np.arccos(np.dot(ray1.ravel(), ray2.ravel())) * 180 / np.pi return fov_h, fov_v, fov_d def unproject_to_depth(self, points, t=None): - """See Camera.unproject_to_depth documentation. - - """ + """See Camera.unproject_to_depth documentation.""" points = self._unproject_to_depth(points, self.depth_map, t=t) return points @@ -672,7 +679,7 @@ def save_depth_viz(self, fname): depth_image[depth_image < 0] = 0 v = vmax - vmin if v > 0: - depth_image /= v/255 + depth_image /= v / 255 depth_image = np.round(depth_image).astype(np.uint8) @@ -706,14 +713,15 @@ class StandardCamera(Camera): :type dist: numpy.ndarray """ - def __init__(self, width, height, K, dist, cam_pos, cam_quat, - platform_pose_provider=None): + + def __init__( + self, width, height, K, dist, cam_pos, cam_quat, platform_pose_provider=None + ): """ See additional documentation from base class above. """ - super(StandardCamera, self).__init__(width, height, - platform_pose_provider) + super(StandardCamera, self).__init__(width, height, platform_pose_provider) self._K = np.array(K, dtype=np.float64) self._dist = np.atleast_1d(dist).astype(np.float32) @@ -721,76 +729,72 @@ def __init__(self, width, height, K, dist, cam_pos, cam_quat, self._cam_quat = np.array(cam_quat, dtype=np.float64) self._cam_quat /= np.linalg.norm(self._cam_quat) self._min_ray_cos = None + self.model_type = "standard" def __str__(self): - string = ['model_type: standard\n'] + string = [f"model_type: {self.model_type}\n"] string.append(super(StandardCamera, self).__str__()) - string.append('\n') - string.append(''.join(['fx: ',repr(self._K[0,0]),'\n'])) - string.append(''.join(['fy: ',repr(self._K[1,1]),'\n'])) - string.append(''.join(['cx: ',repr(self._K[0,2]),'\n'])) - string.append(''.join(['cy: ',repr(self._K[1,2]),'\n'])) - string.append(''.join(['distortion_coefficients: ', - repr(tuple(self._dist)), - '\n'])) - string.append(''.join(['camera_quaternion: ', - repr(tuple(self._cam_quat)),'\n'])) - string.append(''.join(['camera_position: ',repr(tuple(self._cam_pos)), - '\n'])) - return ''.join(string) + string.append("\n") + string.append("".join(["fx: ", repr(self._K[0, 0]), "\n"])) + string.append("".join(["fy: ", repr(self._K[1, 1]), "\n"])) + string.append("".join(["cx: ", repr(self._K[0, 2]), "\n"])) + string.append("".join(["cy: ", repr(self._K[1, 2]), "\n"])) + string.append( + "".join(["distortion_coefficients: ", repr(tuple(self._dist)), "\n"]) + ) + string.append( + "".join(["camera_quaternion: ", repr(tuple(self._cam_quat)), "\n"]) + ) + string.append("".join(["camera_position: ", repr(tuple(self._cam_pos)), "\n"])) + return "".join(string) @classmethod def load_from_file(cls, filename, platform_pose_provider=None): - """See base class Camera documentation. - - """ - with open(filename, 'r') as f: + """See base class Camera documentation.""" + with open(filename, "r") as f: calib = yaml.safe_load(f) - assert calib['model_type'] == 'standard' + assert calib["model_type"] == "standard" # fill in CameraInfo fields - width = int(calib['image_width']) - height = int(calib['image_height']) - dist = calib['distortion_coefficients'] + width = int(calib["image_width"]) + height = int(calib["image_height"]) + dist = calib["distortion_coefficients"] - if dist == 'None': + if dist == "None": dist = np.zeros(4) dist = np.float64(dist) - fx = np.float64(calib['fx']) - fy = np.float64(calib['fy']) - cx = np.float64(calib['cx']) - cy = np.float64(calib['cy']) - K = np.array([[fx,0,cx],[0,fy,cy],[0,0,1]]) + fx = np.float64(calib["fx"]) + fy = np.float64(calib["fy"]) + cx = np.float64(calib["cx"]) + cy = np.float64(calib["cy"]) + K = np.array([[fx, 0, cx], [0, fy, cy], [0, 0, 1]]) - cam_quat = calib['camera_quaternion'] - cam_pos = calib['camera_position'] + cam_quat = calib["camera_quaternion"] + cam_pos = calib["camera_position"] - return cls(width, height, K, dist, cam_pos, cam_quat, - platform_pose_provider) + return cls(width, height, K, dist, cam_pos, cam_quat, platform_pose_provider) @classmethod def load_from_krtd(cls, filename): - """See base class Camera documentation. - - """ + """See base class Camera documentation.""" data = [] with open(filename) as f: for line in f.readlines(): - data.append(line.strip('\n')) + data.append(line.strip("\n")) - fx = float(data[0].split(' ' )[0]) - fy = float(data[1].split(' ' )[1]) - cx = float(data[0].split(' ' )[2]) - cy = float(data[1].split(' ' )[2]) + fx = float(data[0].split(" ")[0]) + fy = float(data[1].split(" ")[1]) + cx = float(data[0].split(" ")[2]) + cy = float(data[1].split(" ")[2]) R = np.zeros((3, 3)) for i in range(3): - R[i] = [float(d) for d in data[4 + i].split(' ')] + R[i] = [float(d) for d in data[4 + i].split(" ")] - tvec = [float(d) for d in data[8].split(' ')] + tvec = [float(d) for d in data[8].split(" ")] cam_pos = -np.dot(R.T, tvec).ravel() @@ -800,59 +804,102 @@ def load_from_krtd(cls, filename): width = None height = None - dist = [float(d) for d in data[10].split(' ') if len(d) > 0] + dist = [float(d) for d in data[10].split(" ") if len(d) > 0] dist = np.array(dist) - K = np.array([[fx, 0, cx], [0, fy, cy],[0, 0, 1]]) + K = np.array([[fx, 0, cx], [0, fy, cy], [0, 0, 1]]) return cls(width, height, K, dist, cam_pos, cam_quat) def save_to_file(self, filename): - """See base class Camera documentation. - - """ - with open(filename, 'w') as f: - f.write(''.join(['# The type of camera model.\n', - 'model_type: standard\n\n', - '# Image dimensions\n'])) - - f.write(''.join(['image_width: ',to_str(self.width),'\n'])) - f.write(''.join(['image_height: ',to_str(self.height),'\n\n'])) - - f.write('# Focal length along the image\'s x-axis.\n') - f.write(''.join(['fx: ',to_str(self.K[0,0]),'\n\n'])) - - f.write('# Focal length along the image\'s y-axis.\n') - f.write(''.join(['fy: ',to_str(self.K[1,1]),'\n\n'])) - - f.write('# Principal point is located at (cx,cy).\n') - f.write(''.join(['cx: ',to_str(self.K[0,2]),'\n'])) - f.write(''.join(['cy: ',to_str(self.K[1,2]),'\n\n'])) - - f.write(''.join(['# Distortion coefficients following OpenCv\'s ', - 'convention\n'])) + """See base class Camera documentation.""" + with open(filename, "w") as f: + f.write( + "".join( + [ + "# The type of camera model.\n", + f"model_type: {self.model_type}\n\n", + "# Image dimensions\n", + ] + ) + ) + + f.write("".join(["image_width: ", to_str(self.width), "\n"])) + f.write("".join(["image_height: ", to_str(self.height), "\n\n"])) + + f.write("# Focal length along the image's x-axis.\n") + f.write("".join(["fx: ", to_str(self.K[0, 0]), "\n\n"])) + + f.write("# Focal length along the image's y-axis.\n") + f.write("".join(["fy: ", to_str(self.K[1, 1]), "\n\n"])) + + f.write("# Principal point is located at (cx,cy).\n") + f.write("".join(["cx: ", to_str(self.K[0, 2]), "\n"])) + f.write("".join(["cy: ", to_str(self.K[1, 2]), "\n\n"])) + + f.write( + "".join( + ["# Distortion coefficients following OpenCv's ", "convention\n"] + ) + ) dist = self.dist if np.all(dist == 0): - dist = 'None' - - f.write(''.join(['distortion_coefficients: ', - to_str(self.dist),'\n\n'])) - - f.write(''.join(['# Quaternion (x, y, z, w) specifying the ', - 'orientation of the camera relative to\n# the ', - 'platform coordinate system. The quaternion ', - 'represents a coordinate\n# system rotation that ', - 'takes the platform coordinate system and ', - 'rotates it\n# into the camera coordinate ', - 'system.\ncamera_quaternion: ', - to_str(self.cam_quat),'\n\n'])) - - f.write(''.join(['# Position of the camera\'s center of ', - 'projection within the navigation\n# coordinate ', - 'system.\n', - 'camera_position: ', to_str(self.cam_pos), - '\n\n'])) + dist = "None" + + f.write("".join(["distortion_coefficients: ", to_str(self.dist), "\n\n"])) + + f.write( + "".join( + [ + "# Quaternion (x, y, z, w) specifying the ", + "orientation of the camera relative to\n# the ", + "platform coordinate system. The quaternion ", + "represents a coordinate\n# system rotation that ", + "takes the platform coordinate system and ", + "rotates it\n# into the camera coordinate ", + "system.\ncamera_quaternion: ", + to_str(self.cam_quat), + "\n\n", + ] + ) + ) + + f.write( + "".join( + [ + "# Position of the camera's center of ", + "projection within the navigation\n# coordinate ", + "system.\n", + "camera_position: ", + to_str(self.cam_pos), + "\n\n", + ] + ) + ) + + def save_to_krtd(self, filename): + """Write a single camera in ASCII KRTD format to the file object. + + Args: + camera (list[np.ndarray]): A length-4 of type (K, R, t, d) + fout (str | os.PathLike): _description_ + """ + K = self.K + R = Rotation.from_quat(self.cam_quat).as_matrix() + t = self.cam_pos + d = self.dist + t = np.reshape(np.array(t), 3) + with open(filename, "w") as fout: + fout.write("%.12g %.12g %.12g\n" % tuple(K.tolist()[0])) + fout.write("%.12g %.12g %.12g\n" % tuple(K.tolist()[1])) + fout.write("%.12g %.12g %.12g\n\n" % tuple(K.tolist()[2])) + fout.write("%.12g %.12g %.12g\n" % tuple(R.tolist()[0])) + fout.write("%.12g %.12g %.12g\n" % tuple(R.tolist()[1])) + fout.write("%.12g %.12g %.12g\n\n" % tuple(R.tolist()[2])) + fout.write("%.12g %.12g %.12g\n\n" % tuple(t.tolist())) + for v in d: + fout.write("%.12g " % v) @property def K(self): @@ -860,77 +907,74 @@ def K(self): @property def K_no_skew(self): - """Returns a compact version of K assuming no skew. - - """ + """Returns a compact version of K assuming no skew.""" K = self.K - return np.array([K[0,0],K[1,1],K[0,2],K[1,2]]) + return np.array([K[0, 0], K[1, 1], K[0, 2], K[1, 2]]) @K_no_skew.setter def K_no_skew(self, value): - """fx, fy, cx, cy - """ - K = np.zeros((3,3), dtype=np.float64) - K[0,0] = value[0] - K[1,1] = value[1] - K[0,2] = value[2] - K[1,2] = value[3] + """fx, fy, cx, cy""" + K = np.zeros((3, 3), dtype=np.float64) + K[0, 0] = value[0] + K[1, 1] = value[1] + K[0, 2] = value[2] + K[1, 2] = value[3] self._K = K self._min_ray_cos = None @property def focal_length(self): - return self._K[0,0] + return self._K[0, 0] @focal_length.setter def focal_length(self, value): - self._K[0,0] = value - self._K[1,1] = value + self._K[0, 0] = value + self._K[1, 1] = value self._min_ray_cos = None @property def fx(self): - return self._K[0,0] + return self._K[0, 0] @property def fy(self): - return self._K[1,1] + return self._K[1, 1] @fx.setter def fx(self, value): - self._K[0,0] = value + self._K[0, 0] = value self._min_ray_cos = None @fy.setter def fy(self, value): - self._K[1,1] = value + self._K[1, 1] = value self._min_ray_cos = None @property def cx(self): - return self._K[0,2] + return self._K[0, 2] @property def cy(self): - return self._K[1,2] + return self._K[1, 2] @cx.setter def cx(self, value): - self._K[0,2] = value + self._K[0, 2] = value self._min_ray_cos = None @cy.setter def cy(self, value): - self._K[1,2] = value + self._K[1, 2] = value self._min_ray_cos = None @property def aspect_ratio(self): - return self._K[0,0]/self._K[1,1] + return self._K[0, 0] / self._K[1, 1] @aspect_ratio.setter def aspect_ratio(self, value): - self._K[1,1] = self._K[0,0]*value + self._K[1, 1] = self._K[0, 0] * value @property def dist(self): @@ -948,6 +992,10 @@ def dist(self, value): def cam_pos(self): return self._cam_pos + @cam_pos.setter + def cam_pos(self, value): + self._cam_pos = value + @property def cam_quat(self): return self._cam_quat @@ -977,8 +1025,8 @@ def min_ray_cos(self): ray0 = self.unproject(center, t, normalize_ray_dir=True)[1].ravel() w, h = self.width, self.height self._min_ray_cos = 1 - for x,y in [[0,0],[w,0],[w,h],[0,h]]: - ray1 = self.unproject([x, y], t, normalize_ray_dir=True)[1] + for x, y in [[0, 0], [w, 0], [w, h], [0, h]]: + ray1 = self.unproject([x, y], t, normalize_ray_dir=True)[1] ray1 = ray1.ravel() ray_cosi = np.dot(ray0, ray1) self._min_ray_cos = np.minimum(self._min_ray_cos, ray_cosi) @@ -988,8 +1036,7 @@ def min_ray_cos(self): return self._min_ray_cos def update_intrinsics(self, K=None, cam_quat=None, dist=None): - """ - """ + """ """ if K is not None: self._K = K.astype(np.float64) if cam_quat is not None: @@ -1022,9 +1069,7 @@ def get_camera_pose(self, t=None): return np.dot(p_cam, p_ins)[:3] def project(self, points, t=None): - """See Camera.project documentation. - - """ + """See Camera.project documentation.""" points = np.array(points, dtype=np.float64) if points.ndim == 1: points = np.atleast_2d(points).T @@ -1033,53 +1078,50 @@ def project(self, points, t=None): t = time.time() pose_mat = self.get_camera_pose(t) - pose_mat = np.vstack((pose_mat, np.array([0,0,0,1]))) + pose_mat = np.vstack((pose_mat, np.array([0, 0, 0, 1]))) # Project rays into camera coordinate system. - rvec = cv2.Rodrigues(pose_mat[:3,:3])[0].ravel() + rvec = cv2.Rodrigues(pose_mat[:3, :3])[0].ravel() tvec = pose_mat[:3, 3] - im_pts = cv2.projectPoints(points.T, rvec, tvec, self._K, - self._dist)[0] + im_pts = cv2.projectPoints(points.T, rvec, tvec, self._K, self._dist)[0] im_pts = np.squeeze(im_pts, 1).T # Make homogeneous points = np.vstack([points, np.ones(points.shape[1])]) points = np.dot(pose_mat, points) - #points /= np.sqrt(np.sum(points**2, 0)) + # points /= np.sqrt(np.sum(points**2, 0)) points /= points[3, :] # Add the 1e-8 to avoid "falling off the focal plane" due to rounding # error. # ind = points[2] <= self.min_ray_cos - #im_pts[:, ind] = np.nan + # im_pts[:, ind] = np.nan return im_pts def unproject(self, points, t=None, normalize_ray_dir=True): - """See Camera.unproject documentation. - - """ + """See Camera.unproject documentation.""" points = np.array(points, dtype=np.float64) if points.ndim == 1: points = np.atleast_2d(points).T - points = np.reshape(points, (2,-1)) + points = np.reshape(points, (2, -1)) if t is None: t = time.time() ins_pos, ins_quat = self.platform_pose_provider.pose(t) - #print('ins_pos', ins_pos) - #print('ins_quat', ins_quat) # Unproject rays into the camera coordinate system. - ray_dir = np.ones((3,points.shape[1]), dtype=points.dtype) - ray_dir0 = cv2.undistortPoints(np.expand_dims(points.T, 1), - self._K, self._dist, R=None) + ray_dir = np.ones((3, points.shape[1]), dtype=points.dtype) + ray_dir0 = cv2.undistortPoints( + np.expand_dims(points.T, 1), self._K, self._dist, R=None + ) ray_dir[:2] = np.squeeze(ray_dir0, 1).T + R_cam_to_world = Rotation.from_quat(self._cam_quat).as_matrix() # Rotate rays into the navigation coordinate system. - ray_dir = np.dot(quaternion_matrix(self._cam_quat)[:3,:3], ray_dir) + ray_dir = np.dot(R_cam_to_world, ray_dir) # Translate ray positions into their navigation coordinate system # definition. @@ -1089,13 +1131,13 @@ def unproject(self, points, t=None, normalize_ray_dir=True): ray_pos[2] = self._cam_pos[2] # Rotate and translate rays into the world coordinate system. - R_ins_to_world = quaternion_matrix(ins_quat)[:3,:3] + R_ins_to_world = Rotation.from_quat(ins_quat).as_matrix() ray_dir = np.dot(R_ins_to_world, ray_dir) ray_pos = np.dot(R_ins_to_world, ray_pos) + np.atleast_2d(ins_pos).T if normalize_ray_dir: # Normalize - ray_dir /= np.sqrt(np.sum(ray_dir**2, 0)) + ray_dir /= np.sqrt(np.sum(ray_dir ** 2, 0)) return ray_pos, ray_dir @@ -1126,108 +1168,143 @@ class RollingShutterCamera(StandardCamera): :type dist: numpy.ndarray """ - def __init__(self, width, height, K, dist, cam_pos, cam_quat, - shutter_roll_time, platform_pose_provider=None): + + def __init__( + self, + width, + height, + K, + dist, + cam_pos, + cam_quat, + shutter_roll_time, + platform_pose_provider=None, + ): """ See additional documentation from base class above. """ - super(RollingShutterCamera, self).__init__(width, height, K, dist, - cam_pos, cam_quat, - platform_pose_provider) + super(RollingShutterCamera, self).__init__( + width, height, K, dist, cam_pos, cam_quat, platform_pose_provider + ) self.shutter_roll_time = shutter_roll_time def __str__(self): - string = ['model_type: rolling_shutter\n'] + string = ["model_type: rolling_shutter\n"] string.append(super(RollingShutterCamera, self).__str__()) - string.append('shutter_roll_time: %s\n' %self.shutter_roll_time) - return ''.join(string) + string.append("shutter_roll_time: %s\n" % self.shutter_roll_time) + return "".join(string) @classmethod def load_from_file(cls, filename, platform_pose_provider=None): - """See base class Camera documentation. - - """ - with open(filename, 'r') as f: + """See base class Camera documentation.""" + with open(filename, "r") as f: calib = yaml.safe_load(f) - assert calib['model_type'] == 'rolling_shutter' + assert calib["model_type"] == "rolling_shutter" # fill in CameraInfo fields - width = calib['image_width'] - height = calib['image_height'] - shutter_roll_time = calib['shutter_roll_time'] - dist = calib['distortion_coefficients'] + width = calib["image_width"] + height = calib["image_height"] + shutter_roll_time = calib["shutter_roll_time"] + dist = calib["distortion_coefficients"] - if dist == 'None': + if dist == "None": dist = np.zeros(4) - fx = calib['fx'] - fy = calib['fy'] - cx = calib['cx'] - cy = calib['cy'] - K = np.array([[fx,0,cx],[0,fy,cy],[0,0,1]]) - - cam_quat = calib['camera_quaternion'] - cam_pos = calib['camera_position'] - - return cls(width, height, K, dist, cam_pos, cam_quat, - shutter_roll_time, platform_pose_provider) + fx = calib["fx"] + fy = calib["fy"] + cx = calib["cx"] + cy = calib["cy"] + K = np.array([[fx, 0, cx], [0, fy, cy], [0, 0, 1]]) + + cam_quat = calib["camera_quaternion"] + cam_pos = calib["camera_position"] + + return cls( + width, + height, + K, + dist, + cam_pos, + cam_quat, + shutter_roll_time, + platform_pose_provider, + ) def save_to_file(self, filename): - """See base class Camera documentation. - - """ - with open(filename, 'w') as f: - f.write(''.join(['# The type of camera model.\n', - 'model_type: rolling_shutter\n\n', - '# Image dimensions\n'])) - - f.write(''.join(['image_width: ',to_str(self.width),'\n'])) - f.write(''.join(['image_height: ',to_str(self.height),'\n\n'])) - - f.write(''.join(['shutter_roll_time: ', - to_str(self.shutter_roll_time),'\n\n'])) - - f.write('# Focal length along the image\'s x-axis.\n') - f.write(''.join(['fx: ',to_str(self.K[0,0]),'\n\n'])) - - f.write('# Focal length along the image\'s y-axis.\n') - f.write(''.join(['fy: ',to_str(self.K[1,1]),'\n\n'])) - - f.write('# Principal point is located at (cx,cy).\n') - f.write(''.join(['cx: ',to_str(self.K[0,2]),'\n'])) - f.write(''.join(['cy: ',to_str(self.K[1,2]),'\n\n'])) - - f.write(''.join(['# Distortion coefficients following OpenCv\'s ', - 'convention\n'])) + """See base class Camera documentation.""" + with open(filename, "w") as f: + f.write( + "".join( + [ + "# The type of camera model.\n", + "model_type: rolling_shutter\n\n", + "# Image dimensions\n", + ] + ) + ) + + f.write("".join(["image_width: ", to_str(self.width), "\n"])) + f.write("".join(["image_height: ", to_str(self.height), "\n\n"])) + + f.write( + "".join(["shutter_roll_time: ", to_str(self.shutter_roll_time), "\n\n"]) + ) + + f.write("# Focal length along the image's x-axis.\n") + f.write("".join(["fx: ", to_str(self.K[0, 0]), "\n\n"])) + + f.write("# Focal length along the image's y-axis.\n") + f.write("".join(["fy: ", to_str(self.K[1, 1]), "\n\n"])) + + f.write("# Principal point is located at (cx,cy).\n") + f.write("".join(["cx: ", to_str(self.K[0, 2]), "\n"])) + f.write("".join(["cy: ", to_str(self.K[1, 2]), "\n\n"])) + + f.write( + "".join( + ["# Distortion coefficients following OpenCv's ", "convention\n"] + ) + ) dist = self.dist if np.all(dist == 0): - dist = 'None' - - f.write(''.join(['distortion_coefficients: ', - to_str(self.dist),'\n\n'])) - - f.write(''.join(['# Quaternion (x, y, z, w) specifying the ', - 'orientation of the camera relative to\n# the ', - 'platform coordinate system. The quaternion ', - 'represents a coordinate\n# system rotation that ', - 'takes the platform coordinate system and ', - 'rotates it\n# into the camera coordinate ', - 'system.\ncamera_quaternion: ', - to_str(self.cam_quat),'\n\n'])) - - f.write(''.join(['# Position of the camera\'s center of ', - 'projection within the navigation\n# coordinate ', - 'system.\n', - 'camera_position: ',to_str(self.cam_pos), - '\n\n'])) + dist = "None" + + f.write("".join(["distortion_coefficients: ", to_str(self.dist), "\n\n"])) + + f.write( + "".join( + [ + "# Quaternion (x, y, z, w) specifying the ", + "orientation of the camera relative to\n# the ", + "platform coordinate system. The quaternion ", + "represents a coordinate\n# system rotation that ", + "takes the platform coordinate system and ", + "rotates it\n# into the camera coordinate ", + "system.\ncamera_quaternion: ", + to_str(self.cam_quat), + "\n\n", + ] + ) + ) + + f.write( + "".join( + [ + "# Position of the camera's center of ", + "projection within the navigation\n# coordinate ", + "system.\n", + "camera_position: ", + to_str(self.cam_pos), + "\n\n", + ] + ) + ) def project(self, points, t=None): - """See Camera.project documentation. - - """ + """See Camera.project documentation.""" # The challenge projecting into a rolling shutter camera is that every # row of the image is exposed at a different time. So, if you assume a # particular time to evaluate the pose at and then project into the @@ -1238,7 +1315,7 @@ def project(self, points, t=None): # We start by projecting assuming all points are at the time associated # with the center of the field of view. - im_pts = proj_fun(points, t + 0.5*self.shutter_roll_time) + im_pts = proj_fun(points, t + 0.5 * self.shutter_roll_time) if False: # Slower but more accurate. @@ -1255,27 +1332,29 @@ def project(self, points, t=None): if ind[i]: # The fraction of the rolling shutter time this y # coordinate has accumulated. - alpha = np.clip(im_pts[1, i]/self.height, 0, 1) - t_ = t + alpha*self.shutter_roll_time - im_pt_ = proj_fun(points[:, i:i+1], t_) - d = sqrt(np.sum((im_pt_ - im_pts[:, i:i+1])**2)) + alpha = np.clip(im_pts[1, i] / self.height, 0, 1) + t_ = t + alpha * self.shutter_roll_time + im_pt_ = proj_fun(points[:, i : i + 1], t_) + d = sqrt(np.sum((im_pt_ - im_pts[:, i : i + 1]) ** 2)) if d > 0.01: cont = True else: ind[i] = False - im_pts[:, i:i+1] = im_pt_ + im_pts[:, i : i + 1] = im_pt_ if not cont: break else: N = 10 alphas = np.linspace(0, 1, N) - im_pts_list = [proj_fun(points, t + alpha*self.shutter_roll_time).T - for alpha in alphas] + im_pts_list = [ + proj_fun(points, t + alpha * self.shutter_roll_time).T + for alpha in alphas + ] im_pts_list = np.array(im_pts_list).T - alphas2 = np.clip(im_pts_list[1]/self.height, 0, 1) + alphas2 = np.clip(im_pts_list[1] / self.height, 0, 1) alpha_err = alphas2 - alphas # We want to interpolate to the zero-valued alpha error. @@ -1295,32 +1374,40 @@ def project(self, points, t=None): delta = alpha_err1 - alpha_err2 w = np.ones(len(alpha_err1)) ind = delta != 0 - w[ind] = (alpha_err1[ind])/delta[ind] + w[ind] = (alpha_err1[ind]) / delta[ind] - im_pts1 = np.hstack([np.take_along_axis(im_pts_list[0], ind1, axis=1), - np.take_along_axis(im_pts_list[1], ind1, axis=1)]).T + im_pts1 = np.hstack( + [ + np.take_along_axis(im_pts_list[0], ind1, axis=1), + np.take_along_axis(im_pts_list[1], ind1, axis=1), + ] + ).T - im_pts2 = np.hstack([np.take_along_axis(im_pts_list[0], ind2, axis=1), - np.take_along_axis(im_pts_list[1], ind2, axis=1)]).T + im_pts2 = np.hstack( + [ + np.take_along_axis(im_pts_list[0], ind2, axis=1), + np.take_along_axis(im_pts_list[1], ind2, axis=1), + ] + ).T - im_pts = w*im_pts2 + (1-w)*im_pts1 + im_pts = w * im_pts2 + (1 - w) * im_pts1 return im_pts def unproject(self, points, t, normalize_ray_dir=True): - """See Camera.unproject documentation. - - """ + """See Camera.unproject documentation.""" points = np.array(points, dtype=np.float64) if points.ndim == 1: points = np.atleast_2d(points).T - points = np.reshape(points, (2,-1)) + points = np.reshape(points, (2, -1)) - alphas = np.clip(points[1]/self.height, 0, 1) - ts_ = t + (alphas*self.shutter_roll_time).astype(np.float64) + alphas = np.clip(points[1] / self.height, 0, 1) + ts_ = t + (alphas * self.shutter_roll_time).astype(np.float64) - ret = [super(RollingShutterCamera, self).unproject(points[:, i:i+1], ts_[i]) - for i in range(len(ts_))] + ret = [ + super(RollingShutterCamera, self).unproject(points[:, i : i + 1], ts_[i]) + for i in range(len(ts_)) + ] ray_pos = np.hstack([ret_[0] for ret_ in ret]) ray_dir = np.hstack([ret_[1] for ret_ in ret]) @@ -1328,125 +1415,166 @@ def unproject(self, points, t, normalize_ray_dir=True): class DepthCamera(StandardCamera): - """Camera with depth map. - - """ - def __init__(self, width, height, K, dist, cam_pos, cam_quat, depth_map, - platform_pose_provider=None): + """Camera with depth map.""" + + def __init__( + self, + width, + height, + K, + dist, + cam_pos, + cam_quat, + depth_map, + platform_pose_provider=None, + ): """ See additional documentation from base class above. """ - super(DepthCamera, self).__init__(width=width, height=height, K=K, - dist=dist, cam_pos=cam_pos, - cam_quat=cam_quat, - platform_pose_provider=platform_pose_provider) + super(DepthCamera, self).__init__( + width=width, + height=height, + K=K, + dist=dist, + cam_pos=cam_pos, + cam_quat=cam_quat, + platform_pose_provider=platform_pose_provider, + ) self._depth_map = depth_map + self.model_type = "depth" @classmethod def load_from_file(cls, filename, platform_pose_provider=None): - """See base class Camera documentation. - - """ - with open(filename, 'r') as f: + """See base class Camera documentation.""" + with open(filename, "r") as f: calib = yaml.safe_load(f) - assert calib['model_type'] == 'depth' + assert calib["model_type"] == "depth" # fill in CameraInfo fields - width = calib['image_width'] - height = calib['image_height'] - dist = calib['distortion_coefficients'] + width = calib["image_width"] + height = calib["image_height"] + dist = calib["distortion_coefficients"] - if dist == 'None': + if dist == "None": dist = np.zeros(4) - fx = calib['fx'] - fy = calib['fy'] - cx = calib['cx'] - cy = calib['cy'] - K = np.array([[fx,0,cx],[0,fy,cy],[0,0,1]]) - - cam_quat = calib['camera_quaternion'] - cam_pos = calib['camera_position'] - - return cls(width, height, K, dist, cam_pos, cam_quat, - platform_pose_provider) - - def save_to_file(self, filename, save_depth_viz=True): - """See base class Camera documentation. - - """ - with open(filename, 'w') as f: - f.write(''.join(['# The type of camera model.\n', - 'model_type: depth\n\n', - '# Image dimensions\n'])) - - f.write(''.join(['image_width: ',to_str(self.width),'\n'])) - f.write(''.join(['image_height: ',to_str(self.height),'\n\n'])) + fx = calib["fx"] + fy = calib["fy"] + cx = calib["cx"] + cy = calib["cy"] + K = np.array([[fx, 0, cx], [0, fy, cy], [0, 0, 1]]) - f.write('# Focal length along the image\'s x-axis.\n') - f.write(''.join(['fx: ',to_str(self.K[0,0]),'\n\n'])) + cam_quat = calib["camera_quaternion"] + cam_pos = calib["camera_position"] - f.write('# Focal length along the image\'s y-axis.\n') - f.write(''.join(['fy: ',to_str(self.K[1,1]),'\n\n'])) + depth_map_fname = "%s_depth_map.tif" % os.path.splitext(filename)[0] + try: + depth_map = np.asarray(PIL.Image.open(depth_map_fname)) + except OSError: + depth_map = None - f.write('# Principal point is located at (cx,cy).\n') - f.write(''.join(['cx: ',to_str(self.K[0,2]),'\n'])) - f.write(''.join(['cy: ',to_str(self.K[1,2]),'\n\n'])) + return cls( + width, height, K, dist, cam_pos, cam_quat, depth_map, platform_pose_provider + ) - f.write(''.join(['# Distortion coefficients following OpenCv\'s ', - 'convention\n'])) + def save_to_file(self, filename, save_depth_viz=True): + """See base class Camera documentation.""" + with open(filename, "w") as f: + f.write( + "".join( + [ + "# The type of camera model.\n", + "model_type: depth\n\n", + "# Image dimensions\n", + ] + ) + ) + + f.write("".join(["image_width: ", to_str(self.width), "\n"])) + f.write("".join(["image_height: ", to_str(self.height), "\n\n"])) + + f.write("# Focal length along the image's x-axis.\n") + f.write("".join(["fx: ", to_str(self.K[0, 0]), "\n\n"])) + + f.write("# Focal length along the image's y-axis.\n") + f.write("".join(["fy: ", to_str(self.K[1, 1]), "\n\n"])) + + f.write("# Principal point is located at (cx,cy).\n") + f.write("".join(["cx: ", to_str(self.K[0, 2]), "\n"])) + f.write("".join(["cy: ", to_str(self.K[1, 2]), "\n\n"])) + + f.write( + "".join( + ["# Distortion coefficients following OpenCv's ", "convention\n"] + ) + ) dist = self.dist if np.all(dist == 0): - dist = 'None' - - f.write(''.join(['distortion_coefficients: ', - to_str(self.dist),'\n\n'])) - - f.write(''.join(['# Quaternion (x, y, z, w) specifying the ', - 'orientation of the camera relative to\n# the ', - 'navigation coordinate system. The quaternion ', - 'represents a coordinate\n# system rotation that ', - 'takes the navigation coordinate system and ', - 'rotates it\n# into the camera coordinate ', - 'system.\n camera_quaternion: ', - to_str(self.cam_quat),'\n\n'])) - - f.write(''.join(['# Position of the camera\'s center of ', - 'projection within the navigation\n# coordinate ', - 'system.\n', - 'camera_position: ',to_str(self.cam_pos), - '\n\n'])) + dist = "None" + + f.write("".join(["distortion_coefficients: ", to_str(self.dist), "\n\n"])) + + f.write( + "".join( + [ + "# Quaternion (x, y, z, w) specifying the ", + "orientation of the camera relative to\n# the ", + "navigation coordinate system. The quaternion ", + "represents a coordinate\n# system rotation that ", + "takes the navigation coordinate system and ", + "rotates it\n# into the camera coordinate ", + "system.\n camera_quaternion: ", + to_str(self.cam_quat), + "\n\n", + ] + ) + ) + + f.write( + "".join( + [ + "# Position of the camera's center of ", + "projection within the navigation\n# coordinate ", + "system.\n", + "camera_position: ", + to_str(self.cam_pos), + "\n\n", + ] + ) + ) if self.depth_map is not None: - im = PIL.Image.fromarray(self.depth_map.astype(np.float32), - mode='F') # float32 - depth_map_fname = '%s_depth_map.tif' % os.path.splitext(filename)[0] + im = PIL.Image.fromarray( + self.depth_map.astype(np.float32), mode="F" + ) # float32 + depth_map_fname = "%s_depth_map.tif" % os.path.splitext(filename)[0] im.save(depth_map_fname) if save_depth_viz: - depth_viz_fname = ('%s/depth_vizualization.png' % - os.path.split(filename)[0]) + depth_viz_fname = ( + "%s/depth_vizualization.png" % os.path.splitext(filename)[0] + ) self.save_depth_viz(depth_viz_fname) def __str__(self): - string = ['model_type: depth\n'] + string = ["model_type: depth\n"] string.append(super(DepthCamera, self).__str__()) - string.append('\n') - string.append(''.join(['fx: ',repr(self._K[0,0]),'\n'])) - string.append(''.join(['fy: ',repr(self._K[1,1]),'\n'])) - string.append(''.join(['cx: ',repr(self._K[0,2]),'\n'])) - string.append(''.join(['cy: ',repr(self._K[1,2]),'\n'])) - string.append(''.join(['distortion_coefficients: ', - repr(tuple(self._dist)), - '\n'])) - string.append(''.join(['camera_quaternion: ', - repr(tuple(self._cam_quat)),'\n'])) - string.append(''.join(['camera_position: ',repr(tuple(self._cam_pos)), - '\n'])) - return ''.join(string) + string.append("\n") + string.append("".join(["fx: ", repr(self._K[0, 0]), "\n"])) + string.append("".join(["fy: ", repr(self._K[1, 1]), "\n"])) + string.append("".join(["cx: ", repr(self._K[0, 2]), "\n"])) + string.append("".join(["cy: ", repr(self._K[1, 2]), "\n"])) + string.append( + "".join(["distortion_coefficients: ", repr(tuple(self._dist)), "\n"]) + ) + string.append( + "".join(["camera_quaternion: ", repr(tuple(self._cam_quat)), "\n"]) + ) + string.append("".join(["camera_position: ", repr(tuple(self._cam_pos)), "\n"])) + return "".join(string) def _unproject_to_depth(self, points, depth_map, t=None): """Unproject image points into the world at a particular time. @@ -1468,11 +1596,11 @@ def _unproject_to_depth(self, points, depth_map, t=None): """ points = np.atleast_2d(points) - points = np.reshape(points, (2,-1)) + points = np.reshape(points, (2, -1)) ray_pos, ray_dir = self.unproject(points, t=t, normalize_ray_dir=False) for i in range(points.shape[1]): - x,y = points[:,i] + x, y = points[:, i] # Get ray distance traveled until intersection. Therefore, we need # to evaluate the depth map at x,y. We need to convert from image # coordinates (i.e., upper-left corner of upper-left pixel is 0,0) @@ -1495,11 +1623,12 @@ def _unproject_to_depth(self, points, depth_map, t=None): if ix < 0 or iy < 0 or ix >= self.width or iy >= self.height: print(x == self.width) print(y == self.height) - raise ValueError('Coordinates (%0.1f,%0.f) are outside the ' - '%ix%i image' % - (x,y,self.width,self.height)) + raise ValueError( + "Coordinates (%0.1f,%0.f) are outside the " + "%ix%i image" % (x, y, self.width, self.height) + ) - ray_pos[:,i] += ray_dir[:,i]*depth_map[iy,ix] + ray_pos[:, i] += ray_dir[:, i] * depth_map[iy, ix] return ray_pos @@ -1510,8 +1639,10 @@ class GeoStaticCamera(DepthCamera): width, height, K, dist, lat, lon, altitude, cam_quat """ - def __init__(self, width, height, K, dist, depth_map, latitude, longitude, - altitude, R): + + def __init__( + self, width, height, K, dist, depth_map, latitude, longitude, altitude, R + ): """ See additional documentation from base class above. @@ -1527,140 +1658,160 @@ def __init__(self, width, height, K, dist, depth_map, latitude, longitude, """ R = np.array(R) R /= np.linalg.det(R) - cam_pos = np.array([0,0,0]) - cam_quat = np.array([0,0,0,1]) + cam_pos = np.array([0, 0, 0]) + cam_quat = np.array([0, 0, 0, 1]) # Quaternion for level system (z down) with x-axis pointing north. - enu_quat = np.array([1/np.sqrt(2),1/np.sqrt(2),0,0]) - - platform_pose_provider = PlatformPoseFixed(pos=np.array([0,0,0]), - quat=enu_quat, - lat0=latitude, lon0=longitude, - h0=altitude) - - super(GeoStaticCamera, self).__init__(width=width, height=height, K=K, - dist=dist, cam_pos=cam_pos, - cam_quat=cam_quat, - depth_map=depth_map, - platform_pose_provider=platform_pose_provider) + enu_quat = np.array([1 / np.sqrt(2), 1 / np.sqrt(2), 0, 0]) + + platform_pose_provider = PlatformPoseFixed( + pos=np.array([0, 0, 0]), + quat=enu_quat, + lat0=latitude, + lon0=longitude, + h0=altitude, + ) + + super(GeoStaticCamera, self).__init__( + width=width, + height=height, + K=K, + dist=dist, + cam_pos=cam_pos, + cam_quat=cam_quat, + depth_map=depth_map, + platform_pose_provider=platform_pose_provider, + ) self._R = R self._depth_map = depth_map # The local ENU coordinate system is located at the camera. - self._tvec = np.array([[0],[0],[0]], dtype=np.float64) - self._camera_pose = np.hstack([R,self._tvec]) + self._tvec = np.array([[0], [0], [0]], dtype=np.float64) + self._camera_pose = np.hstack([R, self._tvec]) def __str__(self): - string = ['model_type: static\n'] + string = ["model_type: static\n"] string.append(super(GeoStaticCamera, self).__str__()) - string.append('\n') - string.append(''.join(['fx: ',repr(self._K[0,0]),'\n'])) - string.append(''.join(['fy: ',repr(self._K[1,1]),'\n'])) - string.append(''.join(['cx: ',repr(self._K[0,2]),'\n'])) - string.append(''.join(['cy: ',repr(self._K[1,2]),'\n'])) - string.append(''.join(['distortion_coefficients: ', - repr(tuple(self._dist)), - '\n'])) - string.append(''.join(['latitude: %0.8f' % self.latitude, - '\n'])) - string.append(''.join(['longitude: %0.8f' % self.longitude, - '\n'])) - string.append(''.join(['altitude: %0.8f' % self.altitude, - '\n'])) - string.append(''.join(['R: ', - repr(tuple(self.R)),'\n'])) - return ''.join(string) + string.append("\n") + string.append("".join(["fx: ", repr(self._K[0, 0]), "\n"])) + string.append("".join(["fy: ", repr(self._K[1, 1]), "\n"])) + string.append("".join(["cx: ", repr(self._K[0, 2]), "\n"])) + string.append("".join(["cy: ", repr(self._K[1, 2]), "\n"])) + string.append( + "".join(["distortion_coefficients: ", repr(tuple(self._dist)), "\n"]) + ) + string.append("".join(["latitude: %0.8f" % self.latitude, "\n"])) + string.append("".join(["longitude: %0.8f" % self.longitude, "\n"])) + string.append("".join(["altitude: %0.8f" % self.altitude, "\n"])) + string.append("".join(["R: ", repr(tuple(self.R)), "\n"])) + return "".join(string) @classmethod def load_from_file(cls, filename, platform_pose_provider=None): - """See base class Camera documentation. - - """ - with open(filename, 'r') as f: + """See base class Camera documentation.""" + with open(filename, "r") as f: calib = yaml.safe_load(f) - assert calib['model_type'] == 'static' + assert calib["model_type"] == "static" # fill in CameraInfo fields - width = calib['image_width'] - height = calib['image_height'] - dist = np.array(calib['distortion_coefficients'], dtype=np.float64) + width = calib["image_width"] + height = calib["image_height"] + dist = np.array(calib["distortion_coefficients"], dtype=np.float64) - if isinstance(dist, str) and dist == 'None': + if isinstance(dist, str) and dist == "None": dist = np.zeros(4, dtype=np.float64) - fx = calib['fx'] - fy = calib['fy'] - cx = calib['cx'] - cy = calib['cy'] - K = np.array([[fx,0,cx],[0,fy,cy],[0,0,1]]) - R = np.reshape(np.array(calib['R']), (3,3)) - latitude = calib['latitude'] - longitude = calib['longitude'] - altitude = calib['altitude'] - - depth_map_fname = '%s_depth_map.tif' % os.path.splitext(filename)[0] + fx = calib["fx"] + fy = calib["fy"] + cx = calib["cx"] + cy = calib["cy"] + K = np.array([[fx, 0, cx], [0, fy, cy], [0, 0, 1]]) + R = np.reshape(np.array(calib["R"]), (3, 3)) + latitude = calib["latitude"] + longitude = calib["longitude"] + altitude = calib["altitude"] + + depth_map_fname = "%s_depth_map.tif" % os.path.splitext(filename)[0] try: depth_map = np.asarray(PIL.Image.open(depth_map_fname)) except OSError: depth_map = None - return cls(width, height, K, dist, depth_map, latitude, longitude, - altitude, R) + return cls(width, height, K, dist, depth_map, latitude, longitude, altitude, R) def save_to_file(self, filename): - """See base class Camera documentation. - - """ - with open(filename, 'w') as f: - f.write(''.join(['# The type of camera model.\n', - 'model_type: static\n\n', - '# Image dimensions\n'])) - - f.write(''.join(['image_width: ',to_str(self.width),'\n'])) - f.write(''.join(['image_height: ',to_str(self.height),'\n\n'])) - - f.write('# Focal length along the image\'s x-axis.\n') - f.write(''.join(['fx: ',to_str(self._K[0,0]),'\n\n'])) - - f.write('# Focal length along the image\'s y-axis.\n') - f.write(''.join(['fy: ',to_str(self._K[1,1]),'\n\n'])) - - f.write('# Principal point is located at (cx,cy).\n') - f.write(''.join(['cx: ',to_str(self._K[0,2]),'\n'])) - f.write(''.join(['cy: ',to_str(self._K[1,2]),'\n\n'])) - - f.write(''.join(['# Distortion coefficients following OpenCv\'s ', - 'convention\n'])) + """See base class Camera documentation.""" + with open(filename, "w") as f: + f.write( + "".join( + [ + "# The type of camera model.\n", + "model_type: static\n\n", + "# Image dimensions\n", + ] + ) + ) + + f.write("".join(["image_width: ", to_str(self.width), "\n"])) + f.write("".join(["image_height: ", to_str(self.height), "\n\n"])) + + f.write("# Focal length along the image's x-axis.\n") + f.write("".join(["fx: ", to_str(self._K[0, 0]), "\n\n"])) + + f.write("# Focal length along the image's y-axis.\n") + f.write("".join(["fy: ", to_str(self._K[1, 1]), "\n\n"])) + + f.write("# Principal point is located at (cx,cy).\n") + f.write("".join(["cx: ", to_str(self._K[0, 2]), "\n"])) + f.write("".join(["cy: ", to_str(self._K[1, 2]), "\n\n"])) + + f.write( + "".join( + ["# Distortion coefficients following OpenCv's ", "convention\n"] + ) + ) dist = self._dist if np.all(dist == 0): - dist = 'None' - - f.write(''.join(['distortion_coefficients: ', - to_str(self._dist),'\n\n'])) - - f.write(''.join(['# Rotation matrix mapping vectors defined in an ' - 'east/north/up coordinate system\n# centered at ' - 'the camera into vectors defined in the camera' - 'coordinate system.\n', - 'R: [%0.10f, %0.10f, %0.10f,\n' - ' %0.10f, %0.10f, %0.10f,\n' - ' %0.10f, %0.10f, %0.10f]' % - tuple(self.R.ravel()), '\n\n'])) - - f.write(''.join(['# Location of the camera\'s center of ' - 'projection. Latitude and longitude are in\n# ' - 'degrees, and altitude is meters above the WGS84 ' - 'ellipsoid.\n', - 'latitude: %0.10f\n' % self.latitude, - 'longitude: %0.10f\n' % self.longitude, - 'altitude: %0.10f' % self.altitude,'\n\n'])) + dist = "None" + + f.write("".join(["distortion_coefficients: ", to_str(self._dist), "\n\n"])) + + f.write( + "".join( + [ + "# Rotation matrix mapping vectors defined in an " + "east/north/up coordinate system\n# centered at " + "the camera into vectors defined in the camera" + "coordinate system.\n", + "R: [%0.10f, %0.10f, %0.10f,\n" + " %0.10f, %0.10f, %0.10f,\n" + " %0.10f, %0.10f, %0.10f]" % tuple(self.R.ravel()), + "\n\n", + ] + ) + ) + + f.write( + "".join( + [ + "# Location of the camera's center of " + "projection. Latitude and longitude are in\n# " + "degrees, and altitude is meters above the WGS84 " + "ellipsoid.\n", + "latitude: %0.10f\n" % self.latitude, + "longitude: %0.10f\n" % self.longitude, + "altitude: %0.10f" % self.altitude, + "\n\n", + ] + ) + ) if self.depth_map is not None: - im = PIL.Image.fromarray(self.depth_map, mode='F') # float32 - depth_map_fname = '%s_depth_map.tif' % os.path.splitext(filename)[0] + im = PIL.Image.fromarray(self.depth_map, mode="F") # float32 + depth_map_fname = "%s_depth_map.tif" % os.path.splitext(filename)[0] im.save(depth_map_fname) @property @@ -1669,7 +1820,7 @@ def R(self): @R.setter def R(self, value): - self._R /= value/np.linalg.det(value) + self._R /= value / np.linalg.det(value) self._rvec = cv2.Rodrigues(self.R)[0].ravel() @property @@ -1716,8 +1867,9 @@ def project(self, points, t=None): points = np.atleast_2d(points).T # Project rays into camera coordinate system. - im_pts = cv2.projectPoints(points.T, self._rvec, self._tvec, self.K, - self.dist)[0] + im_pts = cv2.projectPoints(points.T, self._rvec, self._tvec, self.K, self.dist)[ + 0 + ] return np.squeeze(im_pts, 1).T def unproject(self, points, t=None, normalize_ray_dir=True): @@ -1734,19 +1886,20 @@ def unproject(self, points, t=None, normalize_ray_dir=True): points = np.array(points, dtype=np.float64) if points.ndim == 1: points = np.atleast_2d(points).T - points = np.reshape(points, (2,-1)) + points = np.reshape(points, (2, -1)) # Unproject rays into the camera coordinate system. - ray_dir = np.ones((3,points.shape[1]), dtype=points.dtype) - ray_dir0 = cv2.undistortPoints(np.expand_dims(points.T, 1), - self.K, self.dist, R=None) + ray_dir = np.ones((3, points.shape[1]), dtype=points.dtype) + ray_dir0 = cv2.undistortPoints( + np.expand_dims(points.T, 1), self.K, self.dist, R=None + ) ray_dir[:2] = np.squeeze(ray_dir0, 1).T # Rotate rays into the local east/north/up coordinate system. ray_dir = np.dot(self.R.T, ray_dir) if normalize_ray_dir: - ray_dir /= np.sqrt(np.sum(ray_dir**2, 0)) + ray_dir /= np.sqrt(np.sum(ray_dir ** 2, 0)) ray_pos = np.zeros_like(ray_dir) @@ -1759,10 +1912,12 @@ class MapCamera(Camera): This object is primarily built around GDAL. """ + def __init__(self, base_layer): - super(MapCamera, self).__init__(width=base_layer.res_x, - height=base_layer.res_y) + super(MapCamera, self).__init__(width=base_layer.res_x, height=base_layer.res_y) self.base_layer = base_layer def project(self, points, t=None): - return np.array([self.base_layer.meters_to_raster(point) for point in points.T]).T + return np.array( + [self.base_layer.meters_to_raster(point) for point in points.T] + ).T diff --git a/kamera/colmap_processing/colmap_interface.py b/kamera/colmap_processing/colmap_interface.py deleted file mode 100644 index 0948a3c..0000000 --- a/kamera/colmap_processing/colmap_interface.py +++ /dev/null @@ -1,605 +0,0 @@ -#!/usr/bin/env python - -# Copyright (c) 2018, ETH Zurich and UNC Chapel Hill. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# -# * Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in the -# documentation and/or other materials provided with the distribution. -# -# * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of -# its contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE -# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -# Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) -import numpy as np -import os -import collections -import struct - -from kamera.colmap_processing.camera_models import StandardCamera -from kamera.colmap_processing.platform_pose import PlatformPoseInterp - - -CameraModel = collections.namedtuple( - "CameraModel", ["model_id", "model_name", "num_params"]) -Camera = collections.namedtuple( - "Camera", ["id", "model", "width", "height", "params"]) -BaseImage = collections.namedtuple( - "Image", ["id", "qvec", "tvec", "camera_id", "name", "xys", "point3D_ids"]) -Point3D = collections.namedtuple( - "Point3D", ["id", "xyz", "rgb", "error", "image_ids", "point2D_idxs"]) - - -class Image(BaseImage): - def qvec2rotmat(self): - return qvec2rotmat(self.qvec) - -model_type_to_int = {"SIMPLE_PINHOLE":0, - "PINHOLE":1, - "SIMPLE_RADIAL":2, - "RADIAL":3, - "OPENCV":4, - "OPENCV_FISHEYE":5, - "FULL_OPENCV":6, - "FOV":7, - "SIMPLE_RADIAL_FISHEYE":8, - "RADIAL_FISHEYE":9, - "THIN_PRISM_FISHEYE":10} - -CAMERA_MODELS = { - CameraModel(model_id=0, model_name="SIMPLE_PINHOLE", num_params=3), - CameraModel(model_id=1, model_name="PINHOLE", num_params=4), - CameraModel(model_id=2, model_name="SIMPLE_RADIAL", num_params=4), - CameraModel(model_id=3, model_name="RADIAL", num_params=5), - CameraModel(model_id=4, model_name="OPENCV", num_params=8), - CameraModel(model_id=5, model_name="OPENCV_FISHEYE", num_params=8), - CameraModel(model_id=6, model_name="FULL_OPENCV", num_params=12), - CameraModel(model_id=7, model_name="FOV", num_params=5), - CameraModel(model_id=8, model_name="SIMPLE_RADIAL_FISHEYE", num_params=4), - CameraModel(model_id=9, model_name="RADIAL_FISHEYE", num_params=5), - CameraModel(model_id=10, model_name="THIN_PRISM_FISHEYE", num_params=12) -} - -CAMERA_MODEL_IDS = dict([(camera_model.model_id, camera_model) \ - for camera_model in CAMERA_MODELS]) -CAMERA_MODEL_NAMES = dict([(camera_model.model_name, camera_model) - for camera_model in CAMERA_MODELS]) -CAMERA_MODEL_NAMES_TO_IND = {"SIMPLE_PINHOLE":0, - "PINHOLE":1, - "SIMPLE_RADIAL":2, - "RADIAL":3, - "OPENCV":4, - "OPENCV_FISHEYE":5, - "FULL_OPENCV":6, - "FOV":7, - "SIMPLE_RADIAL_FISHEYE":8, - "RADIAL_FISHEYE":9, - "THIN_PRISM_FISHEYE":10} - - -def read_next_bytes(fid, num_bytes, format_char_sequence, endian_character="<"): - """Read and unpack the next bytes from a binary file. - :param fid: - :param num_bytes: Sum of combination of {2, 4, 8}, e.g. 2, 6, 16, 30, etc. - :param format_char_sequence: List of {c, e, f, d, h, H, i, I, l, L, q, Q}. - :param endian_character: Any of {@, =, <, >, !} - :return: Tuple of read and unpacked values. - """ - data = fid.read(num_bytes) - return struct.unpack(endian_character + format_char_sequence, data) - - -def write_next_bytes(fid, data, format_char_sequence, endian_character="<"): - """pack and write to a binary file. - :param fid: - :param data: data to send, if multiple elements are sent at the same time, - they should be encapsuled either in a list or a tuple - :param format_char_sequence: List of {c, e, f, d, h, H, i, I, l, L, q, Q}. - should be the same length as the data list or tuple - :param endian_character: Any of {@, =, <, >, !} - """ - if isinstance(data, (list, tuple)): - bytes = struct.pack(endian_character + format_char_sequence, *data) - else: - bytes = struct.pack(endian_character + format_char_sequence, data) - fid.write(bytes) - - -def read_cameras_text(path): - """ - see: src/base/reconstruction.cc - void Reconstruction::WriteCamerasText(const std::string& path) - void Reconstruction::ReadCamerasText(const std::string& path) - """ - cameras = {} - with open(path, "r") as fid: - while True: - line = fid.readline() - if not line: - break - line = line.strip() - if len(line) > 0 and line[0] != "#": - elems = line.split() - camera_id = int(elems[0]) - model = elems[1] - width = int(elems[2]) - height = int(elems[3]) - params = np.array(tuple(map(float, elems[4:]))) - cameras[camera_id] = Camera(id=camera_id, model=model, - width=width, height=height, - params=params) - return cameras - - -def read_cameras_binary(path_to_model_file): - """ - see: src/base/reconstruction.cc - void Reconstruction::WriteCamerasBinary(const std::string& path) - void Reconstruction::ReadCamerasBinary(const std::string& path) - """ - cameras = {} - with open(path_to_model_file, "rb") as fid: - num_cameras = read_next_bytes(fid, 8, "Q")[0] - for camera_line_index in range(num_cameras): - camera_properties = read_next_bytes( - fid, num_bytes=24, format_char_sequence="iiQQ") - camera_id = camera_properties[0] - model_id = camera_properties[1] - model_name = CAMERA_MODEL_IDS[camera_properties[1]].model_name - width = camera_properties[2] - height = camera_properties[3] - num_params = CAMERA_MODEL_IDS[model_id].num_params - params = read_next_bytes(fid, num_bytes=8*num_params, - format_char_sequence="d"*num_params) - cameras[camera_id] = Camera(id=camera_id, - model=model_name, - width=width, - height=height, - params=np.array(params)) - assert len(cameras) == num_cameras - return cameras - - -def write_cameras_text(cameras, path): - """ - see: src/base/reconstruction.cc - void Reconstruction::WriteCamerasText(const std::string& path) - void Reconstruction::ReadCamerasText(const std::string& path) - """ - HEADER = "# Camera list with one line of data per camera:\n" + \ - "# CAMERA_ID, MODEL, WIDTH, HEIGHT, PARAMS[]\n" + \ - "# Number of cameras: {}\n".format(len(cameras)) - with open(path, "w") as fid: - fid.write(HEADER) - for _, cam in cameras.items(): - to_write = [cam.id, cam.model, cam.width, cam.height] + cam.params - line = " ".join([str(elem) for elem in to_write]) - fid.write(line + "\n") - - -def write_cameras_binary(cameras, path_to_model_file): - """ - see: src/base/reconstruction.cc - void Reconstruction::WriteCamerasBinary(const std::string& path) - void Reconstruction::ReadCamerasBinary(const std::string& path) - """ - with open(path_to_model_file, "wb") as fid: - write_next_bytes(fid, len(cameras), "Q") - for _, cam in cameras.items(): - model_id = CAMERA_MODEL_NAMES[cam.model].model_id - camera_properties = [cam.id, - model_id, - cam.width, - cam.height] - write_next_bytes(fid, camera_properties, "iiQQ") - for p in cam.params: - write_next_bytes(fid, float(p), "d") - return cameras - - -def read_images_text(path): - """ - see: src/base/reconstruction.cc - void Reconstruction::ReadImagesText(const std::string& path) - void Reconstruction::WriteImagesText(const std::string& path) - """ - images = {} - with open(path, "r") as fid: - while True: - line = fid.readline() - if not line: - break - line = line.strip() - if len(line) > 0 and line[0] != "#": - elems = line.split() - image_id = int(elems[0]) - qvec = np.array(tuple(map(float, elems[1:5]))) - tvec = np.array(tuple(map(float, elems[5:8]))) - camera_id = int(elems[8]) - image_name = elems[9] - elems = fid.readline().split() - xys = np.column_stack([tuple(map(float, elems[0::3])), - tuple(map(float, elems[1::3]))]) - point3D_ids = np.array(tuple(map(int, elems[2::3]))) - images[image_id] = Image( - id=image_id, qvec=qvec, tvec=tvec, - camera_id=camera_id, name=image_name, - xys=xys, point3D_ids=point3D_ids) - return images - - -def read_images_binary(path_to_model_file): - """ - see: src/base/reconstruction.cc - void Reconstruction::ReadImagesBinary(const std::string& path) - void Reconstruction::WriteImagesBinary(const std::string& path) - """ - images = {} - with open(path_to_model_file, "rb") as fid: - num_reg_images = read_next_bytes(fid, 8, "Q")[0] - for image_index in range(num_reg_images): - binary_image_properties = read_next_bytes( - fid, num_bytes=64, format_char_sequence="idddddddi") - image_id = binary_image_properties[0] - qvec = np.array(binary_image_properties[1:5]) - tvec = np.array(binary_image_properties[5:8]) - camera_id = binary_image_properties[8] - image_name = "" - current_char = read_next_bytes(fid, 1, "c")[0] - while current_char != b"\x00": # look for the ASCII 0 entry - image_name += current_char.decode("utf-8") - current_char = read_next_bytes(fid, 1, "c")[0] - num_points2D = read_next_bytes(fid, num_bytes=8, - format_char_sequence="Q")[0] - x_y_id_s = read_next_bytes(fid, num_bytes=24*num_points2D, - format_char_sequence="ddq"*num_points2D) - xys = np.column_stack([tuple(map(float, x_y_id_s[0::3])), - tuple(map(float, x_y_id_s[1::3]))]) - point3D_ids = np.array(tuple(map(int, x_y_id_s[2::3]))) - images[image_id] = Image( - id=image_id, qvec=qvec, tvec=tvec, - camera_id=camera_id, name=image_name, - xys=xys, point3D_ids=point3D_ids) - return images - - -def write_images_text(images, path): - """ - see: src/base/reconstruction.cc - void Reconstruction::ReadImagesText(const std::string& path) - void Reconstruction::WriteImagesText(const std::string& path) - """ - if len(images) == 0: - mean_observations = 0 - else: - mean_observations = sum((len(img.point3D_ids) for _, img in images.items()))/len(images) - HEADER = "# Image list with two lines of data per image:\n" + \ - "# IMAGE_ID, QW, QX, QY, QZ, TX, TY, TZ, CAMERA_ID, NAME\n" + \ - "# POINTS2D[] as (X, Y, POINT3D_ID)\n" + \ - "# Number of images: {}, mean observations per image: {}\n".format(len(images), mean_observations) - - with open(path, "w") as fid: - fid.write(HEADER) - for _, img in images.items(): - image_header = [img.id] + img.qvec + img.tvec + [img.camera_id, img.name] - first_line = " ".join(map(str, image_header)) - fid.write(first_line + "\n") - - points_strings = [] - for xy, point3D_id in zip(img.xys, img.point3D_ids): - points_strings.append(" ".join(map(str, xy + [point3D_id]))) - fid.write(" ".join(points_strings) + "\n") - - -def write_images_binary(images, path_to_model_file): - """ - see: src/base/reconstruction.cc - void Reconstruction::ReadImagesBinary(const std::string& path) - void Reconstruction::WriteImagesBinary(const std::string& path) - """ - with open(path_to_model_file, "wb") as fid: - write_next_bytes(fid, len(images), "Q") - for _, img in images.items(): - write_next_bytes(fid, img.id, "i") - write_next_bytes(fid, img.qvec.tolist(), "dddd") - write_next_bytes(fid, img.tvec.tolist(), "ddd") - write_next_bytes(fid, img.camera_id, "i") - for char in img.name: - write_next_bytes(fid, char.encode("utf-8"), "c") - write_next_bytes(fid, b"\x00", "c") - write_next_bytes(fid, len(img.point3D_ids), "Q") - for xy, p3d_id in zip(img.xys, img.point3D_ids): - write_next_bytes(fid, xy + [p3d_id], "ddq") - - -def read_points3D_text(path): - """ - see: src/base/reconstruction.cc - void Reconstruction::ReadPoints3DText(const std::string& path) - void Reconstruction::WritePoints3DText(const std::string& path) - """ - points3D = {} - with open(path, "r") as fid: - while True: - line = fid.readline() - if not line: - break - line = line.strip() - if len(line) > 0 and line[0] != "#": - elems = line.split() - point3D_id = int(elems[0]) - xyz = np.array(tuple(map(float, elems[1:4]))) - rgb = np.array(tuple(map(int, elems[4:7]))) - error = float(elems[7]) - image_ids = np.array(tuple(map(int, elems[8::2]))) - point2D_idxs = np.array(tuple(map(int, elems[9::2]))) - points3D[point3D_id] = Point3D(id=point3D_id, xyz=xyz, rgb=rgb, - error=error, image_ids=image_ids, - point2D_idxs=point2D_idxs) - return points3D - - -def read_points3D_binary(path_to_model_file): - """ - see: src/base/reconstruction.cc - void Reconstruction::ReadPoints3DBinary(const std::string& path) - void Reconstruction::WritePoints3DBinary(const std::string& path) - """ - points3D = {} - with open(path_to_model_file, "rb") as fid: - num_points = read_next_bytes(fid, 8, "Q")[0] - for _ in range(num_points): - binary_point_line_properties = read_next_bytes( - fid, num_bytes=43, format_char_sequence="QdddBBBd") - point3D_id = binary_point_line_properties[0] - xyz = np.array(binary_point_line_properties[1:4]) - rgb = np.array(binary_point_line_properties[4:7]) - error = np.array(binary_point_line_properties[7]) - track_length = read_next_bytes( - fid, num_bytes=8, format_char_sequence="Q")[0] - track_elems = read_next_bytes( - fid, num_bytes=8*track_length, - format_char_sequence="ii"*track_length) - image_ids = np.array(tuple(map(int, track_elems[0::2]))) - point2D_idxs = np.array(tuple(map(int, track_elems[1::2]))) - points3D[point3D_id] = Point3D( - id=point3D_id, xyz=xyz, rgb=rgb, - error=error, image_ids=image_ids, - point2D_idxs=point2D_idxs) - return points3D - - -def write_points3D_text(points3D, path): - """ - see: src/base/reconstruction.cc - void Reconstruction::ReadPoints3DText(const std::string& path) - void Reconstruction::WritePoints3DText(const std::string& path) - """ - if len(points3D) == 0: - mean_track_length = 0 - else: - mean_track_length = sum((len(pt.image_ids) for _, pt in points3D.items()))/len(points3D) - HEADER = "# 3D point list with one line of data per point:\n" + \ - "# POINT3D_ID, X, Y, Z, R, G, B, ERROR, TRACK[] as (IMAGE_ID, POINT2D_IDX)\n" + \ - "# Number of points: {}, mean track length: {}\n".format(len(points3D), mean_track_length) - - with open(path, "w") as fid: - fid.write(HEADER) - for _, pt in points3D.items(): - point_header = [pt.id] + pt.xyz + [pt.rgb, pt.error] - fid.write(" ".join(map(str, point_header)) + " ") - track_strings = [] - for image_id, point2D in zip(pt.image_ids, pt.point2D_idxs): - track_strings.append(" ".join(map(str, [image_id, point2D]))) - fid.write(" ".join(track_strings) + "\n") - - -def write_points3D_binary(points3D, path_to_model_file): - """ - see: src/base/reconstruction.cc - void Reconstruction::ReadPoints3DBinary(const std::string& path) - void Reconstruction::WritePoints3DBinary(const std::string& path) - """ - with open(path_to_model_file, "wb") as fid: - write_next_bytes(fid, len(points3D), "Q") - for _, pt in points3D.items(): - write_next_bytes(fid, pt.id, "Q") - write_next_bytes(fid, pt.xyz.tolist(), "ddd") - write_next_bytes(fid, pt.rgb.tolist(), "BBB") - write_next_bytes(fid, pt.error, "d") - track_length = pt.image_ids.shape[0] - write_next_bytes(fid, track_length, "Q") - for image_id, point2D_id in zip(pt.image_ids, pt.point2D_idxs): - write_next_bytes(fid, [image_id, point2D_id], "ii") - - -def detect_model_format(path, ext): - if os.path.isfile(os.path.join(path, "cameras" + ext)) and \ - os.path.isfile(os.path.join(path, "images" + ext)) and \ - os.path.isfile(os.path.join(path, "points3D" + ext)): - print("Detected model format: '" + ext + "'") - return True - - return False - - -def read_model(path, ext=""): - # try to detect the extension automatically - if ext == "": - if detect_model_format(path, ".bin"): - ext = ".bin" - elif detect_model_format(path, ".txt"): - ext = ".txt" - else: - print("Provide model format: '.bin' or '.txt'") - return - - if ext == ".txt": - cameras = read_cameras_text(os.path.join(path, "cameras" + ext)) - images = read_images_text(os.path.join(path, "images" + ext)) - points3D = read_points3D_text(os.path.join(path, "points3D") + ext) - else: - cameras = read_cameras_binary(os.path.join(path, "cameras" + ext)) - images = read_images_binary(os.path.join(path, "images" + ext)) - points3D = read_points3D_binary(os.path.join(path, "points3D") + ext) - return cameras, images, points3D - - -def write_model(cameras, images, points3D, path, ext=".bin"): - if ext == ".txt": - write_cameras_text(cameras, os.path.join(path, "cameras" + ext)) - write_images_text(images, os.path.join(path, "images" + ext)) - write_points3D_text(points3D, os.path.join(path, "points3D") + ext) - else: - write_cameras_binary(cameras, os.path.join(path, "cameras" + ext)) - write_images_binary(images, os.path.join(path, "images" + ext)) - write_points3D_binary(points3D, os.path.join(path, "points3D") + ext) - return cameras, images, points3D - - -def write_points3d_binary(points3D, path_to_model_file): - """ - see: src/base/reconstruction.cc - void Reconstruction::ReadPoints3DBinary(const std::string& path) - void Reconstruction::WritePoints3DBinary(const std::string& path) - """ - with open(path_to_model_file, "wb") as fid: - write_next_bytes(fid, len(points3D), "Q") - for _, pt in points3D.items(): - write_next_bytes(fid, pt.id, "Q") - write_next_bytes(fid, pt.xyz.tolist(), "ddd") - write_next_bytes(fid, pt.rgb.tolist(), "BBB") - write_next_bytes(fid, pt.error, "d") - track_length = pt.image_ids.shape[0] - write_next_bytes(fid, track_length, "Q") - for image_id, point2D_id in zip(pt.image_ids, pt.point2D_idxs): - write_next_bytes(fid, [image_id, point2D_id], "ii") - - -def qvec2rotmat(qvec): - return np.array([ - [1 - 2 * qvec[2]**2 - 2 * qvec[3]**2, - 2 * qvec[1] * qvec[2] - 2 * qvec[0] * qvec[3], - 2 * qvec[3] * qvec[1] + 2 * qvec[0] * qvec[2]], - [2 * qvec[1] * qvec[2] + 2 * qvec[0] * qvec[3], - 1 - 2 * qvec[1]**2 - 2 * qvec[3]**2, - 2 * qvec[2] * qvec[3] - 2 * qvec[0] * qvec[1]], - [2 * qvec[3] * qvec[1] - 2 * qvec[0] * qvec[2], - 2 * qvec[2] * qvec[3] + 2 * qvec[0] * qvec[1], - 1 - 2 * qvec[1]**2 - 2 * qvec[2]**2]]) - - -def rotmat2qvec(R): - Rxx, Ryx, Rzx, Rxy, Ryy, Rzy, Rxz, Ryz, Rzz = R.flat - K = np.array([ - [Rxx - Ryy - Rzz, 0, 0, 0], - [Ryx + Rxy, Ryy - Rxx - Rzz, 0, 0], - [Rzx + Rxz, Rzy + Ryz, Rzz - Rxx - Ryy, 0], - [Ryz - Rzy, Rzx - Rxz, Rxy - Ryx, Rxx + Ryy + Rzz]]) / 3.0 - eigvals, eigvecs = np.linalg.eigh(K) - qvec = eigvecs[[3, 0, 1, 2], np.argmax(eigvals)] - if qvec[0] < 0: - qvec *= -1 - return qvec - - -def standard_cameras_from_colmap(cameras, images=None, image_times=None): - """Parse Colmap 'cameras' and 'images' into instances of StandardCamera. - - :param cameras: Output from read_cameras_text or read_cameras_binary. - :type cameras: dict - - :param images: Output from read_images_text or read_images_binary. - :type images: dict | None - - :param image_times: If None, image times (seconds) are set to the image - index in 'images'. If a dictionary, it should accept image index and - return image time in seconds. - :type image_times: None | dict - - :return: Dictionary indexable by camera_id returning StandardCamera. If - arguement 'images' is not None, an instance of PlatformPoseProvider is - returned where its time input arguement should be the image index. - :rtype: (dict, PlatformPoseProvider | None) - - """ - if images is not None: - # Pretend image index is the time. - platform_pose_provider = PlatformPoseInterp() - for image_id in images: - image = images[image_id] - - R = qvec2rotmat(image.qvec) - pos = -np.dot(R.T, image.tvec) - - # The qvec used by Colmap is a (w, x, y, z) quaternion - # representing the rotation of a vector defined in the world - # coordinate system into the camera coordinate system. However, - # the 'camera_models' module assumes (x, y, z, w) quaternions - # representing a coordinate system rotation. Also, the quaternion - # used by 'camera_models' represents a coordinate system rotation - # versus the coordinate system transform of Colmap's convention, - # so we need an inverse. - - #quat = transformations.quaternion_inverse(image.qvec) - quat = image.qvec / np.linalg.norm(image.qvec) - quat[0] = -quat[0] - - quat = [quat[1], quat[2], quat[3], quat[0]] - - if image_times is None: - t = image_id - else: - t = image_times[image_id] - - platform_pose_provider.add_to_pose_time_series(t, pos, quat) - else: - platform_pose_provider = None - - std_cams = {} - for camera_id in set([images[image_id].camera_id for image_id in images]): - colmap_camera = cameras[camera_id] - - if colmap_camera.model == 'OPENCV': - fx, fy, cx, cy, d1, d2, d3, d4 = colmap_camera.params - elif colmap_camera.model == 'PINHOLE': - fx, fy, cx, cy = colmap_camera.params - d1 = d2 = d3 = d4 = 0 - elif colmap_camera.model == 'SIMPLE_RADIAL': - fx, cx, cy, d1 = colmap_camera.params - fy = fx - d2 = d3 = d4 = 0 - elif colmap_camera.model == 'RADIAL': - fx, cx, cy, d1, d2 = colmap_camera.params - fy = fx - d3 = d4 = 0 - else: - raise NotImplementedError() - - K = K = np.array([[fx, 0, cx], [0, fy, cy], [0, 0, 1]]) - dist = np.array([d1, d2, d3, d4]) - std_cams[camera_id] = StandardCamera(colmap_camera.width, - colmap_camera.height, K, dist, - [0, 0, 0], [0, 0, 0, 1], - platform_pose_provider) - - return std_cams diff --git a/kamera/colmap_processing/database.py b/kamera/colmap_processing/database.py deleted file mode 100644 index b67e4a4..0000000 --- a/kamera/colmap_processing/database.py +++ /dev/null @@ -1,418 +0,0 @@ -# Copyright (c) 2018, ETH Zurich and UNC Chapel Hill. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# -# * Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in the -# documentation and/or other materials provided with the distribution. -# -# * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of -# its contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE -# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -# Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) - -# This script is based on an original implementation by True Price. - -import sys -import sqlite3 -import numpy as np - - -IS_PYTHON3 = sys.version_info[0] >= 3 - -MAX_IMAGE_ID = 2**31 - 1 - -CREATE_CAMERAS_TABLE = """CREATE TABLE IF NOT EXISTS cameras ( - camera_id INTEGER PRIMARY KEY AUTOINCREMENT NOT NULL, - model INTEGER NOT NULL, - width INTEGER NOT NULL, - height INTEGER NOT NULL, - params BLOB, - prior_focal_length INTEGER NOT NULL)""" - -CREATE_DESCRIPTORS_TABLE = """CREATE TABLE IF NOT EXISTS descriptors ( - image_id INTEGER PRIMARY KEY NOT NULL, - rows INTEGER NOT NULL, - cols INTEGER NOT NULL, - data BLOB, - FOREIGN KEY(image_id) REFERENCES images(image_id) ON DELETE CASCADE)""" - -CREATE_IMAGES_TABLE = """CREATE TABLE IF NOT EXISTS images ( - image_id INTEGER PRIMARY KEY AUTOINCREMENT NOT NULL, - name TEXT NOT NULL UNIQUE, - camera_id INTEGER NOT NULL, - prior_qw REAL, - prior_qx REAL, - prior_qy REAL, - prior_qz REAL, - prior_tx REAL, - prior_ty REAL, - prior_tz REAL, - CONSTRAINT image_id_check CHECK(image_id >= 0 and image_id < {}), - FOREIGN KEY(camera_id) REFERENCES cameras(camera_id)) -""".format(MAX_IMAGE_ID) - -CREATE_TWO_VIEW_GEOMETRIES_TABLE = """ -CREATE TABLE IF NOT EXISTS two_view_geometries ( - pair_id INTEGER PRIMARY KEY NOT NULL, - rows INTEGER NOT NULL, - cols INTEGER NOT NULL, - data BLOB, - config INTEGER NOT NULL, - F BLOB, - E BLOB, - H BLOB) -""" - -CREATE_KEYPOINTS_TABLE = """CREATE TABLE IF NOT EXISTS keypoints ( - image_id INTEGER PRIMARY KEY NOT NULL, - rows INTEGER NOT NULL, - cols INTEGER NOT NULL, - data BLOB, - FOREIGN KEY(image_id) REFERENCES images(image_id) ON DELETE CASCADE) -""" - -CREATE_MATCHES_TABLE = """CREATE TABLE IF NOT EXISTS matches ( - pair_id INTEGER PRIMARY KEY NOT NULL, - rows INTEGER NOT NULL, - cols INTEGER NOT NULL, - data BLOB)""" - -CREATE_NAME_INDEX = \ - "CREATE UNIQUE INDEX IF NOT EXISTS index_name ON images(name)" - -CREATE_ALL = "; ".join([ - CREATE_CAMERAS_TABLE, - CREATE_IMAGES_TABLE, - CREATE_KEYPOINTS_TABLE, - CREATE_DESCRIPTORS_TABLE, - CREATE_MATCHES_TABLE, - CREATE_TWO_VIEW_GEOMETRIES_TABLE, - CREATE_NAME_INDEX -]) - - -def image_ids_to_pair_id(image_id1, image_id2): - if image_id1 > image_id2: - image_id1, image_id2 = image_id2, image_id1 - return int(image_id1 * MAX_IMAGE_ID + image_id2) - - -def pair_id_to_image_ids(pair_id): - image_id2 = pair_id % MAX_IMAGE_ID - image_id1 = (pair_id - image_id2) / MAX_IMAGE_ID - return int(image_id1), int(image_id2) - - -def array_to_blob(array): - if IS_PYTHON3: - return array.tostring() - else: - return np.getbuffer(array) - - -def blob_to_array(blob, dtype, shape=(-1,)): - if blob is None: - return None - - if IS_PYTHON3: - return np.fromstring(blob, dtype=dtype).reshape(*shape) - else: - return np.frombuffer(blob, dtype=dtype).reshape(*shape) - - -class COLMAPDatabase(sqlite3.Connection): - @staticmethod - def connect(database_path): - return sqlite3.connect(database_path, factory=COLMAPDatabase) - - - def __init__(self, *args, **kwargs): - super(COLMAPDatabase, self).__init__(*args, **kwargs) - - self.create_tables = lambda: self.executescript(CREATE_ALL) - self.create_cameras_table = \ - lambda: self.executescript(CREATE_CAMERAS_TABLE) - self.create_descriptors_table = \ - lambda: self.executescript(CREATE_DESCRIPTORS_TABLE) - self.create_images_table = \ - lambda: self.executescript(CREATE_IMAGES_TABLE) - self.create_two_view_geometries_table = \ - lambda: self.executescript(CREATE_TWO_VIEW_GEOMETRIES_TABLE) - self.create_keypoints_table = \ - lambda: self.executescript(CREATE_KEYPOINTS_TABLE) - self.create_matches_table = \ - lambda: self.executescript(CREATE_MATCHES_TABLE) - self.create_name_index = lambda: self.executescript(CREATE_NAME_INDEX) - - def add_camera(self, model, width, height, params, - prior_focal_length=False, camera_id=None): - params = np.asarray(params, np.float64) - cursor = self.execute( - "INSERT INTO cameras VALUES (?, ?, ?, ?, ?, ?)", - (camera_id, model, width, height, array_to_blob(params), - prior_focal_length)) - return cursor.lastrowid - - def add_image(self, name, camera_id, - prior_q=np.zeros(4), prior_t=np.zeros(3), image_id=None): - cursor = self.execute( - "INSERT INTO images VALUES (?, ?, ?, ?, ?, ?, ?, ?, ?, ?)", - (image_id, name, camera_id, prior_q[0], prior_q[1], prior_q[2], - prior_q[3], prior_t[0], prior_t[1], prior_t[2])) - return cursor.lastrowid - - def add_keypoints(self, image_id, keypoints): - assert(len(keypoints.shape) == 2) - assert(keypoints.shape[1] in [2, 4, 6]) - - keypoints = np.asarray(keypoints, np.float32) - self.execute( - "INSERT INTO keypoints VALUES (?, ?, ?, ?)", - (image_id,) + keypoints.shape + (array_to_blob(keypoints),)) - - def add_descriptors(self, image_id, descriptors): - descriptors = np.ascontiguousarray(descriptors, np.uint8) - self.execute( - "INSERT INTO descriptors VALUES (?, ?, ?, ?)", - (image_id,) + descriptors.shape + (array_to_blob(descriptors),)) - - def add_matches(self, image_id1, image_id2, matches): - assert(len(matches.shape) == 2) - assert(matches.shape[1] == 2) - - if image_id1 > image_id2: - matches = matches[:,::-1] - - pair_id = image_ids_to_pair_id(image_id1, image_id2) - matches = np.asarray(matches, np.uint32) - self.execute( - "INSERT INTO matches VALUES (?, ?, ?, ?)", - (pair_id,) + matches.shape + (array_to_blob(matches),)) - - def add_two_view_geometry(self, image_id1, image_id2, matches, - F=np.eye(3), E=np.eye(3), H=np.eye(3), config=2): - assert(len(matches.shape) == 2) - assert(matches.shape[1] == 2) - - if image_id1 > image_id2: - matches = matches[:,::-1] - - pair_id = image_ids_to_pair_id(image_id1, image_id2) - matches = np.asarray(matches, np.uint32) - F = np.asarray(F, dtype=np.float64) - E = np.asarray(E, dtype=np.float64) - H = np.asarray(H, dtype=np.float64) - self.execute( - "INSERT INTO two_view_geometries VALUES (?, ?, ?, ?, ?, ?, ?, ?)", - (pair_id,) + matches.shape + (array_to_blob(matches), config, - array_to_blob(F), array_to_blob(E), array_to_blob(H))) - - def get_all_two_view_geometry(self, min_matches=0): - cursor = self.cursor() - cursor.execute("SELECT pair_id, data, config, F, E, H FROM two_view_geometries") - inlier_matches = [] - pair_ids = [] - image_ids = [] - config = [] - F = [] - H = [] - E = [] - for row in cursor: - pair_id = row[0] - if row[1] is not None: - inlier_matches_ = np.fromstring(row[1], - dtype=np.uint32).reshape(-1, 2) - if len(inlier_matches_) > min_matches: - pair_ids.append(pair_id) - image_ids.append(pair_id_to_image_ids(pair_id)) - inlier_matches.append(inlier_matches_) - - config.append(row[2]) - F.append(blob_to_array(row[3], dtype=np.float64, shape=(3,3))) - E.append(blob_to_array(row[4], dtype=np.float64, shape=(3,3))) - H.append(blob_to_array(row[5], dtype=np.float64, shape=(3,3))) - - return pair_ids, image_ids, inlier_matches, F, E, H, config - - def get_all_pair_id(self): - cursor = self.cursor() - cursor.execute("SELECT pair_id, data FROM matches") - all_pairs = [pair_id for pair_id, _ in cursor] - return all_pairs - - def get_match_dictionary(self): - """Return dictionary of image pairs as keys and matches as values. - - """ - cursor = self.cursor() - cursor.execute("SELECT pair_id, data FROM matches") - matches = dict((pair_id_to_image_ids(pair_id), - blob_to_array(data, np.uint32, (-1, 2))) - for pair_id, data in cursor if data is not None) - return matches - - def get_keypoint_from_image_dict(self): - cursor = self.cursor() - cursor.execute("SELECT image_id, rows, cols, data FROM keypoints") - keypoints = dict((image_id, blob_to_array(data, np.float32, (row, col))) - for image_id, row, col, data in cursor) - return keypoints - - def get_descriptors_from_image_dict(self): - cursor = self.cursor() - cursor.execute("SELECT image_id, data FROM descriptors") - descriptors = dict((image_id, blob_to_array(data, np.uint8, (-1, 128))) - for image_id, data in cursor) - return descriptors - - def delete_pair(self, pair_id): - """Remove a two-view geometry pair. - - """ - self.execute("DELETE FROM matches WHERE pair_id=?", (pair_id,)) - - -def example_usage(): - import os - import argparse - - parser = argparse.ArgumentParser() - parser.add_argument("--database_path", default="database.db") - args = parser.parse_args() - - if os.path.exists(args.database_path): - print("ERROR: database path already exists -- will not modify it.") - return - - # Open the database. - - db = COLMAPDatabase.connect(args.database_path) - - # For convenience, try creating all the tables upfront. - - db.create_tables() - - # Create dummy cameras. - - model1, width1, height1, params1 = \ - 0, 1024, 768, np.array((1024., 512., 384.)) - model2, width2, height2, params2 = \ - 2, 1024, 768, np.array((1024., 512., 384., 0.1)) - - camera_id1 = db.add_camera(model1, width1, height1, params1) - camera_id2 = db.add_camera(model2, width2, height2, params2) - - # Create dummy images. - - image_id1 = db.add_image("image1.png", camera_id1) - image_id2 = db.add_image("image2.png", camera_id1) - image_id3 = db.add_image("image3.png", camera_id2) - image_id4 = db.add_image("image4.png", camera_id2) - - # Create dummy keypoints. - # - # Note that COLMAP supports: - # - 2D keypoints: (x, y) - # - 4D keypoints: (x, y, theta, scale) - # - 6D affine keypoints: (x, y, a_11, a_12, a_21, a_22) - - num_keypoints = 1000 - keypoints1 = np.random.rand(num_keypoints, 2) * (width1, height1) - keypoints2 = np.random.rand(num_keypoints, 2) * (width1, height1) - keypoints3 = np.random.rand(num_keypoints, 2) * (width2, height2) - keypoints4 = np.random.rand(num_keypoints, 2) * (width2, height2) - - db.add_keypoints(image_id1, keypoints1) - db.add_keypoints(image_id2, keypoints2) - db.add_keypoints(image_id3, keypoints3) - db.add_keypoints(image_id4, keypoints4) - - # Create dummy matches. - - M = 50 - matches12 = np.random.randint(num_keypoints, size=(M, 2)) - matches23 = np.random.randint(num_keypoints, size=(M, 2)) - matches34 = np.random.randint(num_keypoints, size=(M, 2)) - - db.add_matches(image_id1, image_id2, matches12) - db.add_matches(image_id2, image_id3, matches23) - db.add_matches(image_id3, image_id4, matches34) - - # Commit the data to the file. - - db.commit() - - # Read and check cameras. - - rows = db.execute("SELECT * FROM cameras") - - camera_id, model, width, height, params, prior = next(rows) - params = blob_to_array(params, np.float64) - assert camera_id == camera_id1 - assert model == model1 and width == width1 and height == height1 - assert np.allclose(params, params1) - - camera_id, model, width, height, params, prior = next(rows) - params = blob_to_array(params, np.float64) - assert camera_id == camera_id2 - assert model == model2 and width == width2 and height == height2 - assert np.allclose(params, params2) - - # Read and check keypoints. - - keypoints = dict( - (image_id, blob_to_array(data, np.float32, (-1, 2))) - for image_id, data in db.execute( - "SELECT image_id, data FROM keypoints")) - - assert np.allclose(keypoints[image_id1], keypoints1) - assert np.allclose(keypoints[image_id2], keypoints2) - assert np.allclose(keypoints[image_id3], keypoints3) - assert np.allclose(keypoints[image_id4], keypoints4) - - # Read and check matches. - - pair_ids = [image_ids_to_pair_id(*pair) for pair in - ((image_id1, image_id2), - (image_id2, image_id3), - (image_id3, image_id4))] - - matches = dict( - (pair_id_to_image_ids(pair_id), - blob_to_array(data, np.uint32, (-1, 2))) - for pair_id, data in db.execute("SELECT pair_id, data FROM matches") - ) - - assert np.all(matches[(image_id1, image_id2)] == matches12) - assert np.all(matches[(image_id2, image_id3)] == matches23) - assert np.all(matches[(image_id3, image_id4)] == matches34) - - # Clean up. - - db.close() - - if os.path.exists(args.database_path): - os.remove(args.database_path) - - -if __name__ == "__main__": - example_usage() diff --git a/kamera/colmap_processing/docker/README.md b/kamera/colmap_processing/docker/README.md deleted file mode 100644 index f306ac2..0000000 --- a/kamera/colmap_processing/docker/README.md +++ /dev/null @@ -1,115 +0,0 @@ -## Mapper Parameters - -**min_num_matches** (default=15) : Only load image pairs with a minimum number of matches. - -**ignore_watermarks** (default=0) : Whether to ignore watermark image pairs. - -**multiple_models** (default=1) : Whether to allow multiple models to be generated. - -**max_num_models** (default=50) : Maximum numbers of models to be generate. - -**max_model_overlap** (default=20) : The maximum number of overlapping images between sub-models. If the current sub-models shares more than this number of images with another model, then the reconstruction is stopped. - -**min_model_size** (default=10) : The minimum number of registered images of a sub-model, otherwise the sub-model is discarded. - -**init_image_id1** (default=-1) : The image identifiers used to initialize the reconstruction. Note that only one or both image identifiers can be specified. In the former case, the second image is automatically determined. - -**init_image_id2** (default=-1) : See **init_image_id2**. - -**init_num_trials** (default=200) : The number of trials to initialize the reconstruction. - -**extract_colors** (default=1) : Whether to extract colors for reconstructed points. - -**num_threads** (default=-1) : The number of threads to use during reconstruction. - -**min_focal_length_ratio** (default=0.10000000000000001) : Thresholds for filtering images with degenerate intrinsics. - -**max_focal_length_ratio** (default=10) : Thresholds for filtering images with degenerate intrinsics. - -**max_extra_param** (default=1) - -**ba_refine_focal_length** (default=1) : Refine focal lengths during bundle adjustment. - -**ba_refine_principal_point** (default=0) : Refine principal point during bundle adjustment. - -**ba_refine_extra_params** (default=1) : Refine extra parameters during bundle adjustment. - -**ba_min_num_residuals_for_multi_threading** (default=50000) : The minimum number of residuals per bundle adjustment problem to enable multi-threading solving of the problems. - -**ba_local_num_images** (default=6) : The number of images to optimize in local bundle adjustment. - -**ba_local_max_num_iterations** (default=25) : The maximum number of local bundle adjustment iterations. - -**ba_global_use_pba** (default=0) : Whether to use PBA in global bundle adjustment. - -**ba_global_pba_gpu_index** (default=-1) : The GPU index for PBA bundle adjustment. - -**ba_global_images_ratio** (default=1.1000000000000001) : The growth rates after which to perform global bundle adjustment. - -**ba_global_points_ratio** (default=1.1000000000000001) : The growth rates after which to perform global bundle adjustment. - -**ba_global_images_freq** (default=500) : The growth rates after which to perform global bundle adjustment. - -**ba_global_points_freq** (default=250000) : The growth rates after which to perform global bundle adjustment. - -**ba_global_max_num_iterations** (default=50) : The maximum number of global bundle adjustment iterations. - -**ba_global_max_refinements** (default=5) : The thresholds for iterative bundle adjustment refinements. - -**ba_global_max_refinement_change** (default=0.00050000000000000001) : The thresholds for iterative bundle adjustment refinements. - -**ba_local_max_refinements** (default=2) : The thresholds for iterative bundle adjustment refinements. - -**ba_local_max_refinement_change** (default=0.001) : The thresholds for iterative bundle adjustment refinements. - -**snapshot_path** : Path to a folder with reconstruction snapshots during incremental reconstruction. Snapshots will be saved according to the specified frequency of registered images. - -**snapshot_images_freq** (default=0) : See **snapshot_path**. - -**fix_existing_images** (default=0) : If reconstruction is provided as input, fix the existing image poses. - -**init_min_num_inliers** (default=100) : Minimum number of inliers for initial image pair. - -**init_max_error** (default=4) : Maximum error in pixels for two-view geometry estimation for initial image pair. - -**init_max_forward_motion** (default=0.94999999999999996) : Maximum forward motion for initial image pair. - -**init_min_tri_angle** (default=16) : Minimum triangulation angle (degrees) for initial image pair. - -**init_max_reg_trials** (default=2) : Maximum number of trials to use an image for initialization. - -**abs_pose_max_error** (default=12) : Maximum reprojection error in absolute pose estimation. - -**abs_pose_min_num_inliers** (default=30) : Minimum number of inliers in absolute pose estimation. - -**abs_pose_min_inlier_ratio** (default=0.25) : Minimum inlier ratio in absolute pose estimation. - -**filter_max_reproj_error** (default=4) : Maximum reprojection error in pixels for observations. - -**filter_min_tri_angle** (default=1.5) : Minimum triangulation angle in degrees for stable 3D points. - -**max_reg_trials** (default=3) : Maximum number of trials to register an image. - -Triangulation includes creation of new points, continuation of existing points, and merging of separate points if given image bridges tracks. Note that the given image must be registered and its pose must be set in the associated reconstruction. - -**tri_max_transitivity** (default=1) : Maximum transitivity to search for correspondences. - -**tri_create_max_angle_error** (default=2) : Maximum angular error (degrees) to create new triangulations. - -**tri_continue_max_angle_error** (default=2) : Maximum angular error (degrees) to continue existing triangulations. - -**tri_merge_max_reproj_error** (default=4) : Maximum reprojection error in pixels to merge triangulations. - -**tri_complete_max_reproj_error** (default=4) : Maximum reprojection error to complete an existing triangulation. - -**tri_complete_max_transitivity** (default=5) : Maximum transitivity for track completion. - -**tri_re_max_angle_error** (default=5) : Maximum angular error (degrees) to re-triangulate under-reconstructed image pairs. - -**tri_re_min_ratio** (default=0.20000000000000001) : Minimum ratio of common triangulations between an image pair over the number of correspondences between that image pair to be considered as under-reconstructed. - -**tri_re_max_trials** (default=1) : Maximum number of trials to re-triangulate an image pair. - -**tri_min_angle** (default=1.5) : Minimum pairwise triangulation angle for a stable triangulation. - -**tri_ignore_two_view_tracks** (default=1) : Whether to ignore two-view tracks. \ No newline at end of file diff --git a/kamera/colmap_processing/docker/batch/exhaustive_matcher.sh b/kamera/colmap_processing/docker/batch/exhaustive_matcher.sh deleted file mode 100755 index 469adec..0000000 --- a/kamera/colmap_processing/docker/batch/exhaustive_matcher.sh +++ /dev/null @@ -1,27 +0,0 @@ -# Pass the path to the Colmap data folder. - -docker run -it \ - --gpus all \ - --network="host" \ - -e "DISPLAY" \ - -v "/tmp/.X11-unix:/tmp/.X11-unix" \ - -w /working \ - -v $1:/working \ - colmap/colmap:latest \ - colmap exhaustive_matcher \ - --database_path database.db \ - --SiftMatching.num_threads=-1 \ - --SiftMatching.use_gpu=1 \ - --SiftMatching.gpu_index=-1 \ - --SiftMatching.max_ratio=0.80000000000000004 \ - --SiftMatching.max_distance=0.69999999999999996 \ - --SiftMatching.cross_check=1 \ - --SiftMatching.max_error=4 \ - --SiftMatching.max_num_matches=32768 \ - --SiftMatching.confidence=0.999 \ - --SiftMatching.max_num_trials=10000 \ - --SiftMatching.min_inlier_ratio=0.25 \ - --SiftMatching.min_num_inliers=15 \ - --SiftMatching.multiple_models=0 \ - --SiftMatching.guided_matching=0 \ - --ExhaustiveMatching.block_size=50 diff --git a/kamera/colmap_processing/docker/batch/feature_extractor.sh b/kamera/colmap_processing/docker/batch/feature_extractor.sh deleted file mode 100755 index 940f11d..0000000 --- a/kamera/colmap_processing/docker/batch/feature_extractor.sh +++ /dev/null @@ -1,35 +0,0 @@ -# Pass the path to the Colmap data folder. - -docker run -it \ - --gpus all \ - --network="host" \ - -e "DISPLAY" \ - -v "/tmp/.X11-unix:/tmp/.X11-unix" \ - -w /working \ - -v $1:/working \ - colmap/colmap:latest \ - colmap feature_extractor \ - --database_path database.db \ - --image_path images0 \ - --ImageReader.camera_model=OPENCV \ - --ImageReader.single_camera=0 \ - --ImageReader.single_camera_per_folder=1 \ - --ImageReader.existing_camera_id=-1 \ - --ImageReader.default_focal_length_factor=1.2 \ - --SiftExtraction.num_threads=-1 \ - --SiftExtraction.use_gpu=1 \ - --SiftExtraction.gpu_index=-1 \ - --SiftExtraction.max_image_size=3200 \ - --SiftExtraction.max_num_features=8192 \ - --SiftExtraction.first_octave=-1 \ - --SiftExtraction.num_octaves=11 \ - --SiftExtraction.octave_resolution=3 \ - --SiftExtraction.peak_threshold=0.0066666666666666671 \ - --SiftExtraction.edge_threshold=10 \ - --SiftExtraction.estimate_affine_shape=0 \ - --SiftExtraction.max_num_orientations=2 \ - --SiftExtraction.upright=0 \ - --SiftExtraction.domain_size_pooling=0 \ - --SiftExtraction.dsp_min_scale=0.16666666666666666 \ - --SiftExtraction.dsp_max_scale=3 \ - --SiftExtraction.dsp_num_scales=10 diff --git a/kamera/colmap_processing/docker/batch/mapper.sh b/kamera/colmap_processing/docker/batch/mapper.sh deleted file mode 100755 index 5a49502..0000000 --- a/kamera/colmap_processing/docker/batch/mapper.sh +++ /dev/null @@ -1,70 +0,0 @@ -# Pass the path to the Colmap data folder. - -docker run -it \ - --gpus all \ - --network="host" \ - -e "DISPLAY" \ - -v "/tmp/.X11-unix:/tmp/.X11-unix" \ - -w /working \ - -v $1:/working \ - colmap/colmap:latest \ - colmap mapper \ - --database_path database.db \ - --image_path images0 \ - --output_path sparse \ - --Mapper.min_num_matches=15 \ - --Mapper.ignore_watermarks=0 \ - --Mapper.multiple_models=1 \ - --Mapper.max_num_models=50 \ - --Mapper.max_model_overlap=20 \ - --Mapper.min_model_size=10 \ - --Mapper.init_image_id1=-1 \ - --Mapper.init_image_id2=-1 \ - --Mapper.init_num_trials=200 \ - --Mapper.extract_colors=1 \ - --Mapper.num_threads=-1 \ - --Mapper.min_focal_length_ratio=0.10000000000000001 \ - --Mapper.max_focal_length_ratio=10 \ - --Mapper.max_extra_param=1 \ - --Mapper.ba_refine_focal_length=1 \ - --Mapper.ba_refine_principal_point=0 \ - --Mapper.ba_refine_extra_params=1 \ - --Mapper.ba_min_num_residuals_for_multi_threading=50000 \ - --Mapper.ba_local_num_images=6 \ - --Mapper.ba_local_max_num_iterations=25 \ - --Mapper.ba_global_use_pba=1 \ - --Mapper.ba_global_pba_gpu_index=-1 \ - --Mapper.ba_global_images_ratio=1.1000000000000001 \ - --Mapper.ba_global_points_ratio=1.1000000000000001 \ - --Mapper.ba_global_images_freq=500 \ - --Mapper.ba_global_points_freq=250000 \ - --Mapper.ba_global_max_num_iterations=50 \ - --Mapper.ba_global_max_refinements=5 \ - --Mapper.ba_global_max_refinement_change=0.00050000000000000001 \ - --Mapper.ba_local_max_refinements=2 \ - --Mapper.ba_local_max_refinement_change=0.001 \ - --Mapper.snapshot_images_freq=100 \ - --Mapper.fix_existing_images=0 \ - --Mapper.init_min_num_inliers=100 \ - --Mapper.init_max_error=4 \ - --Mapper.init_max_forward_motion=0.94999999999999996 \ - --Mapper.init_min_tri_angle=16 \ - --Mapper.init_max_reg_trials=2 \ - --Mapper.abs_pose_max_error=12 \ - --Mapper.abs_pose_min_num_inliers=30 \ - --Mapper.abs_pose_min_inlier_ratio=0.25 \ - --Mapper.filter_max_reproj_error=4 \ - --Mapper.filter_min_tri_angle=1.5 \ - --Mapper.max_reg_trials=3 \ - --Mapper.tri_max_transitivity=1 \ - --Mapper.tri_create_max_angle_error=2 \ - --Mapper.tri_continue_max_angle_error=2 \ - --Mapper.tri_merge_max_reproj_error=4 \ - --Mapper.tri_complete_max_reproj_error=4 \ - --Mapper.tri_complete_max_transitivity=5 \ - --Mapper.tri_re_max_angle_error=5 \ - --Mapper.tri_re_min_ratio=0.20000000000000001 \ - --Mapper.tri_re_max_trials=1 \ - --Mapper.tri_min_angle=1.5 \ - --Mapper.tri_ignore_two_view_tracks=1 \ - --Mapper.snapshot_path snapshots diff --git a/kamera/colmap_processing/docker/batch/run.sh b/kamera/colmap_processing/docker/batch/run.sh deleted file mode 100755 index 6d5fb13..0000000 --- a/kamera/colmap_processing/docker/batch/run.sh +++ /dev/null @@ -1,50 +0,0 @@ -#!/bin/bash -set -e -mp4="$1" -DIR=$(dirname "${mp4}") -FILE=$(basename "${mp4}") -BASE="${FILE%.*}" -colmap_dir="${DIR}/${BASE}" -LOG=$DIR/log.txt -[[ -f $LOG ]] || touch $LOG - -echo "Starting processing of $mp4" -s0=`date +%s` - -# Create folder of the same name as the mp4 -mkdir -p $colmap_dir/sparse $colmap_dir/snapshots $colmap_dir/images0 - -# Extract mp4 into images -echo "Extracting mp4 into images at: $colmap_dir/images0" -docker run --rm -it -v "$DIR:/tmp/workdir" jrottenberg/ffmpeg -i "/tmp/workdir/$FILE" "/tmp/workdir/${BASE}/images0/frame%06d.png" -end=`date +%s` -runtime=$( echo "$end - $s0" | bc -l ) -start=`date +%s` -echo "Finished ffmpeg frame extraction on $mp4 in $runtime seconds." | tee -a $LOG - -echo "Running feature extraction." -./feature_extractor.sh $colmap_dir -end=`date +%s` -runtime=$( echo "$end - $start" | bc -l ) -start=`date +%s` -echo "Finished feature extraction on $mp4 in $runtime seconds." | tee -a $LOG - -echo "Running the exhaustive feature matcher." -./exhaustive_matcher.sh $colmap_dir -end=`date +%s` -runtime=$( echo "$end - $start" | bc -l ) -start=`date +%s` -echo "Finished exhaustive matcher on $mp4 in $runtime seconds." | tee -a $LOG - -echo "Running the mapper." -./mapper.sh $colmap_dir -end=`date +%s` -runtime=$( echo "$end - $start" | bc -l ) -start=`date +%s` -echo "Finished mapping on $mp4 in $runtime seconds." | tee -a $LOG - -end=`date +%s` -runtime=$( echo "$end - $s0" | bc -l ) - -echo "Finished processing $mp4 in $runtime seconds." | tee -a $LOG -echo "=============================================" | tee -a $LOG diff --git a/kamera/colmap_processing/docker/batch/run_all.sh b/kamera/colmap_processing/docker/batch/run_all.sh deleted file mode 100755 index 6cefca7..0000000 --- a/kamera/colmap_processing/docker/batch/run_all.sh +++ /dev/null @@ -1,6 +0,0 @@ -#!/bin/bash -set -e -MP4DIR=${1} -for mp4 in ${MP4DIR}/*.mp4; do - ./run.sh $mp4 -done diff --git a/kamera/colmap_processing/docker/bundle_adjuster.sh b/kamera/colmap_processing/docker/bundle_adjuster.sh deleted file mode 100755 index cadd3c4..0000000 --- a/kamera/colmap_processing/docker/bundle_adjuster.sh +++ /dev/null @@ -1,25 +0,0 @@ -# Pass the path to the Colmap data folder. - -docker pull colmap/colmap:latest - -docker run -it \ - --gpus all \ - --network="host" \ - -e "DISPLAY" \ - -v "/tmp/.X11-unix:/tmp/.X11-unix" \ - -w /working \ - -v $1:/working \ - colmap/colmap:latest \ - colmap bundle_adjuster \ - --input_path $2 \ - --output_path $3 \ - --BundleAdjustment.max_num_iterations=100 \ - --BundleAdjustment.max_linear_solver_iterations=200 \ - --BundleAdjustment.function_tolerance=0 \ - --BundleAdjustment.gradient_tolerance=0 \ - --BundleAdjustment.parameter_tolerance=0 \ - --BundleAdjustment.refine_focal_length=1 \ - --BundleAdjustment.refine_principal_point=0 \ - --BundleAdjustment.refine_extra_params=1 \ - --BundleAdjustment.refine_extrinsics=1 - diff --git a/kamera/colmap_processing/docker/exhaustive_matcher.sh b/kamera/colmap_processing/docker/exhaustive_matcher.sh deleted file mode 100755 index 037008c..0000000 --- a/kamera/colmap_processing/docker/exhaustive_matcher.sh +++ /dev/null @@ -1,29 +0,0 @@ -# Pass the path to the Colmap data folder. - -docker pull colmap/colmap:latest - -docker run -it \ - --gpus all \ - --network="host" \ - -e "DISPLAY" \ - -v "/tmp/.X11-unix:/tmp/.X11-unix" \ - -w /working \ - -v $1:/working \ - colmap/colmap:latest \ - colmap exhaustive_matcher \ - --database_path database.db \ - --SiftMatching.num_threads=-1 \ - --SiftMatching.use_gpu=1 \ - --SiftMatching.gpu_index=-1 \ - --SiftMatching.max_ratio=0.80000000000000004 \ - --SiftMatching.max_distance=0.69999999999999996 \ - --SiftMatching.cross_check=1 \ - --SiftMatching.max_error=4 \ - --SiftMatching.max_num_matches=32768 \ - --SiftMatching.confidence=0.999 \ - --SiftMatching.max_num_trials=10000 \ - --SiftMatching.min_inlier_ratio=0.25 \ - --SiftMatching.min_num_inliers=15 \ - --SiftMatching.multiple_models=0 \ - --SiftMatching.guided_matching=0 \ - --ExhaustiveMatching.block_size=50 diff --git a/kamera/colmap_processing/docker/feature_extractor.sh b/kamera/colmap_processing/docker/feature_extractor.sh deleted file mode 100755 index 8be0bd7..0000000 --- a/kamera/colmap_processing/docker/feature_extractor.sh +++ /dev/null @@ -1,37 +0,0 @@ -# Pass the path to the Colmap data folder. - -docker pull colmap/colmap:latest - -docker run -it \ - --gpus all \ - --network="host" \ - -e "DISPLAY" \ - -v "/tmp/.X11-unix:/tmp/.X11-unix" \ - -w /working \ - -v $1:/working \ - colmap/colmap:latest \ - colmap feature_extractor \ - --database_path database.db \ - --image_path images0 \ - --ImageReader.camera_model=OPENCV \ - --ImageReader.single_camera=0 \ - --ImageReader.single_camera_per_folder=1 \ - --ImageReader.existing_camera_id=-1 \ - --ImageReader.default_focal_length_factor=1.2 \ - --SiftExtraction.num_threads=-1 \ - --SiftExtraction.use_gpu=1 \ - --SiftExtraction.gpu_index=-1 \ - --SiftExtraction.max_image_size=3200 \ - --SiftExtraction.max_num_features=8192 \ - --SiftExtraction.first_octave=-1 \ - --SiftExtraction.num_octaves=11 \ - --SiftExtraction.octave_resolution=3 \ - --SiftExtraction.peak_threshold=0.0066666666666666671 \ - --SiftExtraction.edge_threshold=10 \ - --SiftExtraction.estimate_affine_shape=0 \ - --SiftExtraction.max_num_orientations=2 \ - --SiftExtraction.upright=0 \ - --SiftExtraction.domain_size_pooling=0 \ - --SiftExtraction.dsp_min_scale=0.16666666666666666 \ - --SiftExtraction.dsp_max_scale=3 \ - --SiftExtraction.dsp_num_scales=10 diff --git a/kamera/colmap_processing/docker/full.sh b/kamera/colmap_processing/docker/full.sh deleted file mode 100755 index b0dfc47..0000000 --- a/kamera/colmap_processing/docker/full.sh +++ /dev/null @@ -1,9 +0,0 @@ -# Location of this script. -DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null 2>&1 && pwd )" - -source $DIR/feature_extractor.sh $1 -source $DIR/vocab_tree_matcher.sh $1 -#source $DIR/exhaustive_matcher.sh $1 -mkdir $DIR/sparse -mkdir $DIR/snapshots -source $DIR/mapper.sh $1 diff --git a/kamera/colmap_processing/docker/hierarchical_mapper.sh b/kamera/colmap_processing/docker/hierarchical_mapper.sh deleted file mode 100755 index 4edac1f..0000000 --- a/kamera/colmap_processing/docker/hierarchical_mapper.sh +++ /dev/null @@ -1,75 +0,0 @@ -# Pass the path to the Colmap data folder. - -docker pull colmap/colmap:latest - -docker run -it \ - --gpus all \ - --network="host" \ - -e "DISPLAY" \ - -v "/tmp/.X11-unix:/tmp/.X11-unix" \ - -w /working \ - -v $1:/working \ - colmap/colmap:latest \ - colmap hierarchical_mapper \ - --database_path database.db \ - --image_path images0 \ - --output_path sparse \ - --num_workers=-1 \ - --image_overlap=50 \ - --leaf_max_num_images=500 \ - --Mapper.min_num_matches=15 \ - --Mapper.ignore_watermarks=0 \ - --Mapper.multiple_models=1 \ - --Mapper.max_num_models=50 \ - --Mapper.max_model_overlap=20 \ - --Mapper.min_model_size=10 \ - --Mapper.init_image_id1=-1 \ - --Mapper.init_image_id2=-1 \ - --Mapper.init_num_trials=200 \ - --Mapper.extract_colors=1 \ - --Mapper.num_threads=-1 \ - --Mapper.min_focal_length_ratio=0.10000000000000001 \ - --Mapper.max_focal_length_ratio=10 \ - --Mapper.max_extra_param=1 \ - --Mapper.ba_refine_focal_length=1 \ - --Mapper.ba_refine_principal_point=0 \ - --Mapper.ba_refine_extra_params=1 \ - --Mapper.ba_min_num_residuals_for_multi_threading=50000 \ - --Mapper.ba_local_num_images=6 \ - --Mapper.ba_local_max_num_iterations=25 \ - --Mapper.ba_global_use_pba=0 \ - --Mapper.ba_global_pba_gpu_index=-1 \ - --Mapper.ba_global_images_ratio=1.1000000000000001 \ - --Mapper.ba_global_points_ratio=1.1000000000000001 \ - --Mapper.ba_global_images_freq=500 \ - --Mapper.ba_global_points_freq=250000 \ - --Mapper.ba_global_max_num_iterations=50 \ - --Mapper.ba_global_max_refinements=5 \ - --Mapper.ba_global_max_refinement_change=0.00050000000000000001 \ - --Mapper.ba_local_max_refinements=2 \ - --Mapper.ba_local_max_refinement_change=0.001 \ - --Mapper.snapshot_images_freq=0 \ - --Mapper.fix_existing_images=0 \ - --Mapper.init_min_num_inliers=100 \ - --Mapper.init_max_error=4 \ - --Mapper.init_max_forward_motion=0.94999999999999996 \ - --Mapper.init_min_tri_angle=16 \ - --Mapper.init_max_reg_trials=2 \ - --Mapper.abs_pose_max_error=12 \ - --Mapper.abs_pose_min_num_inliers=30 \ - --Mapper.abs_pose_min_inlier_ratio=0.25 \ - --Mapper.filter_max_reproj_error=4 \ - --Mapper.filter_min_tri_angle=1.5 \ - --Mapper.max_reg_trials=3 \ - --Mapper.tri_max_transitivity=1 \ - --Mapper.tri_create_max_angle_error=2 \ - --Mapper.tri_continue_max_angle_error=2 \ - --Mapper.tri_merge_max_reproj_error=4 \ - --Mapper.tri_complete_max_reproj_error=4 \ - --Mapper.tri_complete_max_transitivity=5 \ - --Mapper.tri_re_max_angle_error=5 \ - --Mapper.tri_re_min_ratio=0.20000000000000001 \ - --Mapper.tri_re_max_trials=1 \ - --Mapper.tri_min_angle=1.5 \ - --Mapper.tri_ignore_two_view_tracks=1 \ - --Mapper.snapshot_path snapshots diff --git a/kamera/colmap_processing/docker/image_rectifier.sh b/kamera/colmap_processing/docker/image_rectifier.sh deleted file mode 100755 index 101dc27..0000000 --- a/kamera/colmap_processing/docker/image_rectifier.sh +++ /dev/null @@ -1,13 +0,0 @@ -# Pass the path to the Colmap data folder. - -docker pull colmap/colmap:latest - -docker run -it \ - --gpus all \ - --network="host" \ - -e "DISPLAY" \ - -v "/tmp/.X11-unix:/tmp/.X11-unix" \ - -w /working \ - -v $1:/working \ - colmap/colmap:latest \ - colmap image_rectifier diff --git a/kamera/colmap_processing/docker/image_undistorter.sh b/kamera/colmap_processing/docker/image_undistorter.sh deleted file mode 100755 index ae05e49..0000000 --- a/kamera/colmap_processing/docker/image_undistorter.sh +++ /dev/null @@ -1,18 +0,0 @@ -# Pass the path to the Colmap data folder. - -docker pull colmap/colmap:latest - -docker run -it \ - --gpus all \ - --network="host" \ - -e "DISPLAY" \ - -v "/tmp/.X11-unix:/tmp/.X11-unix" \ - -w /working \ - -v $1:/working \ - colmap/colmap:latest \ - colmap image_undistorter \ - --image_path images0 \ - --input_path sparse/$2 \ - --output_path . \ - --output_type COLMAP \ - --max_image_size 4000 diff --git a/kamera/colmap_processing/docker/mapper.sh b/kamera/colmap_processing/docker/mapper.sh deleted file mode 100755 index d3a6fc5..0000000 --- a/kamera/colmap_processing/docker/mapper.sh +++ /dev/null @@ -1,72 +0,0 @@ -# Pass the path to the Colmap data folder. - -docker pull colmap/colmap:latest - -docker run -it \ - --gpus all \ - --network="host" \ - -e "DISPLAY" \ - -v "/tmp/.X11-unix:/tmp/.X11-unix" \ - -w /working \ - -v $1:/working \ - colmap/colmap:latest \ - colmap mapper \ - --database_path database.db \ - --image_path images0 \ - --output_path sparse \ - --Mapper.min_num_matches=15 \ - --Mapper.ignore_watermarks=0 \ - --Mapper.multiple_models=1 \ - --Mapper.max_num_models=50 \ - --Mapper.max_model_overlap=20 \ - --Mapper.min_model_size=10 \ - --Mapper.init_image_id1=-1 \ - --Mapper.init_image_id2=-1 \ - --Mapper.init_num_trials=200 \ - --Mapper.extract_colors=1 \ - --Mapper.num_threads=-1 \ - --Mapper.min_focal_length_ratio=0.10000000000000001 \ - --Mapper.max_focal_length_ratio=10 \ - --Mapper.max_extra_param=1 \ - --Mapper.ba_refine_focal_length=1 \ - --Mapper.ba_refine_principal_point=0 \ - --Mapper.ba_refine_extra_params=1 \ - --Mapper.ba_min_num_residuals_for_multi_threading=50000 \ - --Mapper.ba_local_num_images=6 \ - --Mapper.ba_local_max_num_iterations=25 \ - --Mapper.ba_global_use_pba=1 \ - --Mapper.ba_global_pba_gpu_index=-1 \ - --Mapper.ba_global_images_ratio=1.1000000000000001 \ - --Mapper.ba_global_points_ratio=1.1000000000000001 \ - --Mapper.ba_global_images_freq=500 \ - --Mapper.ba_global_points_freq=250000 \ - --Mapper.ba_global_max_num_iterations=50 \ - --Mapper.ba_global_max_refinements=5 \ - --Mapper.ba_global_max_refinement_change=0.00050000000000000001 \ - --Mapper.ba_local_max_refinements=2 \ - --Mapper.ba_local_max_refinement_change=0.001 \ - --Mapper.snapshot_images_freq=100 \ - --Mapper.fix_existing_images=0 \ - --Mapper.init_min_num_inliers=100 \ - --Mapper.init_max_error=4 \ - --Mapper.init_max_forward_motion=0.94999999999999996 \ - --Mapper.init_min_tri_angle=16 \ - --Mapper.init_max_reg_trials=2 \ - --Mapper.abs_pose_max_error=12 \ - --Mapper.abs_pose_min_num_inliers=30 \ - --Mapper.abs_pose_min_inlier_ratio=0.25 \ - --Mapper.filter_max_reproj_error=4 \ - --Mapper.filter_min_tri_angle=1.5 \ - --Mapper.max_reg_trials=3 \ - --Mapper.tri_max_transitivity=1 \ - --Mapper.tri_create_max_angle_error=2 \ - --Mapper.tri_continue_max_angle_error=2 \ - --Mapper.tri_merge_max_reproj_error=4 \ - --Mapper.tri_complete_max_reproj_error=4 \ - --Mapper.tri_complete_max_transitivity=5 \ - --Mapper.tri_re_max_angle_error=5 \ - --Mapper.tri_re_min_ratio=0.20000000000000001 \ - --Mapper.tri_re_max_trials=1 \ - --Mapper.tri_min_angle=1.5 \ - --Mapper.tri_ignore_two_view_tracks=1 \ - --Mapper.snapshot_path snapshots diff --git a/kamera/colmap_processing/docker/model_aligner.sh b/kamera/colmap_processing/docker/model_aligner.sh deleted file mode 100755 index 0e2a7cd..0000000 --- a/kamera/colmap_processing/docker/model_aligner.sh +++ /dev/null @@ -1,19 +0,0 @@ -# Pass the path to the Colmap data folder. - -docker pull colmap/colmap:latest - -docker run -it \ - --gpus all \ - --network="host" \ - -e "DISPLAY" \ - -v "/tmp/.X11-unix:/tmp/.X11-unix" \ - -w /working \ - -v $1:/working \ - colmap/colmap:latest \ - colmap model_aligner \ - --input_path $2 \ - --ref_images_path $3 \ - --output_path $4 \ - --min_common_images=3 \ - --robust_alignment=1 \ - --robust_alignment_max_error=5 diff --git a/kamera/colmap_processing/docker/model_merger.sh b/kamera/colmap_processing/docker/model_merger.sh deleted file mode 100755 index bc2fece..0000000 --- a/kamera/colmap_processing/docker/model_merger.sh +++ /dev/null @@ -1,16 +0,0 @@ -# Pass the path to the Colmap data folder. - -docker pull colmap/colmap:latest - -docker run -it \ - --gpus all \ - --network="host" \ - -e "DISPLAY" \ - -v "/tmp/.X11-unix:/tmp/.X11-unix" \ - -w /working \ - -v $1:/working \ - colmap/colmap:latest \ - colmap model_merger \ - --input_path1 sparse/$2 \ - --input_path2 sparse/$3 \ - --output_path sparse/merged diff --git a/kamera/colmap_processing/docker/patch_match_stereo.sh b/kamera/colmap_processing/docker/patch_match_stereo.sh deleted file mode 100755 index c46546e..0000000 --- a/kamera/colmap_processing/docker/patch_match_stereo.sh +++ /dev/null @@ -1,71 +0,0 @@ -# Pass the path to the Colmap data folder. - -docker pull colmap/colmap:latest - -docker run -it \ - --gpus all \ - --network="host" \ - -e "DISPLAY" \ - -v "/tmp/.X11-unix:/tmp/.X11-unix" \ - -w /working \ - -v $1:/working \ - colmap/colmap:latest \ - colmap patch_match_stereo \ - --workspace_path . \ - --workspace_format COLMAP \ - --pmvs_option_name option-all \ - --PatchMatchStereo.max_image_size -1 \ - --PatchMatchStereo.gpu_index -1 \ - --PatchMatchStereo.depth_min -1 \ - --PatchMatchStereo.depth_max -1 \ - --PatchMatchStereo.window_radius 5 \ - --PatchMatchStereo.window_step 1 \ - --PatchMatchStereo.sigma_spatial -1 \ - --PatchMatchStereo.sigma_color 0.20000000298023224 \ - --PatchMatchStereo.num_samples 15 \ - --PatchMatchStereo.ncc_sigma 0.60000002384185791 \ - --PatchMatchStereo.min_triangulation_angle 1 \ - --PatchMatchStereo.incident_angle_sigma 0.89999997615814209 \ - --PatchMatchStereo.num_iterations 5 \ - --PatchMatchStereo.geom_consistency 1 \ - --PatchMatchStereo.geom_consistency_regularizer 0.30000001192092896 \ - --PatchMatchStereo.geom_consistency_max_cost 3 \ - --PatchMatchStereo.filter 1 \ - --PatchMatchStereo.filter_min_ncc 0.10000000149011612 \ - --PatchMatchStereo.filter_min_triangulation_angle 3 \ - --PatchMatchStereo.filter_min_num_consistent 2 \ - --PatchMatchStereo.filter_geom_consistency_max_cost 1 \ - --PatchMatchStereo.cache_size 128 \ - --PatchMatchStereo.write_consistency_graph 0 - -# Options -# --random_seed arg (=0) -# --project_path arg -# --workspace_path arg Path to the folder containing the -# undistorted images -# --workspace_format arg (=COLMAP) {COLMAP, PMVS} -# --pmvs_option_name arg (=option-all) -# --PatchMatchStereo.max_image_size arg (=-1) -# --PatchMatchStereo.gpu_index arg (=-1) -# --PatchMatchStereo.depth_min arg (=-1) -# --PatchMatchStereo.depth_max arg (=-1) -# --PatchMatchStereo.window_radius arg (=5) -# --PatchMatchStereo.window_step arg (=1) -# --PatchMatchStereo.sigma_spatial arg (=-1) -# --PatchMatchStereo.sigma_color arg (=0.20000000298023224) -# --PatchMatchStereo.num_samples arg (=15) -# --PatchMatchStereo.ncc_sigma arg (=0.60000002384185791) -# --PatchMatchStereo.min_triangulation_angle arg (=1) -# --PatchMatchStereo.incident_angle_sigma arg (=0.89999997615814209) -# --PatchMatchStereo.num_iterations arg (=5) -# --PatchMatchStereo.geom_consistency arg (=1) -# --PatchMatchStereo.geom_consistency_regularizer arg (=0.30000001192092896) -# --PatchMatchStereo.geom_consistency_max_cost arg (=3) -# --PatchMatchStereo.filter arg (=1) -# --PatchMatchStereo.filter_min_ncc arg (=0.10000000149011612) -# --PatchMatchStereo.filter_min_triangulation_angle arg (=3) -# --PatchMatchStereo.filter_min_num_consistent arg (=2) -# --PatchMatchStereo.filter_geom_consistency_max_cost arg (=1) -# --PatchMatchStereo.cache_size arg (=32) -# --PatchMatchStereo.write_consistency_graph arg (=0) - diff --git a/kamera/colmap_processing/docker/point_filtering.sh b/kamera/colmap_processing/docker/point_filtering.sh deleted file mode 100755 index d8fc2eb..0000000 --- a/kamera/colmap_processing/docker/point_filtering.sh +++ /dev/null @@ -1,18 +0,0 @@ -# Pass the path to the Colmap data folder. - -docker pull colmap/colmap:latest - -docker run -it \ - --gpus all \ - --network="host" \ - -e "DISPLAY" \ - -v "/tmp/.X11-unix:/tmp/.X11-unix" \ - -w /working \ - -v $1:/working \ - colmap/colmap:latest \ - colmap point_filtering \ - --input_path $2 \ - --output_path sparse \ - --min_track_len=5 \ - --max_reproj_error=4 \ - --min_tri_angle=3 \ diff --git a/kamera/colmap_processing/docker/quick-start.sh b/kamera/colmap_processing/docker/quick-start.sh deleted file mode 100755 index a407742..0000000 --- a/kamera/colmap_processing/docker/quick-start.sh +++ /dev/null @@ -1,10 +0,0 @@ -docker pull colmap/colmap:latest - -docker run -it \ - --gpus all \ - --network="host" \ - -e "DISPLAY" \ - -v "/tmp/.X11-unix:/tmp/.X11-unix" \ - -w /working \ - -v $1:/working \ - colmap/colmap:latest diff --git a/kamera/colmap_processing/docker/stereo_fusion.sh b/kamera/colmap_processing/docker/stereo_fusion.sh deleted file mode 100755 index cf86e8f..0000000 --- a/kamera/colmap_processing/docker/stereo_fusion.sh +++ /dev/null @@ -1,37 +0,0 @@ -# Pass the path to the Colmap data folder. - -docker pull colmap/colmap:latest - -docker run -it \ - --gpus all \ - --network="host" \ - -e "DISPLAY" \ - -v "/tmp/.X11-unix:/tmp/.X11-unix" \ - -w /working \ - -v $1:/working \ - colmap/colmap:latest \ - colmap stereo_fusion \ - --workspace_path . \ - --workspace_format COLMAP \ - --output_path fused.ply \ - --input_type photometric \ - --StereoFusion.max_image_size -1 \ - --StereoFusion.min_num_pixels 10 \ - --StereoFusion.max_num_pixels 10000 \ - --StereoFusion.max_traversal_depth 100 \ - --StereoFusion.max_reproj_error 2 \ - --StereoFusion.max_depth_error 0.0099999997764825821 \ - --StereoFusion.max_normal_error 10 \ - --StereoFusion.check_num_images 3 \ - --StereoFusion.cache_size 100 \ - -# photometric (default=geometric) {photometric, geometric} -# max_image_size -1 \ # (default=-1) -# min_num_pixels 10 \ # (default=5) -# max_num_pixels 10000 \ # (default=10000) -# max_traversal_depth 100 \ # (default=100) -# max_reproj_error 2 \ # (default=2) -# max_depth_error 0.0099999997764825821 \ # (default=0.0099999997764825821) -# max_normal_error 10 \ # (default=10) -# check_num_images 50 \ # (default=50) -# cache_size 100 \ # (default=32) diff --git a/kamera/colmap_processing/docker/vocab_tree_matcher.sh b/kamera/colmap_processing/docker/vocab_tree_matcher.sh deleted file mode 100755 index 3dc1cf4..0000000 --- a/kamera/colmap_processing/docker/vocab_tree_matcher.sh +++ /dev/null @@ -1,38 +0,0 @@ -# Pass the path to the Colmap data folder. - -docker pull colmap/colmap:latest - -# Location of this script. -DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null 2>&1 && pwd )" - -docker run -it \ - --gpus all \ - --network="host" \ - -e "DISPLAY" \ - -v "/tmp/.X11-unix:/tmp/.X11-unix" \ - -w /working \ - -v $1:/working \ - -v $DIR:/opt/colmap \ - colmap/colmap:latest \ - colmap vocab_tree_matcher \ - --database_path database.db \ - --SiftMatching.num_threads=-1 \ - --SiftMatching.use_gpu=1 \ - --SiftMatching.gpu_index=-1 \ - --SiftMatching.max_ratio=0.80000000000000004 \ - --SiftMatching.max_distance=0.69999999999999996 \ - --SiftMatching.cross_check=1 \ - --SiftMatching.max_error=4 \ - --SiftMatching.max_num_matches=32768 \ - --SiftMatching.confidence=0.999 \ - --SiftMatching.max_num_trials=10000 \ - --SiftMatching.min_inlier_ratio=0.25 \ - --SiftMatching.min_num_inliers=15 \ - --SiftMatching.multiple_models=0 \ - --SiftMatching.guided_matching=0 \ - --VocabTreeMatching.num_images=100 \ - --VocabTreeMatching.num_nearest_neighbors=5 \ - --VocabTreeMatching.num_checks=256 \ - --VocabTreeMatching.num_images_after_verification=0 \ - --VocabTreeMatching.max_num_features=-1 \ - --VocabTreeMatching.vocab_tree_path /opt/colmap/vocab_tree_flickr100K_words1M.bin diff --git a/kamera/colmap_processing/dp.py b/kamera/colmap_processing/dp.py deleted file mode 100644 index d6f6b72..0000000 --- a/kamera/colmap_processing/dp.py +++ /dev/null @@ -1,187 +0,0 @@ -#!/usr/bin/env python -"""Find height, width of the largest rectangle containing all 0's in the matrix. - -The algorithm for `max_size()` is suggested by @j_random_hacker [1]. -The algorithm for `max_rectangle_size()` is from [2]. -The Python implementation [3] is dual licensed under CC BY-SA 3.0 -and ISC license. - -[1]: http://stackoverflow.com/questions/2478447/find-largest-rectangle-containing-only-zeros-in-an-nn-binary-matrix#comment5169734_4671342 - -[2]: http://blog.csdn.net/arbuckle/archive/2006/05/06/710988.aspx - -[3]: http://stackoverflow.com/a/4671342 - -Copyright (c) 2014, zed - -Permission to use, copy, modify, and/or distribute this software for any -purpose with or without fee is hereby granted, provided that the above -copyright notice and this permission notice appear in all copies. - -THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES -WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF -MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR -ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES -WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN -ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF -OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. -""" -from collections import namedtuple -from operator import mul -import numpy as np - -try: - reduce = reduce -except NameError: - from functools import reduce # py3k - -Info = namedtuple('Info', 'start height') - -def max_size(mat, value=0): - """Find height, width of the largest rectangle containing all `value`'s. - - For each row solve "Largest Rectangle in a Histrogram" problem [1]: - - [1]: http://blog.csdn.net/arbuckle/archive/2006/05/06/710988.aspx - """ - it = iter(mat) - hist = [(el==value) for el in next(it, [])] - max_sizei, left_col_index = max_rectangle_size(hist) - curr_best_area = 0 - max_size = max_sizei - row_start = row_end = col_start = col_end = None - for bottom_row_index, row in enumerate(it): - hist = [(1+h) if el == value else 0 for h, el in zip(hist, row)] - #print(max_rectangle_size(hist)) - max_sizei, left_col_index = max_rectangle_size(hist) - if area(max_sizei) > curr_best_area: - curr_best_area = area(max_sizei) - col_start = left_col_index - col_end = left_col_index + max_sizei[1] - row_end = bottom_row_index + 2 - row_start = row_end - max_sizei[0] - max_size = max_sizei - - if row_start is not None: - mat = np.array(mat) - a = mat[row_start:row_end,col_start:col_end] - import copy; mat2 = copy.copy(mat) - mat2[row_start:row_end,col_start:col_end] = 100 - assert np.all(a == value) - assert area(a.shape) == curr_best_area - - return max_size, row_start, row_end, col_start, col_end - - -def max_rectangle_size(histogram): - """Find height, width of the largest rectangle that fits entirely under - the histogram. - - >>> f = max_rectangle_size - >>> f([5,3,1]) - (3, 2) - >>> f([1,3,5]) - (3, 2) - >>> f([3,1,5]) - (5, 1) - >>> f([4,8,3,2,0]) - (3, 3) - >>> f([4,8,3,1,1,0]) - (3, 3) - >>> f([1,2,1]) - (1, 3) - - Algorithm is "Linear search using a stack of incomplete subproblems" [1]. - - [1]: http://blog.csdn.net/arbuckle/archive/2006/05/06/710988.aspx - """ - stack = [] - top = lambda: stack[-1] - max_size = (0, 0) # height, width of the largest rectangle - pos = 0 # current position in the histogram - best_start = None - for pos, height in enumerate(histogram): - start = pos # position where rectangle starts - while True: - if not stack or height > top().height: - stack.append(Info(start, height)) # push - elif stack and height < top().height: - a = (top().height, (pos - top().start)) - if area(a) >= area(max_size): - max_size = a - best_start = top().start - start, _ = stack.pop() - continue - break # height == top().height goes here - - pos += 1 - for start, height in stack: - if area((height, (pos - start))) >= area(max_size): - max_size = (height, (pos - start)) - best_start = start - - return max_size, best_start - -def area(size): - return reduce(mul, size) - -import unittest -class TestCase(unittest.TestCase): - def test(self): - self.assertEqual(max_size(self.__s2m(""" - 0 0 0 0 1 0 - 0 0 1 0 0 1 - 0 0 0 0 0 0 - 1 0 0 0 0 0 - 0 0 0 0 0 1 - 0 0 1 0 0 0"""))[0], (3, 4)) - self.assertEqual(max_size([[1, 1], [0, 0]])[0], (1, 2)) - self.assertEqual(max_size([[0, 0], [1, 1]])[0], (1, 2)) - self.assertEqual(max_size([[1, 0], [1, 0]])[0], (2, 1)) - self.assertEqual(max_size([[0, 1], [0, 1]])[0], (2, 1)) - self.assertEqual(max_size(self.__s2m(""" - 0 0 0 0 1 0 - 0 0 1 0 0 1 - 0 0 0 0 0 0 - 1 0 0 0 0 0 - 0 0 0 0 0 1 - 0 0 1 0 0 0 - 0 0 0 0 0 0 - 0 0 0 0 0 0"""))[0], (7, 2)) - self.assertEqual(max_size([[]])[0], (0, 0)) - self.assertEqual(max_size([])[0], (0, 0)) - self.assertEqual(max_size(self.__s2m(""" - 0 0 0 0 1 0 - 0 0 1 0 0 1 - 0 0 0 0 0 0 - 1 0 0 0 0 0 - 0 0 0 0 0 0 - 0 0 1 0 0 1 - 0 0 0 0 0 0 - 0 0 0 0 0 0"""))[0], (3, 5)) - self.assertEqual(max_size(self.__s2m(""" - 0 0 0 0 1 0 - 0 0 0 0 0 0 - 0 0 1 0 0 1 - 0 0 0 0 0 0 - 1 0 0 0 0 0 - 0 0 0 0 0 0 - 0 0 1 0 0 1 - 0 0 0 0 0 0 - 0 0 0 0 0 1"""))[0], (8, 2)) - self.assertEqual(max_size(self.__s2m(""" - 0 0 0 0 1 1 1 - 0 0 0 0 0 0 0 - 0 0 0 1 1 1 1 - 0 0 1 1 1 1 1 - 1 0 1 1 1 1 1 - 1 0 1 1 1 1 1 - 1 0 1 1 1 1 1 - """))[0], (3, 3)) - - def __s2m(self, s): - return [map(int, line.split()) - for line in s.splitlines() if line.strip()] - -if __name__=="__main__": - import unittest; unittest.main() diff --git a/kamera/colmap_processing/geotiff.py b/kamera/colmap_processing/geotiff.py index d8651e2..99a0598 100644 --- a/kamera/colmap_processing/geotiff.py +++ b/kamera/colmap_processing/geotiff.py @@ -1,41 +1,9 @@ #!/usr/bin/env python """ -ckwg +31 -Copyright 2017-2018 by Kitware, Inc. -All rights reserved. - -Redistribution and use in source and binary forms, with or without -modification, are permitted provided that the following conditions are met: - - * Redistributions of source code must retain the above copyright notice, - this list of conditions and the following disclaimer. - - * Redistributions in binary form must reproduce the above copyright notice, - this list of conditions and the following disclaimer in the documentation - and/or other materials provided with the distribution. - - * Neither name of Kitware, Inc. nor the names of any contributors may be used - to endorse or promote products derived from this software without specific - prior written permission. - -THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS ``AS IS'' -AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHORS OR CONTRIBUTORS BE LIABLE FOR -ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - -============================================================================== - Library handling projection operations of a standard camera model. Note: the raster coordiante system has its origin at the center of the top left pixel. - """ from __future__ import division, print_function, absolute_import import numpy as np @@ -47,7 +15,7 @@ pass # Kitware imports -from colmap_processing.geo_conversions import enu_to_llh, llh_to_enu, \ +from kamera.colmap_processing.geo_conversions import enu_to_llh, llh_to_enu, \ FastENUConverter diff --git a/kamera/colmap_processing/image_renderer.py b/kamera/colmap_processing/image_renderer.py index af7f720..9bcd4b7 100644 --- a/kamera/colmap_processing/image_renderer.py +++ b/kamera/colmap_processing/image_renderer.py @@ -6,8 +6,7 @@ import PIL -def save_gif(images, fname, duration=500, - loop=0): +def save_gif(images, fname, duration=500, loop=0): """ :param img1 :type img1: @@ -25,12 +24,12 @@ def save_gif(images, fname, duration=500, :type loop: int """ images = [PIL.Image.fromarray(img) for img in images] - images[0].save(fname, save_all=True, append_images=images[1:], - duration=duration, loop=loop) + images[0].save( + fname, save_all=True, append_images=images[1:], duration=duration, loop=loop + ) -def warp_perspective(image, h, dsize, interpolation=0, use_pyr=True, - precrop=True): +def warp_perspective(image, h, dsize, interpolation=0, use_pyr=True, precrop=True): """ :param h: Homography that takes output image coordinates and returns source image coordinates. @@ -69,26 +68,26 @@ def warp_perspective(image, h, dsize, interpolation=0, use_pyr=True, if precrop: # Select points from corners of output image. - im_pts = np.ones((3,4), np.float32) - im_pts[0] = [0,1,1,0] + im_pts = np.ones((3, 4), np.float32) + im_pts[0] = [0, 1, 1, 0] im_pts[0] *= dsize[0] - im_pts[1] = [0,0,1,1] + im_pts[1] = [0, 0, 1, 1] im_pts[1] *= dsize[1] src_im_pts = np.dot(h, im_pts) src_im_pts[0] /= src_im_pts[2] src_im_pts[1] /= src_im_pts[2] # Collect the bounding box on src_img - x_range = np.array([src_im_pts[0].min(),src_im_pts[0].max()]) - y_range = np.array([src_im_pts[1].min(),src_im_pts[1].max()]) + x_range = np.array([src_im_pts[0].min(), src_im_pts[0].max()]) + y_range = np.array([src_im_pts[1].min(), src_im_pts[1].max()]) # Clamp to actual image dimensions if exceeded. # Pad by p for interpolation. p = 8 - x_range[0] = np.maximum(0, x_range[0]-p) - x_range[1] = np.minimum(res_x, x_range[1]+p) - y_range[0] = np.maximum(0, y_range[0]-p) - y_range[1] = np.minimum(res_y, y_range[1]+p) + x_range[0] = np.maximum(0, x_range[0] - p) + x_range[1] = np.minimum(res_x, x_range[1] + p) + y_range[0] = np.maximum(0, y_range[0] - p) + y_range[1] = np.minimum(res_y, y_range[1] + p) x_range = np.round(x_range).astype(np.int) y_range = np.round(y_range).astype(np.int) @@ -96,50 +95,68 @@ def warp_perspective(image, h, dsize, interpolation=0, use_pyr=True, # Want a homography that maps from the full version of image to the # cropped version defined by x_range and y_range. h_crop = np.identity(3) - h_crop[:2,2] = -x_range[0], -y_range[0] + h_crop[:2, 2] = -x_range[0], -y_range[0] h = np.dot(h_crop, h) - image = image[y_range[0]:y_range[1],x_range[0]:x_range[1]] + image = image[y_range[0] : y_range[1], x_range[0] : x_range[1]] if 0 in image.shape: if image.ndim == 3: - return np.zeros((dsize[1],dsize[0],3), image.dtype) + return np.zeros((dsize[1], dsize[0], 3), image.dtype) else: - return np.zeros((dsize[1],dsize[0]), image.dtype) + return np.zeros((dsize[1], dsize[0]), image.dtype) flags = interpolation | cv2.WARP_INVERSE_MAP warped_image = cv2.warpPerspective(image, h, dsize=dsize, flags=flags) return warped_image + def visualize_camera_and_points(pose_mat, points): fig = plt.figure() - ax = fig.add_subplot(111, projection='3d') + ax = fig.add_subplot(111, projection="3d") # Plot camera position cam_pos = pose_mat[:3, 3] - ax.scatter(cam_pos[0], cam_pos[1], cam_pos[2], c='r', marker='^', label='Camera') + ax.scatter(cam_pos[0], cam_pos[1], cam_pos[2], c="r", marker="^", label="Camera") # Plot points points_np = np.array(points) - ax.scatter(points_np[:,0], points_np[:,1], points_np[:,2], c='b', marker='o', label='Points') + ax.scatter( + points_np[:, 0], + points_np[:, 1], + points_np[:, 2], + c="b", + marker="o", + label="Points", + ) # Draw camera orientation axes axes_length = 1.0 rotation_matrix = pose_mat[:3, :3] - for i, color in zip(range(3), ['r', 'g', 'b']): + for i, color in zip(range(3), ["r", "g", "b"]): axis = rotation_matrix[:, i] * axes_length - ax.quiver(cam_pos[0], cam_pos[1], cam_pos[2], - axis[0], axis[1], axis[2], color=color) + ax.quiver( + cam_pos[0], cam_pos[1], cam_pos[2], axis[0], axis[1], axis[2], color=color + ) - ax.set_xlabel('X') - ax.set_ylabel('Y') - ax.set_zlabel('Z') + ax.set_xlabel("X") + ax.set_ylabel("Y") + ax.set_zlabel("Z") ax.legend() plt.show() -def render_view(src_cm, src_img, src_t, dst_cm, dst_t, interpolation=1, - block_size=1, homog_approx=False, world_model=None): +def render_view( + src_cm, + src_img, + src_t, + dst_cm, + dst_t, + interpolation=1, + block_size=1, + homog_approx=False, + world_model=None, +): """Render view onto destination camera from a source camera image. :param src_cm: Camera model for the camera that acquired src_img. @@ -185,15 +202,16 @@ def render_view(src_cm, src_img, src_t, dst_cm, dst_t, interpolation=1, """ if world_model is None: - surface_distance = 1e5 + surface_distance = 1e4 if src_img is not None: if src_cm.width != src_img.shape[1] or src_cm.height != src_img.shape[0]: - print('Camera model for image topic %s with encoded image ' - 'size %i x %i is being used to render images with ' - 'size %i x %i' % (src_cm.image_topic,src_cm.width, - src_cm.height,src_img.shape[1], - src_img.shape[0])) + print( + "Camera model with encoded image " + "size %i x %i is being used to render images with " + "size %i x %i" + % (src_cm.width, src_cm.height, src_img.shape[1], src_img.shape[0]) + ) def get_mask(X, Y, edge_buffer=4): mask = np.logical_and(X > edge_buffer, Y > edge_buffer) @@ -205,17 +223,17 @@ def get_mask(X, Y, edge_buffer=4): if homog_approx: if False: # Select points from a square centered on the focal plane. - im_pts = np.zeros((2,4), np.float32) - im_pts[0] = [0.25,0.75,0.75,0.25] + im_pts = np.zeros((2, 4), np.float32) + im_pts[0] = [0.25, 0.75, 0.75, 0.25] im_pts[0] *= dst_cm.width - im_pts[1] = [0.25,0.25,0.75,0.75] + im_pts[1] = [0.25, 0.25, 0.75, 0.75] im_pts[1] *= dst_cm.height else: # Select points from focal plane corners. - im_pts = np.zeros((2,4), np.float32) - im_pts[0] = [0,1,1,0] + im_pts = np.zeros((2, 4), np.float32) + im_pts[0] = [0, 1, 1, 0] im_pts[0] *= dst_cm.width - im_pts[1] = [0,0,1,1] + im_pts[1] = [0, 0, 1, 1] im_pts[1] *= dst_cm.height # Unproject rays into camera coordinate system. @@ -231,10 +249,11 @@ def get_mask(X, Y, edge_buffer=4): im_pts_src = src_cm.project(points, src_t).astype(np.float32) h = cv2.findHomography(im_pts.T, im_pts_src.T)[0] - #np.dot(h, [res_x/2,res_y/2,1]) - dst_img = warp_perspective(src_img, h, (dst_cm.width, dst_cm.height), - interpolation=interpolation) - mask = np.ones((dst_img.shape[0],dst_img.shape[1]), dtype=np.bool) + # np.dot(h, [res_x/2,res_y/2,1]) + dst_img = warp_perspective( + src_img, h, (dst_cm.width, dst_cm.height), interpolation=interpolation + ) + mask = np.ones((dst_img.shape[0], dst_img.shape[1]), dtype=np.bool) return dst_img, mask else: if interpolation == 0: @@ -250,10 +269,10 @@ def get_mask(X, Y, edge_buffer=4): if block_size == 1: # Densely sample all pixels. - x = np.linspace(0, dst_cm.width-1, dst_cm.width) - y = np.linspace(0, dst_cm.height-1, dst_cm.height) + x = np.linspace(0, dst_cm.width - 1, dst_cm.width) + y = np.linspace(0, dst_cm.height - 1, dst_cm.height) xb, yb = np.meshgrid(x, y) - im_pts = np.vstack([xb.ravel(),yb.ravel()]) + im_pts = np.vstack([xb.ravel(), yb.ravel()]) # Unproject rays into camera coordinate system. ray_pos, ray_dir = dst_cm.unproject(im_pts, dst_t) @@ -267,8 +286,8 @@ def get_mask(X, Y, edge_buffer=4): im_pts_src = src_cm.project(points, src_t).astype(np.float32) - X = np.reshape(im_pts_src[0], (dst_cm.height,dst_cm.width)) - Y = np.reshape(im_pts_src[1], (dst_cm.height,dst_cm.width)) + X = np.reshape(im_pts_src[0], (dst_cm.height, dst_cm.width)) + Y = np.reshape(im_pts_src[1], (dst_cm.height, dst_cm.width)) else: # Define block size and calculate grid sampling points nx = dst_cm.width // block_size @@ -277,7 +296,7 @@ def get_mask(X, Y, edge_buffer=4): y = np.linspace(0, dst_cm.height - 1, ny) # Create sparse grid with consistent indexing - xb, yb = np.meshgrid(x, y, indexing='ij') # Shape: (nx, ny) + xb, yb = np.meshgrid(x, y, indexing="ij") # Shape: (nx, ny) sampled_im_pts = np.vstack([xb.ravel(), yb.ravel()]) # Shape: (2, nx*ny) # Unproject rays into camera coordinate system @@ -298,22 +317,36 @@ def get_mask(X, Y, edge_buffer=4): Y = np.reshape(im_pts_src[1], (nx, ny)) # Shape: (nx, ny) # Create the interpolators with grid axes (x, y) - interpx = RegularGridInterpolator((x, y), X, - bounds_error=False, fill_value=np.nan) - interpy = RegularGridInterpolator((x, y), Y, - bounds_error=False, fill_value=np.nan) + interpx = RegularGridInterpolator( + (x, y), X, bounds_error=False, fill_value=np.nan + ) + interpy = RegularGridInterpolator( + (x, y), Y, bounds_error=False, fill_value=np.nan + ) # Define dense grid for evaluation with consistent indexing xd = np.linspace(0, dst_cm.width - 1, dst_cm.width) yd = np.linspace(0, dst_cm.height - 1, dst_cm.height) - y_grid, x_grid = np.meshgrid(yd, xd, indexing='ij') # Shape: (height, width) + y_grid, x_grid = np.meshgrid( + yd, xd, indexing="ij" + ) # Shape: (height, width) # Prepare points as (n_points, 2) array with (x, y) coordinates - dense_points = np.vstack([x_grid.ravel(), y_grid.ravel()]).T # Shape: (width*height, 2) + dense_points = np.vstack( + [x_grid.ravel(), y_grid.ravel()] + ).T # Shape: (width*height, 2) # Evaluate interpolators with dense_points - X_dense = interpx(dense_points).reshape(dst_cm.height, dst_cm.width).astype(np.float32) - Y_dense = interpy(dense_points).reshape(dst_cm.height, dst_cm.width).astype(np.float32) + X_dense = ( + interpx(dense_points) + .reshape(dst_cm.height, dst_cm.width) + .astype(np.float32) + ) + Y_dense = ( + interpy(dense_points) + .reshape(dst_cm.height, dst_cm.width) + .astype(np.float32) + ) dst_img = cv2.remap(src_img, X_dense, Y_dense, interpolation) @@ -325,15 +358,13 @@ def get_mask(X, Y, edge_buffer=4): def show_warped_points(src_cm, src_img, src_t, dst_cm, dst_t, block_size=1): - """Plot out points to be samples in the source image. - - """ + """Plot out points to be samples in the source image.""" if block_size == 1: # Densely sample all pixels. - x = np.linspace(0, dst_cm.width-1, dst_cm.width) - y = np.linspace(0, dst_cm.height-1, dst_cm.height) - X,Y = np.meshgrid(x, y) - im_pts = np.vstack([X.ravel(),Y.ravel()]) + x = np.linspace(0, dst_cm.width - 1, dst_cm.width) + y = np.linspace(0, dst_cm.height - 1, dst_cm.height) + X, Y = np.meshgrid(x, y) + im_pts = np.vstack([X.ravel(), Y.ravel()]) # Unproject rays into camera coordinate system. ray_pos, ray_dir = dst_cm.unproject(im_pts, dst_t) @@ -344,12 +375,12 @@ def show_warped_points(src_cm, src_img, src_t, dst_cm, dst_t, block_size=1): # Sample the full projection equation every 'blocksize' pixels and # interpolate in between. - nx = dst_cm.width//block_size - ny = dst_cm.height//block_size - x = np.linspace(0, dst_cm.width-1, nx), - y = np.linspace(0, dst_cm.height-1, ny) - X,Y = np.meshgrid(x, y) - im_pts = np.vstack([X.ravel(),Y.ravel()]) + nx = dst_cm.width // block_size + ny = dst_cm.height // block_size + x = (np.linspace(0, dst_cm.width - 1, nx),) + y = np.linspace(0, dst_cm.height - 1, ny) + X, Y = np.meshgrid(x, y) + im_pts = np.vstack([X.ravel(), Y.ravel()]) # Unproject rays into camera coordinate system. ray_pos, ray_dir = dst_cm.unproject(im_pts, dst_t) @@ -357,11 +388,18 @@ def show_warped_points(src_cm, src_img, src_t, dst_cm, dst_t, block_size=1): im_pts_src = src_cm.project(ray_dir, src_t).astype(np.float32) - plt.plot(im_pts_src[0], im_pts_src[1], 'ro') + plt.plot(im_pts_src[0], im_pts_src[1], "ro") -def stitch_images(src_list, dst_cm, dst_t, interpolation=1, block_size=1, - homog_approx=False, world_model=None): +def stitch_images( + src_list, + dst_cm, + dst_t, + interpolation=1, + block_size=1, + homog_approx=False, + world_model=None, +): """ :param src_list: List of frames and cameras of the form [src_cm, src_img, src_t]. @@ -393,8 +431,7 @@ def stitch_images(src_list, dst_cm, dst_t, interpolation=1, block_size=1, out_dtype = src_list[0][1].dtype if out_dtype == np.uint8: out_channels = 3 - dst_img = np.zeros((dst_cm.height, dst_cm.width, out_channels), - out_dtype) + dst_img = np.zeros((dst_cm.height, dst_cm.width, out_channels), out_dtype) else: out_channels = 1 dst_img = np.zeros((dst_cm.height, dst_cm.width), out_dtype) @@ -402,13 +439,19 @@ def stitch_images(src_list, dst_cm, dst_t, interpolation=1, block_size=1, for i in range(len(src_list)): src_cm, src_img, src_t = src_list[i] - #show_warped_points(src_cm, src_img, src_t, dst_cm, dst_t, 10) + # show_warped_points(src_cm, src_img, src_t, dst_cm, dst_t, 10) - img, mask = render_view(src_cm, src_img, src_t, dst_cm, dst_t, - interpolation=interpolation, - block_size=block_size, - homog_approx=homog_approx, - world_model=world_model) + img, mask = render_view( + src_cm, + src_img, + src_t, + dst_cm, + dst_t, + interpolation=interpolation, + block_size=block_size, + homog_approx=homog_approx, + world_model=world_model, + ) if out_channels == 3: if img.ndim == 2: @@ -417,7 +460,7 @@ def stitch_images(src_list, dst_cm, dst_t, interpolation=1, block_size=1, # Couldn't find a more elegant/efficient way to hangle a 2-D mask with # an RGB image. mask2 = np.zeros_like(dst_img, dtype=np.bool) - mask2[:,:,0] = mask2[:,:,1] = mask2[:,:,2] = mask + mask2[:, :, 0] = mask2[:, :, 1] = mask2[:, :, 2] = mask dst_img[mask2] = img[mask2] else: diff --git a/kamera/colmap_processing/scripts/build_calibration_database_from_colmap.py b/kamera/colmap_processing/scripts/build_calibration_database_from_colmap.py deleted file mode 100644 index 82c0215..0000000 --- a/kamera/colmap_processing/scripts/build_calibration_database_from_colmap.py +++ /dev/null @@ -1,613 +0,0 @@ -#!/usr/bin/env python -""" -ckwg +31 -Copyright 2019 by Kitware, Inc. -All rights reserved. - -Redistribution and use in source and binary forms, with or without -modification, are permitted provided that the following conditions are met: - - * Redistributions of source code must retain the above copyright notice, - this list of conditions and the following disclaimer. - - * Redistributions in binary form must reproduce the above copyright notice, - this list of conditions and the following disclaimer in the documentation - and/or other materials provided with the distribution. - - * Neither name of Kitware, Inc. nor the names of any contributors may be used - to endorse or promote products derived from this software without specific - prior written permission. - -THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS ``AS IS'' -AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHORS OR CONTRIBUTORS BE LIABLE FOR -ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - -============================================================================== - -""" -from __future__ import division, print_function, absolute_import -import numpy as np -import matplotlib.pyplot as plt -from scipy.sparse import lil_matrix -import time -from sklearn.decomposition import PCA -from scipy.spatial import KDTree -import threading -from numba import jit - -# Colmap Processing imports. -from colmap_processing.geo_conversions import llh_to_enu -from colmap_processing.colmap_interface import read_images_binary, Image, \ - read_points3D_binary, read_cameras_binary, qvec2rotmat -from colmap_processing.database import COLMAPDatabase, pair_id_to_image_ids, blob_to_array - - - -# ---------------------------------------------------------------------------- -# Base path to the colmap directory. -image_dir = 'small_test' - -# Path to the images.bin file. -images_bin_fname = 'images.bin' - -# Path to the points3D.bin file. -points_3d_bin_fname = 'points3D.bin' - -# Path to save images to in order to geo-register. -georegister_data_dir = 'small' - -model_save_location = 'models' - -# Define ENU coordinate system origin. -lat0 = 42.8646162 -lon0 = -73.7710985 - - -# Read in the details of all images. -images = read_images_binary(images_bin_fname) - - -# Remove image keypoints without associated reconstructed 3-D point. -for image_num in images: - image = images[image_num] - ind = [_ for _ in range(len(image.xys)) if image.point3D_ids[_] != -1] - xys = image.xys[ind] - point3D_ids = image.point3D_ids[ind] - - images[image_num] = Image(id=image.id, qvec=image.qvec, tvec=image.tvec, - camera_id=image.camera_id, name=image.name, - xys=xys, point3D_ids=point3D_ids) - - -if False: - # Save images with keypoints superimposed. This allows selection of - # pixels near keypoints to be geolocated. - - try: - os.makedirs(georegister_data_dir) - except OSError: - pass - - for image_num in images: - image = images[image_num] - img_fname = '%s/%s' % (image_dir, image.name) - img = cv2.imread(img_fname) - - for i, xy in enumerate(image.xys): - if image.point3D_ids[i] == -1: - continue - - xy = tuple(np.round(xy).astype(np.int)) - cv2.circle(img, xy, 5, color=(0, 0, 255), thickness=1) - - img_fname = '%s/images/%s.jpg' % (georegister_data_dir, image.id) - img = cv2.imwrite(img_fname, img) - - -pts_3d = read_points3D_binary(points_3d_bin_fname) - - -# Load in georegistration points. -xyz_to_enu = [] -for fname in glob.glob('%s/*_points.txt' % georegister_data_dir): - image_id = int(os.path.split(fname)[1].split('_points.txt')[0]) - image = images[image_id] - - pts = np.loadtxt(fname) - pts = np.atleast_2d(pts) - - for pt in pts: - ind0 = [_ for _ in range(len(image.xys)) if image.point3D_ids[_] != -1] - xys = image.xys[ind0] - d = np.sqrt(np.sum((xys - np.atleast_2d(pt[:2]))**2, 1)) - ind1 = np.argmin(d) - - print('Distance to select point:', d[ind1], 'pixels') - if d[ind1] > 20: - continue - - xyz = pts_3d[image.point3D_ids[ind0[ind1]]].xyz - - lat, lon = pt[2:] - enu = llh_to_enu(lat, lon, 0, lat0, lon0, 0) - xyz_to_enu.append(np.hstack([xyz, enu])) - - -xyz_to_enu = np.array(xyz_to_enu) -east_north = xyz_to_enu[:, 3:].T -xyz0 = xyz_to_enu[:, :3].T - - -def tform_err(x): - R = cv2.Rodrigues(x[:3])[0] - xyz = np.dot(R, xyz0) - - if x[3] < 0: - return 1e10 - - S = np.diag(np.ones(3)*x[3]) - - xyz = np.dot(S, xyz) - - xyz[0] += x[4] - xyz[1] += x[5] - xyz[2] += x[6] - - err = np.sqrt(np.mean((east_north - xyz)**2)) - print(err) - return err - - -# Solve for optimal transform -x = np.hstack([(np.random.rand(3)*2 - 1)*np.pi, [1, 0, 0, 0]]) -x = minimize(tform_err, x).x - -T = np.identity(4) -T[:3, :3] = np.dot(np.diag(np.ones(3)*x[3]), cv2.Rodrigues(x[:3])[0]) -T[:3, 3] = x[4:] - -pts_enu = {} -c = [] -for key in pts_3d: - xyz = pts_3d[key].xyz - c.append(pts_3d[key].rgb) - - if True: - xyz = np.dot(T, np.hstack([xyz, 1])) - xyz = xyz[:3]/xyz[3] - - pts_enu[key] = xyz - - -if True: - # Verify that the model is right-side up. - pts2 = np.array([_ for _ in pts - if np.all(np.abs(_) < 100) and abs(_[2]) < 40]) - pts2 = pts2.T - - fig = plt.figure() - ax = fig.add_subplot(111, projection='3d') - plt.plot(pts2[0], pts2[1], pts2[2], '.') - plt.xlabel('Easting (m)') - plt.ylabel('Northing (m)') - - -# Compute ORB feature for each 3-D point in each image. -orb = cv2.ORB_create(nfeatures=int(1e4), nlevels=10, scaleFactor=1.2) - -# feature is a dictionary that takes the image index and returns a list of -# [3d_pt_index, descriptor] -features = {} -for image_num in images: - print('Processing image', image_num, 'of', len(images)) - image = images[image_num] - img_fname = '%s/%s' % (image_dir, image.name) - img = cv2.imread(img_fname) - - if False: - col, row = np.round(image.xys).astype(np.int).T - data = np.ones(len(row), dtype=np.uint8) - mask = csr_matrix((data, (row, col)), shape=img.shape[:2]).todense() - mask = cv2.dilate(mask, np.ones((5, 5), np.uint8), iterations=1) - else: - mask = None - - kp, des = orb.detectAndCompute(img, mask) - kp_xy = np.array([_.pt for _ in kp]) - - features[image_num] = [] - - max_d = 5 # pixels - tree = spatial.KDTree(list(zip(kp_xy[:,0], kp_xy[:,1]))) - d, ind = tree.query(image.xys, k=1, distance_upper_bound=max_d) - - features[image_num] = [[pts_enu[image.point3D_ids[i]], des[ind[i]]] - for i in range(len(d)) if d[i] < max_d] - - # [image.xys[_] - np.array(kp[ind[_]].pt) for _ in range(len(d)) if d[_] < max_d] - - -try: - os.makedirs(model_save_location) -except OSError: - pass - -fname = '%s/features_per_image.p' % model_save_location -with open(fname, 'wb') as handle: - pickle.dump(features, handle, protocol=pickle.HIGHEST_PROTOCOL) - -keys = images.keys() -keys.sort() -fname = '%s/source_image_list.txt' % model_save_location -with open(fname, 'w') as f: - for key in keys: - f.write('%s %s\n' % (key, images[key].name)) - - -# Cluster descriptors to do a Bag-of-Words lookup of the nearest image. -# sklearn doesn't support Hamming distance, so convert to binary vector. -descriptors = [] -for image_num in features: - for el in features[image_num]: - descriptors.append(np.unpackbits(el[1])) - -descriptors = np.array(descriptors, np.float32) -num_clusters = len(descriptors)/100 -num_clusters = 100 -clf = KMeans(n_clusters=num_clusters, n_init=1, random_state=0, - precompute_distances=True, verbose=5, n_jobs=10) -kmeans = clf.fit(descriptors).cluster_centers_ -bow = kmeans.cluster_centers_ -#bow = np.array([np.packbits(_) for _ in np.round(bow).astype(np.bool)]) - -tree = spatial.KDTree(bow) - - -def get_hist(desc): - hist = np.zeros(num_clusters, np.int) - inds = tree.query(np.array(desc), k=1)[1] - hist = np.zeros(num_clusters, np.int) - for ind in inds: - hist[ind] += 1 - - hist = hist.astype(np.float32) - hist /= np.linalg.norm(hist) - return hist - - -bow_hist = [] -for image_num in features: - print('Processing image number:', image_num) - desc = [] - for el in features[image_num]: - desc.append(np.unpackbits(el[1])) - - if len(desc) > 0: - hist = get_hist(desc) - else: - hist = np.zeros(num_clusters, np.int) - - bow_hist.append(hist) - -bow_hist = np.array(bow_hist) - - -fname = '%s/bow.p' % model_save_location -with open(fname, 'wb') as handle: - pickle.dump([bow, bow_hist], handle, protocol=pickle.HIGHEST_PROTOCOL) - - - -# Read in test image. -img_fname = 'ref_image.png' - -orb = cv2.ORB_create(nfeatures=int(1e4), nlevels=20, scaleFactor=1.2) -img = cv2.imread(img_fname) -kp, des = orb.detectAndCompute(img, None) -des = np.array([np.unpackbits(_) for _ in des]) -hist = get_hist(des) - -d = np.dot(bow_hist, hist) -ind = np.argsort(d)[::-1] - -keys = images.keys(); keys.sort() -img_fname = '%s/%s' % (image_dir, images[keys[ind[1]]].name) -img_ref = cv2.imread(img_fname) - -plt.subplot(1, 2, 1) -plt.imshow(img) -plt.subplot(1, 2, 2) -plt.imshow(img_ref) - - - - - - - - -img_fname = 'ref_image.png' -orb = cv2.ORB_create(nfeatures=int(1e4), nlevels=30, scaleFactor=1.2) -img = cv2.imread(img_fname) -kp, des = orb.detectAndCompute(img, None) - -ind = 341 -img_fname = '%s/%s' % (image_dir, images[ind].name) -img_ref = cv2.imread(img_fname) -img_ref = cv2.fastNlMeansDenoisingColored(img_ref, None, 10, 10, 7, 21) -kp_ref, des_ref = orb.detectAndCompute(img_ref, None) - -if False: - pts, des0 = zip(*features[ind]) - pts = np.array(pts) - des0 = np.array(des0) - -bf = cv2.BFMatcher(cv2.NORM_HAMMING, crossCheck=False) -matches = bf.match(des, des_ref) - -im_pts = [] -im_pts_ref = [] - -for i, m in enumerate(matches): - im_pts.append(kp[m.queryIdx].pt) - im_pts_ref.append(kp_ref[m.trainIdx].pt) - -im_pts = np.array(im_pts, dtype=np.float32) -im_pts_ref = np.array(im_pts_ref, dtype=np.float32) - -if False: - F, mask = cv2.findFundamentalMat(im_pts, im_pts_ref, cv2.FM_RANSAC) -else: - H, mask = cv2.findHomography(im_pts, im_pts_ref, method=cv2.RANSAC, - ransacReprojThreshold=1) - -mask = mask.ravel() == 1 -good_matches = [matches[_] for _ in range(len(matches)) if mask[_]] - -# Draw first 10 matches. -plt.imshow(cv2.drawMatches(img, kp, img_ref, kp_ref, good_matches, None)) - -# We select only inlier points -im_pts = im_pts[mask.ravel() == 1] -im_pts_ref = im_pts_ref[mask.ravel() == 1] - - -def drawlines(img1, img2, lines, pts1, pts2): - ''' img1 - image on which we draw the epilines for the points in img2 - lines - corresponding epilines ''' - r,c = img1.shape[:2] - - if img1.ndim == 2: - img1 = cv2.cvtColor(img1, cv2.COLOR_GRAY2BGR) - else: - img1 = img1.copy() - - if img2.ndim == 2: - img2 = cv2.cvtColor(img2, cv2.COLOR_GRAY2BGR) - else: - img2 = img2.copy() - - for r,pt1,pt2 in zip(lines,pts1,pts2): - color = tuple(np.random.randint(0,255,3).tolist()) - x0,y0 = map(int, [0, -r[2]/r[1] ]) - x1,y1 = map(int, [c, -(r[2]+r[0]*c)/r[1] ]) - img1 = cv2.line(img1, (x0,y0), (x1,y1), color,1) - img1 = cv2.circle(img1,tuple(pt1),5,color,-1) - img2 = cv2.circle(img2,tuple(pt2),5,color,-1) - return img1,img2 - - -# Find epilines corresponding to points in right image (second image) and -# drawing its lines on left image -lines = cv2.computeCorrespondEpilines(im_pts.reshape(-1,1,2), 2, F) -lines = lines.reshape(-1,3) -img5,img6 = drawlines(img, img_ref, lines, im_pts, im_pts_ref) - -# Find epilines corresponding to points in left image (first image) and -# drawing its lines on right image -lines2 = cv2.computeCorrespondEpilines(pts1.reshape(-1,1,2), 1,F) -lines2 = lines2.reshape(-1,3) -img3,img4 = drawlines(img2,img1,lines2,pts2,pts1) - -plt.subplot(121),plt.imshow(img5) -plt.subplot(122),plt.imshow(img3) -plt.show() - - - - - - - - - inds = [np.argmin(np.sum((image.xys - np.array(_.pt))**2, 1)) for _ in kp] - ds = [np.sqrt(np.sum((image.xys[inds[i]] - np.array(kp[i].pt))**2)) - for i in range(len(inds))] - - - image.xys - - img_fname = '%s/%s' % (image_dir, image.name) - img = cv2.imread(img_fname) - - for i, xy in enumerate(image.xys): - if image.point3D_ids[i] == -1: - continue - - xy = tuple(np.round(xy).astype(np.int)) - cv2.circle(img, xy, 5, color=(0, 0, 255), thickness=1) - - features - - - - -if False: - # Draw manual key points. - save_dir = '' - for key in manual_matches: - image_id1,image_id2 = key - img1 = cv2.imread(image_fnames[image_id1 - 1])[:,:,::-1].copy() - img2 = cv2.imread(image_fnames[image_id2 - 1])[:,:,::-1].copy() - - # These are the manually selected coordinates. - kps = np.round(manual_matches[key]).astype(np.int) - kp1s = kps[:,:2] - kp2s = kps[:,2:] - - for i in range(len(kp1s)): - cv2.circle(img1, (kp1s[i][0],kp1s[i][1]), 5, (255,0,255), 2) - cv2.circle(img2, (kp2s[i][0],kp2s[i][1]), 5, (255,0,255), 2) - - fname1 = os.path.split(image_fnames[image_id1 - 1])[-1] - fname2 = os.path.split(image_fnames[image_id2 - 1])[-1] - cv2.imwrite(save_dir + fname1, img1[:,:,::-1]) - cv2.imwrite(save_dir + fname2, img2[:,:,::-1]) - - -db = COLMAPDatabase.connect(database_path) -cursor = db.cursor() - - -if False: - keep_pairs = set() - # Remove matches with insufficient inliers. - min_num_matches = 20 - cursor.execute("SELECT pair_id, data FROM two_view_geometries") - for row in cursor: - pair_id = row[0] - if row[1] is not None: - inlier_matches = np.fromstring(row[1], - dtype=np.uint32).reshape(-1, 2) - if len(inlier_matches) > min_num_matches: - keep_pairs.add(pair_id) - - all_pairs = [pair_id - for pair_id, _ in db.execute("SELECT pair_id, data FROM matches")] - - for pair_id in all_pairs: - if pair_id not in keep_pairs: - print('Deleting pair:', pair_id) - db.execute("DELETE FROM matches WHERE pair_id=?", (pair_id,)) - - -# Add missing keypoints. -for key in manual_matches: - keypoints = dict((image_id, blob_to_array(data, np.float32, (-1, 2))) - for image_id, data in db.execute( - "SELECT image_id, data FROM keypoints")) - - matches = dict( - (pair_id_to_image_ids(pair_id), - blob_to_array(data, np.uint32, (-1, 2))) - for pair_id, data in db.execute("SELECT pair_id, data FROM matches") - if data is not None) - - image_id1,image_id2 = key - keypoints1 = keypoints[image_id1] - keypoints2 = keypoints[image_id2] - - # These are the manually selected coordinates. - kp1 = manual_matches[key][:,:2] - kp2 = manual_matches[key][:,2:] - - for kp in kp1: - d = np.sqrt(np.sum((kp - keypoints1)**2, 1)) - if d.min() > 2: - keypoints1 = np.vstack([keypoints1,kp]) - - for kp in kp2: - d = np.sqrt(np.sum((kp - keypoints2)**2, 1)) - if d.min() > 2: - keypoints2 = np.vstack([keypoints2,kp]) - - # Remove old set of keypoints - db.execute("DELETE FROM keypoints WHERE image_id=?", (image_id1,)) - db.execute("DELETE FROM keypoints WHERE image_id=?", (image_id2,)) - - db.add_keypoints(image_id1, keypoints1.copy()) - db.add_keypoints(image_id2, keypoints2.copy()) - - -# Rebuild keypoint dictionary. -keypoints = dict((image_id, blob_to_array(data, np.float32, (-1, 2))) - for image_id, data in db.execute( - "SELECT image_id, data FROM keypoints")) - -# Assing manual matches to keypoints. -for key in manual_matches: - image_id1,image_id2 = key - - matches = dict( - (pair_id_to_image_ids(pair_id), - blob_to_array(data, np.uint32, (-1, 2))) - for pair_id, data in db.execute("SELECT pair_id, data FROM matches") - if data is not None) - - try: - matches = matches[(image_id1,image_id2)] - except: - matches = np.zeros((0,2), dtype=np.int) - - keypoints1 = keypoints[image_id1] - keypoints2 = keypoints[image_id2] - - # These are the manually selected coordinates. - kp1 = manual_matches[key][:,:2] - kp2 = manual_matches[key][:,2:] - - if False: - img1 = cv2.imread(image_fnames[image_id1 - 1])[:,:,::-1] - img2 = cv2.imread(image_fnames[image_id2 - 1])[:,:,::-1] - - plt.figure() - plt.imshow(img1) - #plt.plot(keypoints1[:,0], keypoints1[:,1], 'ro') - plt.plot(kp1[:,0], kp1[:,1], 'go') - - plt.figure() - plt.imshow(img2) - #plt.plot(keypoints2[:,0], keypoints2[:,1], 'ro') - plt.plot(kp2[:,0], kp2[:,1], 'go') - - m = matches[(image_id1,image_id2)][:20] - plt.figure() - plt.imshow(img1) - plt.plot(keypoints1[m[:,0],0], keypoints1[m[:,0],1], 'ro') - plt.figure() - plt.imshow(img2) - plt.plot(keypoints2[m[:,0],0], keypoints2[m[:,0],1], 'ro') - - for i in range(len(kp1)): - d = np.sqrt(np.sum((kp1[i] - keypoints1)**2, 1)) - ind1 = np.argmin(d) - assert d[ind1] < 2 - - d = np.sqrt(np.sum((kp2[i] - keypoints2)**2, 1)) - ind2 = np.argmin(d) - assert d[ind2] < 2 - - # Loop to artificially increase confidence. - for _ in range(10): - matches = np.vstack([matches,[ind1,ind2]]) - - pair_id = image_ids_to_pair_id(image_id1, image_id2) - db.execute("DELETE FROM matches WHERE pair_id=?", (pair_id,)) - db.add_matches(image_id1, image_id2, matches.copy()) - - print('Adding manually registered matches to pair:', image_id1, image_id2) - - -# Commit the data to the file. -db.commit() - -# Clean up. - -db.close() \ No newline at end of file diff --git a/kamera/colmap_processing/scripts/build_model.sh b/kamera/colmap_processing/scripts/build_model.sh deleted file mode 100755 index 844f371..0000000 --- a/kamera/colmap_processing/scripts/build_model.sh +++ /dev/null @@ -1,75 +0,0 @@ -#!/usr/bin/env bash -#2021 (C) Eugene.Borovikov@Kitware.com: build COLMAP sparse and dense models. - -# input parameters - -BaseDN=$1 -echo BaseDN=$BaseDN - -DBFN=$2 -echo DBFN=$DBFN - -ImgLstFN=$3 -echo ImgLstFN=$ImgLstFN - -VocabFN=$4 -echo VocabFN=$VocabFN - -# feature extraction -echo colmap feature_extractor \ - --database_path $DBFN \ - --image_path $BaseDN \ - --image_list_path $ImgLstFN \ - --ImageReader.camera_model=OPENCV \ - --ImageReader.single_camera_per_folder=1 - -# vocab-tree matcher -echo colmap vocab_tree_matcher \ - --database_path $DBFN \ - --VocabTreeMatching.vocab_tree_path $VocabFN - -SparseDN=$5 -echo SparseDN=$SparseDN -echo mkdir -p $SparseDN - -# sparse model mapper & bundle-adjuster -echo colmap mapper \ - --database_path $DBFN \ - --image_path $BaseDN \ - --output_path $SparseDN \ - --Mapper.multiple_models=0 - -DenseDN=$6 -echo DenseDN=$DenseDN -echo mkdir -p $DenseDN - -# image undistorter for subsequent dense reconstruction -echo colmap image_undistorter \ - --image_path $BaseDN \ - --input_path $SparseDN/0 \ - --output_path $DenseDN \ - --output_type COLMAP \ - --max_image_size 2000 - -# stereo match -echo colmap patch_match_stereo \ - --workspace_path $DenseDN \ - --workspace_format COLMAP \ - --PatchMatchStereo.geom_consistency true - -# stereo fusion -echo colmap stereo_fusion \ - --workspace_path $DenseDN \ - --workspace_format COLMAP \ - --input_type geometric \ - --output_path $DenseDN/fused.ply - -# optional poisson mesh -echo colmap poisson_mesher \ - --input_path $DenseDN/fused.ply \ - --output_path $DenseDN/poisson.ply - -# optional delaunay mesh -echo colmap delaunay_mesher \ - --input_path $DenseDN \ - --output_path $DenseDN/delaunay.ply diff --git a/kamera/colmap_processing/scripts/calibrate_static_camera_from_level_lines.py b/kamera/colmap_processing/scripts/calibrate_static_camera_from_level_lines.py deleted file mode 100644 index 9e962d0..0000000 --- a/kamera/colmap_processing/scripts/calibrate_static_camera_from_level_lines.py +++ /dev/null @@ -1,527 +0,0 @@ -#! /usr/bin/python -""" -ckwg +31 -Copyright 2021 by Kitware, Inc. -All rights reserved. - -Redistribution and use in source and binary forms, with or without -modification, are permitted provided that the following conditions are met: - - * Redistributions of source code must retain the above copyright notice, - this list of conditions and the following disclaimer. - - * Redistributions in binary form must reproduce the above copyright notice, - this list of conditions and the following disclaimer in the documentation - and/or other materials provided with the distribution. - - * Neither name of Kitware, Inc. nor the names of any contributors may be used - to endorse or promote products derived from this software without specific - prior written permission. - -THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS ``AS IS'' -AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHORS OR CONTRIBUTORS BE LIABLE FOR -ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - -============================================================================== - -""" -from __future__ import division, print_function -import numpy as np -import os -import cv2 -import matplotlib.pyplot as plt -import json -from osgeo import osr, gdal -from scipy.optimize import fmin, minimize, fminbound -from scipy.linalg import lstsq -import itertools - -# Colmap Processing imports. -from colmap_processing.geo_conversions import llh_to_enu -from colmap_processing.colmap_interface import read_images_binary, Image, \ - read_points3D_binary, read_cameras_binary, qvec2rotmat -from colmap_processing.database import COLMAPDatabase, pair_id_to_image_ids, blob_to_array -import colmap_processing.vtk_util as vtk_util -from colmap_processing.geo_conversions import enu_to_llh, llh_to_enu, \ - rmat_ecef_enu, rmat_enu_ecef -from colmap_processing.static_camera_model import save_static_camera, \ - load_static_camera_from_file, write_camera_krtd_file -from colmap_processing.calibration import calibrate_camera_to_xyz, \ - cam_depth_map_plane -from colmap_processing.camera_models import StandardCamera, DepthCamera -from colmap_processing.camera_models import quaternion_from_matrix, \ - quaternion_inverse -from colmap_processing.platform_pose import PlatformPoseFixed - - -# ---------------------------------------------------------------------------- -# Reference image. -cm_dir = '/mnt' -image_fname = '%s/ref_view.jpg' % cm_dir - -# File containing correspondences between image points (columns 0 an d1) and -# easting (m), northing (m), and height above the WGS84 ellipsoid (meters) -# (columns 2-4). -points_fname = '%s/points.txt' % cm_dir - -points_in_meters = True - -mesh_lat0 = 39.04977294 -mesh_lon0 = -85.52924953 -mesh_h0 = 205 - -# Assume flat ground with this elevation. -ground_elevation = -1.458672 - -# COCO json encoding vertical and horizontal lines. . -lines_fname = '%s/lines.json' % cm_dir - -save_dir = os.path.split(image_fname)[0] - -# VTK renderings are limited to monitor resolution (width x height). -monitor_resolution = (1000, 1000) - -dist = [0, 0, 0, 0] -optimize_k1 = True -optimize_k2 = True -optimize_k3 = False -optimize_k4 = False -fix_principal_point = False -fix_aspect_ratio = True -# ---------------------------------------------------------------------------- - -# Load in point correspondences. -ret = np.loadtxt(points_fname) -im_pts = ret[:, :2].T -pts = ret[:, 2:] - -if points_in_meters: - enu_pts = np.array(pts, dtype=np.float32).T -else: - enu_pts = [llh_to_enu(_[0], _[1], _[2], mesh_lat0, mesh_lon0, mesh_h0) - for _ in pts] - enu_pts = np.array(enu_pts, dtype=np.float32).T - -if False: - enu_pts = enu_pts.T.tolist() - im_pts = im_pts.T.tolist() - for i in [0, 1, 2, 3]: - for _ in range(20): - enu_pts.append(enu_pts[i]) - im_pts.append(im_pts[i]) - - enu_pts = np.array(enu_pts).T - im_pts = np.array(im_pts).T - -with open(lines_fname) as json_file: - lines0 = json.load(json_file) - -lines = [np.array(an['segmentation']).reshape(-1, 2).T - for an in lines0['annotations']] - -# Load in the image. -img = cv2.imread(image_fname) - -if img.ndim == 3: - img = img[:, :, ::-1] - -height, width = img.shape[:2] - -dist = np.array(dist, dtype=np.float32) -# ---------------------------------------------------------------------------- - - -# ------------------------------- Visualize ---------------------------------- -# This is a hack, we need better specification of the vertical versus -# horizontal lines. - -vert_lines = [lines[i] for i in [0, 1, 2, 3, 4, 5]] -horz_lines = [lines[i] for i in [6, 7, 8, 9, 10, 11, 12]] - -plt.imshow(img) -for line in vert_lines: - plt.plot(line[0], line[1], 'b-', linewidth=4) - -for line in horz_lines: - plt.plot(line[0], line[1], 'r-', linewidth=4) -# ---------------------------------------------------------------------------- - - -#--------------------- Find a Reasonable Starting Point ---------------------- -# Spoof more points to stabilize the process. -num_pts = im_pts.shape[1] -im_pts_spoof = [] -enu_pts_spoof = [] - -for i, j in list(itertools.combinations(range(num_pts), 2)): - for t in np.linspace(0, 1, 2): - im_pts_spoof.append(im_pts[:, i]*t + im_pts[:, j]*(1 - t)) - enu_pts_spoof.append(enu_pts[:, i]*t + enu_pts[:, j]*(1 - t)) - -im_pts_spoof = np.array(im_pts_spoof) -enu_pts_spoof = np.array(enu_pts_spoof) - -ret0 = calibrate_camera_to_xyz(im_pts_spoof, enu_pts_spoof, img.shape[0], - img.shape[1], fix_aspect_ratio=True, - fix_principal_point=False, fix_k1=True, - fix_k2=True, fix_k3=True, fix_k4=True, - fix_k5=True, fix_k6=True, plot_results=False, - ref_image=None) -K, dist, rvec, tvec = ret0 -rvec = rvec.ravel() -tvec = tvec.ravel() -R = cv2.Rodrigues(rvec)[0] -cam_pos = np.dot(R.T, -tvec) - - -def unproject_lines(xy, K, dist, rvec, tvec, plot=False): - ray_dir = np.ones((3, xy.shape[1]), dtype=np.float) - ray_dir[:2] = np.squeeze(cv2.undistortPoints(xy, K, dist, R=None), 1).T - ray_dir /= np.sqrt(np.sum(ray_dir**2, 0)) - R = cv2.Rodrigues(rvec)[0] - cam_pos = np.dot(R.T, -tvec) - xyz = np.atleast_2d(cam_pos).T + np.dot(R.T, ray_dir) - - # Calculate the mean of the points, i.e. the 'center' of the cloud - datamean = xyz.mean(axis=1) - - demeaned = (xyz.T - datamean) - - # Do an SVD on the mean-centered data. - uu, dd, vv = np.linalg.svd(demeaned) - axis = vv[0] - xyz_straightened = np.atleast_2d(datamean).T + np.dot(demeaned, axis)*np.atleast_2d(axis).T - - if plot: - plt.close('all') - plt.imshow(img) - im_pts_ = cv2.projectPoints(xyz, rvec, tvec, K, dist)[0] - im_pts_ = np.squeeze(im_pts_).T - plt.plot(im_pts_[0], im_pts_[1], 'r.') - plt.plot(xy[0], xy[1], 'b.') - im_pts_ = cv2.projectPoints(xyz_straightened, rvec, tvec, K, dist)[0] - im_pts_ = np.squeeze(im_pts_).T - plt.plot(im_pts_[0], im_pts_[1], 'g.') - np.sqrt(np.sum((xy - im_pts_)**2, 0)) - - return xyz_straightened - - -# Make straight lines straight. -last_err = None -for _ in range(100): - xys = im_pts_spoof.copy().T - xyzs = enu_pts_spoof.copy().T - for xy in vert_lines + horz_lines: - xys = np.hstack([xys, xy]) - xyzs = np.hstack([xyzs, unproject_lines(xy, K, dist, rvec, tvec)]) - - - xyzs = np.array(xyzs) - xys = np.array(xys) - - - flags = cv2.CALIB_ZERO_TANGENT_DIST - flags = flags | cv2.CALIB_USE_INTRINSIC_GUESS - #flags = flags | cv2.CALIB_FIX_PRINCIPAL_POINT - #flags = flags | cv2.CALIB_FIX_ASPECT_RATIO - #flags = flags | cv2.CALIB_FIX_K1 - flags = flags | cv2.CALIB_FIX_K2 - flags = flags | cv2.CALIB_FIX_K3 - flags = flags | cv2.CALIB_FIX_K4 - flags = flags | cv2.CALIB_FIX_K5 - flags = flags | cv2.CALIB_FIX_K6 - ret = cv2.calibrateCamera([xyzs.T.astype(np.float32)], - [xys.T.astype(np.float32)], - (width, height), cameraMatrix=K.copy(), - distCoeffs=dist.copy(), flags=flags) - if last_err is not None and ret[0] > last_err: - break - - err, K, dist, rvecs, tvecs = ret - last_err = err - print(err) - rvec = rvecs[0].ravel() - tvec = tvecs[0].ravel() - R = cv2.Rodrigues(rvec)[0] - cam_pos = np.dot(R.T, -tvec) - - -if False: - im_pts_ = cv2.projectPoints(xyzs, rvec, tvec, K, dist)[0] - im_pts_ = np.squeeze(im_pts_).T - plt.imshow(img) - plt.plot(im_pts_[0], im_pts_[1], 'r.') - plt.plot(xys[0], xys[1], 'b.') - - -if False: - im_pts_ = cv2.projectPoints(enu_pts_spoof, rvec, tvec, K, dist)[0] - im_pts_ = np.squeeze(im_pts_).T - plt.imshow(img) - plt.plot(im_pts_[0], im_pts_[1], 'r.') - plt.plot(im_pts_spoof.T[0], im_pts_spoof.T[1], 'b.') - - -if False: - plt.close('all') - im_pts_ = cv2.projectPoints(enu_pts.T, rvec, tvec, K, dist)[0] - im_pts_ = np.squeeze(im_pts_).T - plt.imshow(img) - plt.plot(im_pts_[0], im_pts_[1], 'r.') - plt.plot(im_pts[0], im_pts[1], 'b.') -# ---------------------------------------------------------------------------- - - -# ------------------------------- First Pass --------------------------------- -def get_params_from_x(x): - rvec = x[:3] - tvec = x[3:6] - fx = abs(x[6]) - fy = abs(x[7]) - - dist = np.zeros(5) - if len(x) > 8: - dist[0] = x[8] - - if len(x) > 9: - dist[1] = x[9] - - if len(x) > 10: - dist[2] = x[10] - - R = cv2.Rodrigues(rvec)[0] - - K = np.identity(3) - K[0, 0] = fx - K[1, 1] = fy - K[0, 2] = img.shape[1]/2 - K[1, 2] = img.shape[0]/2 - - return K, rvec, R, tvec, dist - - -def error(x): - #print('x', x) - K, rvec, R, tvec, dist = get_params_from_x(x) - - cam_pos = np.dot(R.T, -tvec) - - # Project in truthed enu points. - im_pts_ = cv2.projectPoints(enu_pts.T, rvec, tvec, K, dist)[0] - im_pts_ = np.squeeze(im_pts_) - - ray_dir0 = (enu_pts.T - cam_pos).T - ray_dir0 /= np.sqrt(np.sum(ray_dir0**2, 0)) - ray_dir0 = np.dot(R, ray_dir0) - - if np.any(ray_dir0[2] < 0): - return 1e10 - - ray_dir = np.ones((3, len(im_pts_)), dtype=np.float) - ray_dir[:2] = np.squeeze(cv2.undistortPoints(im_pts_, K, dist, R=None), 1).T - ray_dir /= np.sqrt(np.sum(ray_dir**2, 0)) - - if np.any(np.sum(ray_dir * ray_dir0, axis=0) < 0.99): - return 1e10 - - err = np.mean(np.sqrt(np.sum((im_pts.T - im_pts_)**2, 1))) - - print('Reproj error:', err) - - #err *= 10 - - # Unproject rays into the camera coordinate system. - for xy in vert_lines: - ray_dir = np.ones((3, xy.shape[1]), dtype=np.float) - ray_dir0 = cv2.undistortPoints(xy.T, K, dist, R=None) - ray_dir[:2] = np.squeeze(ray_dir0, 1).T - ray_dir = np.dot(R.T, ray_dir) - ray_pos = ray_dir/np.sqrt(np.sum(ray_dir**2, 0)) - err -= sum(np.diff(ray_pos[2]))*1000 - - # Unproject rays into the camera coordinate system. - for xy in horz_lines: - ray_dir = np.ones((3, xy.shape[1]), dtype=np.float) - ray_dir0 = cv2.undistortPoints(xy.T, K, dist, R=None) - ray_dir[:2] = np.squeeze(ray_dir0, 1).T - ray_dir = np.dot(R.T, ray_dir) - - # we need to pick a set of point ranges so that all points end up at - # the same z value. The degenerate solution is that all are at range 0, - # so we force the first point to be at range 1. - ray_dir /= np.sqrt(np.sum(ray_dir**2, 0)) - - z = np.mean(ray_dir[2]) - d = z/ray_dir[2] - xyz = ray_dir*d - - # These should be lines - x, y = xyz[:2] - - p = np.polyfit(x, y, 1) - y2 = np.polyval(p, x) - - if False: - plt.plot(x, y, 'r.') - plt.plot(x, y2, 'b.') - - err += sum(abs(y - y2))*1e6 - - print(err) - return err - - -""" -best_err = np.inf -best_x = None -for _ in range(100): - rvec = np.random.rand(3) - - # Decide on a reasonable tvec. - R = cv2.Rodrigues(rvec)[0] - tvec = -np.dot(R, cam_pos) - - - tvec = enu_pts[0] + np.random.rand(3)*10 - - f = np.random.rand()*10000 - x = np.hstack([rvec, tvec, f]) - x = minimize(error, x).x - err = error(x) - if err < best_err: - best_err = err - best_x = x -""" - -#dist = np.zeros(5) -x = np.hstack([rvec.ravel(), tvec.ravel(), K[0, 0], K[1, 1], dist[0], dist[1]]) - -error(x) - -def err1(k1): - x_ = x.copy() - x_[-2] = k1 - - def err2(k2): - x__ = x_.copy() - x__[-1] = k2 - return error(x__) - - ret = fminbound(err2, -1000, 1000) - - return err2(ret) - -k1 = fminbound(err1, -10, 10) -x_ = x.copy(); x_[-2] = k1 - -if error(x) > error(x_): - x = x_ - -def err2(k2): - x_ = x.copy() - x_[-1] = k2 - return error(x_) - -k2 = fminbound(err2, -100, 100) -x_ = x.copy(); x_[-1] = k1 - -if error(x) > error(x_): - x = x_ - -K, rvec, R, tvec, dist = get_params_from_x(x) - -flags = cv2.CALIB_ZERO_TANGENT_DIST -flags = flags | cv2.CALIB_USE_INTRINSIC_GUESS -flags = flags | cv2.CALIB_FIX_PRINCIPAL_POINT -#flags = flags | cv2.CALIB_FIX_ASPECT_RATIO -flags = flags | cv2.CALIB_FIX_K1 -flags = flags | cv2.CALIB_FIX_K2 -flags = flags | cv2.CALIB_FIX_K3 -flags = flags | cv2.CALIB_FIX_K4 -flags = flags | cv2.CALIB_FIX_K5 -flags = flags | cv2.CALIB_FIX_K6 -ret = cv2.calibrateCamera([enu_pts_spoof.astype(np.float32)], - [im_pts_spoof.astype(np.float32)], - (width, height), cameraMatrix=K.copy(), - distCoeffs=dist.copy(), flags=flags) - -x_ = np.hstack([ret[3][0].ravel(), ret[4][0].ravel(), ret[1][0, 0], - ret[1][1, 1], ret[2][0], ret[2][1]]) -if error(x) > error(x_): - x = x_ - -print(error(x)) -K, rvec, R, tvec, dist = get_params_from_x(x) - - -if False: - for _ in range(10): - x = minimize(error, x).x - - -K, rvec, R, tvec, dist = get_params_from_x(x) -cam_pos = np.dot(R.T, -tvec) - -undistorted_frame = cv2.undistort(img, K, dist) -plt.imshow(undistorted_frame) - - -if False: - im_pts_ = cv2.projectPoints(enu_pts, rvec, tvec, K, dist)[0] - im_pts_ = np.squeeze(im_pts_).T - plt.imshow(img) - plt.plot(im_pts_[0], im_pts_[1], 'ro') - plt.plot(im_pts[0], im_pts[1], 'bo') - - -write_camera_krtd_file([K, R, tvec, dist], '%s/camera_model.krtd' % save_dir) - - -latitude, longitude, altitude = enu_to_llh(cam_pos[0], cam_pos[1], - cam_pos[2], mesh_lat0, - mesh_lon0, mesh_h0) - - -# Save camera model specification. Remembering that 'camera_models' considers -# quaterions to be coordinate system rotations not transformations. So, we have -# to invert the standard computer vision rotation matrix to create the -# orientation quaterions. -quat = quaternion_inverse(quaternion_from_matrix(R)) -pos = -np.dot(R.T, tvec).ravel() -platform_pose_provider = PlatformPoseFixed(pos, quat) -cm = StandardCamera(width, height, K, dist, [0, 0, 0], [0, 0, 0, 1], - platform_pose_provider) -depth = cam_depth_map_plane(cm, [0, 0, ground_elevation], [0, 0, -1]) -cm = DepthCamera(width, height, K, dist, [0, 0, 0], [0, 0, 0, 1], depth, - platform_pose_provider) - -# R currently is relative to ENU coordinate system at latitude0, -# longitude0, altitude0, but we need it to be relative to latitude, -# longitude, altitude. -Rp = np.dot(rmat_enu_ecef(mesh_lat0, mesh_lon0), - rmat_ecef_enu(latitude, longitude)) - -save_static_camera('%s/camera_model.yaml' % save_dir, height, width, K, dist, - np.dot(R, Rp), depth, latitude, longitude, altitude) - -x = np.linspace(0, width, width + 1) -y = np.linspace(0, height, height + 1) -x = (x[1:] + x[:-1])/2 -y = (y[1:] + y[:-1])/2 -X, Y = np.meshgrid(x, y) -im_pts = np.vstack([X.ravel(), Y.ravel()]) -xyz = cm.unproject_to_depth(im_pts) -E = xyz[0].reshape(X.shape) -N = xyz[1].reshape(X.shape) -U = xyz[2].reshape(X.shape) -ENU = np.dstack([E, N, U]).astype(np.float32) -filename = '%s/xyz_per_pixel.npy' % save_dir -np.save(filename, ENU, allow_pickle=False) \ No newline at end of file diff --git a/kamera/colmap_processing/scripts/calibrate_static_camera_from_llh_points.py b/kamera/colmap_processing/scripts/calibrate_static_camera_from_llh_points.py deleted file mode 100644 index 97833fe..0000000 --- a/kamera/colmap_processing/scripts/calibrate_static_camera_from_llh_points.py +++ /dev/null @@ -1,312 +0,0 @@ -#! /usr/bin/python -""" -ckwg +31 -Copyright 2018 by Kitware, Inc. -All rights reserved. - -Redistribution and use in source and binary forms, with or without -modification, are permitted provided that the following conditions are met: - - * Redistributions of source code must retain the above copyright notice, - this list of conditions and the following disclaimer. - - * Redistributions in binary form must reproduce the above copyright notice, - this list of conditions and the following disclaimer in the documentation - and/or other materials provided with the distribution. - - * Neither name of Kitware, Inc. nor the names of any contributors may be used - to endorse or promote products derived from this software without specific - prior written permission. - -THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS ``AS IS'' -AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHORS OR CONTRIBUTORS BE LIABLE FOR -ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - -============================================================================== - -""" -from __future__ import division, print_function -import numpy as np -import os -import cv2 -import subprocess -import matplotlib.pyplot as plt -import glob -import natsort -import math -import PIL -from osgeo import osr, gdal -from scipy.optimize import fmin, minimize, fminbound - -# Colmap Processing imports. -from colmap_processing.geo_conversions import llh_to_enu -from colmap_processing.colmap_interface import read_images_binary, Image, \ - read_points3D_binary, read_cameras_binary, qvec2rotmat -from colmap_processing.database import COLMAPDatabase, pair_id_to_image_ids, blob_to_array -import colmap_processing.vtk_util as vtk_util -from colmap_processing.geo_conversions import enu_to_llh, llh_to_enu, \ - rmat_ecef_enu, rmat_enu_ecef -from colmap_processing.static_camera_model import save_static_camera, \ - load_static_camera_from_file, write_camera_krtd_file - - -# ---------------------------------------------------------------------------- -# Reference image. -cm_dir = 'uav_video' -image_fname = '%s/ref_view.png' % cm_dir - -# File containing correspondences between image points (columns 0 an d1) and -# latitude (degrees), longtigude (degrees), and height above the WGS84 -# ellipsoid (meters) (columns 2-4). -points_fname = '%s/points.txt' % cm_dir - -save_dir = os.path.split(image_fname)[0] - -location = 'khq' - -# VTK renderings are limited to monitor resolution (width x height). -monitor_resolution = (1200, 1200) - -dist = [0, 0, 0, 0] -optimize_k1 = True -optimize_k2 = False -optimize_k3 = False -optimize_k4 = False -fix_principal_point = False -fix_aspect_ratio = True -# ---------------------------------------------------------------------------- - - -if location == 'khq': - # Meshed 3-D model used to render an synthetic view for sanity checking and - # to produce the depth map. - mesh_fname = 'mesh.ply' - mesh_lat0 = 42.86453893 # degrees - mesh_lon0 = -73.77125128 # degrees - mesh_h0 = 73 # meters above WGS84 ellipsoid - -else: - raise Exception('Unrecognized location \'%s\'' % location) - - -# Load in point correspondences. -ret = np.loadtxt(points_fname) -im_pts = ret[:, :2] -llh = ret[:, 2:] - -enu_pts = [llh_to_enu(_[0], _[1], _[2], mesh_lat0, mesh_lon0, mesh_h0) - for _ in llh] -enu_pts = np.array(enu_pts) - -# Load in the image. -real_image = cv2.imread(image_fname) - -if real_image.ndim == 3: - real_image = real_image[:, :, ::-1] - -height, width = real_image.shape[:2] - -dist = np.array(dist, dtype=np.float32) - -# ------------------------------- First Pass --------------------------------- -# Set optimization parameters for first pass. -flags = cv2.CALIB_ZERO_TANGENT_DIST -flags = flags | cv2.CALIB_USE_INTRINSIC_GUESS - -if fix_aspect_ratio: - flags = flags | cv2.CALIB_FIX_ASPECT_RATIO - -flags = flags | cv2.CALIB_FIX_PRINCIPAL_POINT -flags = flags | cv2.CALIB_FIX_K1 -flags = flags | cv2.CALIB_FIX_K2 -flags = flags | cv2.CALIB_FIX_K3 -flags = flags | cv2.CALIB_FIX_K4 -flags = flags | cv2.CALIB_FIX_K5 -flags = flags | cv2.CALIB_FIX_K6 -criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30000, - 0.0000001) - -# Try a bunch of different initial guesses for focal length. -results = [] -for f in np.logspace(0, 5, 100): - K = np.identity(3) - K[0, 2] = width/2 - K[1, 2] = height/2 - K[0, 0] = K[1, 1] = f - ret = cv2.calibrateCamera([enu_pts.astype(np.float32)], - [im_pts.astype(np.float32)], (width, height), - cameraMatrix=K, distCoeffs=dist, flags=flags, - criteria=criteria) - err, K, dist, rvecs, tvecs = ret - if K[0, 0] > 0 and K[1, 1] > 0: - results.append(ret) - -ind = np.argmin([_[0] for _ in results]) -err, K, dist, rvecs, tvecs = results[ind] -print('Error:', err) -rvec, tvec = rvecs[0], tvecs[0] -# ---------------------------------------------------------------------------- - - -# ------------------------------- Second Pass -------------------------------- -flags = cv2.CALIB_ZERO_TANGENT_DIST -flags = flags | cv2.CALIB_USE_INTRINSIC_GUESS - -if fix_aspect_ratio: - flags = flags | cv2.CALIB_FIX_ASPECT_RATIO - -flags = flags | cv2.CALIB_FIX_PRINCIPAL_POINT - -if not optimize_k1: - flags = flags | cv2.CALIB_FIX_K1 - -if not optimize_k2: - flags = flags | cv2.CALIB_FIX_K2 - -if not optimize_k3: - flags = flags | cv2.CALIB_FIX_K3 - -if not optimize_k4: - flags = flags | cv2.CALIB_FIX_K4 - -flags = flags | cv2.CALIB_FIX_K5 -flags = flags | cv2.CALIB_FIX_K6 - -criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30000, - 0.0000001) - -# Try a bunch of different initial guesses for focal length. -ret = cv2.calibrateCamera([enu_pts.astype(np.float32)], - [im_pts.astype(np.float32)], (width, height), - cameraMatrix=K, distCoeffs=dist, flags=flags, - criteria=criteria) -err, K, dist, rvecs, tvecs = ret -print('Error:', err) -rvec, tvec = rvecs[0], tvecs[0] -# ---------------------------------------------------------------------------- - - -# ------------------------------- Third Pass -------------------------------- -flags = cv2.CALIB_ZERO_TANGENT_DIST -flags = flags | cv2.CALIB_USE_INTRINSIC_GUESS - -if fix_aspect_ratio: - flags = flags | cv2.CALIB_FIX_ASPECT_RATIO - -if fix_principal_point: - flags = flags | cv2.CALIB_FIX_PRINCIPAL_POINT - -if not optimize_k1: - flags = flags | cv2.CALIB_FIX_K1 - -if not optimize_k2: - flags = flags | cv2.CALIB_FIX_K2 - -if not optimize_k3: - flags = flags | cv2.CALIB_FIX_K3 - -if not optimize_k4: - flags = flags | cv2.CALIB_FIX_K4 - -flags = flags | cv2.CALIB_FIX_K5 -flags = flags | cv2.CALIB_FIX_K6 - -criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30000, - 0.0000001) - -# Try a bunch of different initial guesses for focal length. -ret = cv2.calibrateCamera([enu_pts.astype(np.float32)], - [im_pts.astype(np.float32)], (width, height), - cameraMatrix=K, distCoeffs=dist, flags=flags, - criteria=criteria) -err, K, dist, rvecs, tvecs = ret -print('Error:', err) -rvec, tvec = rvecs[0], tvecs[0] -# ---------------------------------------------------------------------------- -plt.close('all') - - -def show_err(rvec, tvec, K, dist, plot=False): - im_pts_ = cv2.projectPoints(enu_pts, rvec, tvec, K, dist)[0] - im_pts_ = np.squeeze(im_pts_) - err = np.sqrt(np.sum((im_pts - im_pts_)**2, 1)) - err = np.mean(err) - - if plot: - plt.imshow(real_image) - plt.plot(im_pts.T[0], im_pts.T[1], 'bo') - plt.plot(im_pts_.T[0], im_pts_.T[1], 'ro') - for i in range(len(im_pts_)): - plt.plot([im_pts[i, 0], im_pts_[i, 0]], - [im_pts[i, 1], im_pts_[i, 1]], 'g-') - - return err - - -show_err(rvec, tvec, K, dist, plot=True) - - -R = cv2.Rodrigues(rvec)[0] -cam_pos = -np.dot(R.T, tvec).ravel() - - -# Read model into VTK. -try: - model_reader - assert prev_loaded_fname == mesh_fname -except: - model_reader = vtk_util.load_world_model(mesh_fname) - prev_loaded_fname = mesh_fname - - -# ---------------------------------------------------------------------------- - -clipping_range = [1, 2000] -ret = vtk_util.render_distored_image(width, height, K, dist, cam_pos, R, - model_reader, return_depth=True, - monitor_resolution=(1920, 1080), - clipping_range=clipping_range) -img, depth, E, N, U = ret - - -latitude, longitude, altitude = enu_to_llh(cam_pos[0], cam_pos[1], cam_pos[2], - mesh_lat0, mesh_lon0, mesh_h0) - -cv2.imwrite('%s/rendered_view.png' % save_dir, img[:, :, ::-1]) - -# R currently is relative to ENU coordinate system at latitude0, -# longitude0, altitude0, but we need it to be relative to latitude, -# longitude, altitude. -Rp = np.dot(rmat_enu_ecef(mesh_lat0, mesh_lon0), - rmat_ecef_enu(latitude, longitude)) - -filename = '%s/camera_model.yaml' % save_dir -save_static_camera(filename, height, width, K, dist, np.dot(R, Rp), - depth, latitude, longitude, altitude) - -# Save (x, y, z) meters per pixel. -if False: - filename = '%s/camera_model.krtd' % save_dir - tvec = -np.dot(R, cam_pos).ravel() - write_camera_krtd_file([K, R, tvec, dist], filename) - - ENU = np.dstack([E, N, U]).astype(np.float32) - filename = '%s/xyz_per_pixel.npy' % save_dir - np.save(filename, ENU, allow_pickle=False) - -depth_image = depth.copy() -depth_image[depth_image > clipping_range[1]*0.9] = 0 -depth_image -= depth_image.min() -depth_image /= depth_image.max()/255 -depth_image = np.round(depth_image).astype(np.uint8) - -depth_image = cv2.applyColorMap(depth_image, cv2.COLORMAP_JET) -cv2.imwrite('%s/depth_vizualization.png' % save_dir, - depth_image[:, :, ::-1]) \ No newline at end of file diff --git a/kamera/colmap_processing/scripts/calibrate_static_camera_from_reference.py b/kamera/colmap_processing/scripts/calibrate_static_camera_from_reference.py deleted file mode 100644 index ce4b367..0000000 --- a/kamera/colmap_processing/scripts/calibrate_static_camera_from_reference.py +++ /dev/null @@ -1,615 +0,0 @@ -#! /usr/bin/python -""" -ckwg +31 -Copyright 2018 by Kitware, Inc. -All rights reserved. - -Redistribution and use in source and binary forms, with or without -modification, are permitted provided that the following conditions are met: - - * Redistributions of source code must retain the above copyright notice, - this list of conditions and the following disclaimer. - - * Redistributions in binary form must reproduce the above copyright notice, - this list of conditions and the following disclaimer in the documentation - and/or other materials provided with the distribution. - - * Neither name of Kitware, Inc. nor the names of any contributors may be used - to endorse or promote products derived from this software without specific - prior written permission. - -THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS ``AS IS'' -AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHORS OR CONTRIBUTORS BE LIABLE FOR -ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - -============================================================================== - -""" -from __future__ import division, print_function -import numpy as np -import os -import cv2 -import subprocess -import matplotlib.pyplot as plt -import glob -import natsort -import trimesh -import math -import PIL -from osgeo import osr, gdal -from scipy.optimize import fmin, minimize, fminbound - -# Colmap Processing imports. -from colmap_processing.geo_conversions import llh_to_enu -from colmap_processing.colmap_interface import read_images_binary, Image, \ - read_points3D_binary, read_cameras_binary, qvec2rotmat -from colmap_processing.database import COLMAPDatabase, pair_id_to_image_ids, blob_to_array -import colmap_processing.vtk_util as vtk_util -from colmap_processing.geo_conversions import enu_to_llh, llh_to_enu, \ - rmat_ecef_enu, rmat_enu_ecef -from colmap_processing.static_camera_model import save_static_camera, \ - load_static_camera_from_file, write_camera_krtd_file - - -# ---------------------------------------------------------------------------- -save_dir = 'cameras' - -# Base path to the colmap directory. -image_dir = 'collect' - -# Image to turn into a static camera. -image_fname = '50.png' - -# Path to the images.bin file. -images_bin_fname = '%s/images.bin' % image_dir -camera_bin_fname = '%s/cameras.bin' % image_dir -points_3d_bin_fname = '%s/points3D.bin' % image_dir - -if True: - # Cville. - mesh_fname = 'coarse.ply' - - H_enu = np.identity(4) - latitude0 = 0 # degrees - longitude0 = 0 # degrees - altitude0 = 0 # meters above WGS84 ellipsoid -# ---------------------------------------------------------------------------- - - -# Read model into VTK. -model_reader = vtk_util.load_world_model(mesh_fname) - -# Read in the details of all images. -images = read_images_binary(images_bin_fname) -cameras = read_cameras_binary(camera_bin_fname) -pts_3d = read_points3D_binary(points_3d_bin_fname) - - -def get_xyz_from_image_pt(image_id, im_pt): - """Return model 3-D point from image index and image coordinate of point. - - """ - image = images[image_id] - ind0 = [_ for _ in range(len(image.xys)) if image.point3D_ids[_] != -1] - xys = image.xys[ind0] - d = np.sqrt(np.sum((xys - np.atleast_2d(im_pt))**2, 1)) - ind1 = np.argmin(d) - - print('Distance to select point:', d[ind1], 'pixels') - if d[ind1] > 10: - return None - - xyz = pts_3d[image.point3D_ids[ind0[ind1]]].xyz - xyz = np.hstack([xyz, 1]) - xyz = np.dot(H_enu, xyz) - xyz = xyz[:3]/xyz[3] - - return xyz - - -def process(image_fname, ref_image_id, points_fname, save_dir, K, - dist=[0, 0, 0, 0,], optimize_k1=False, optimize_k2=False, - optimize_k3=False, optimize_k4=False): - - - colmap_image = images[ref_image_id] - camera = cameras[colmap_image.camera_id] - - point_pairs = np.loadtxt(points_fname) - - if False: - point_pairs = point_pairs[3:] - - ref_pts = point_pairs[:, :2] - im_pts = point_pairs[:, 2:].astype(np.float32) - - enu = [get_xyz_from_image_pt(ref_image_id, im_pt) for im_pt in ref_pts] - enu = np.array(enu).astype(np.float32) - - calibrate_from_enu_pts(image_fname, im_pts, enu, save_dir, K, dist=dist, - optimize_k1=optimize_k1, optimize_k2=optimize_k2, - optimize_k3=optimize_k3, optimize_k4=optimize_k4) - - -def calibrate_from_llh_file(image_fname, points_fname, save_dir, - K, dist=[0, 0, 0, 0], optimize_k1=False, - optimize_k2=False, optimize_k3=False, - optimize_k4=False, fix_principal_point=True): - ret = np.loadtxt(points_fname) - im_pts = ret[:, :2] - llh = ret[:, 2:] - enu = [llh_to_enu(_[0], _[1], _[2], latitude0, longitude0, altitude0) - for _ in llh] - enu = np.array(enu) - calibrate_from_enu_pts(image_fname, im_pts, enu, save_dir, K, dist=dist, - optimize_k1=optimize_k1, optimize_k2=optimize_k2, - optimize_k3=optimize_k3, optimize_k4=optimize_k4, - fix_principal_point=fix_principal_point) - - -def calibrate_manual_clicked_reference(image_fname, points_fname, - ref_camera_fname, save_dir, - dist=[0, 0, 0, 0], optimize_k1=False, - optimize_k2=False, optimize_k3=False, - optimize_k4=False, - fix_principal_point=True): - """points_fname from reference to image. - - """ - ret = np.loadtxt(points_fname) - ref_pts = ret[:, :2] - im_pts = ret[:, 2:] - - ret = load_static_camera_from_file(ref_camera_fname) - K_ref, dist_ref, R, depth_map, latitude, longitude, altitude = ret[2:] - height, width = depth_map.shape - - enu0 = llh_to_enu(latitude, longitude, altitude, latitude0, longitude0, - altitude0) - enu0 = np.array(enu0) - enu = np.zeros((len(ref_pts), 3)) - - # Unproject rays into the camera coordinate system. - ray_dir = np.ones((3, len(ref_pts)), dtype=np.float) - ray_dir0 = cv2.undistortPoints(np.expand_dims(ref_pts, 0), - K_ref, dist_ref, R=None) - ray_dir[:2] = np.squeeze(ray_dir0, 1).T - - # Rotate rays into the local east/north/up coordinate system. - ray_dir = np.dot(R.T, ray_dir) - #ray_dir /= np.sqrt(np.sum(ray_dir**2, 0)) - - for i in range(ref_pts.shape[0]): - x, y = ref_pts[i] - if x == 0: - ix = 0 - elif x == width: - ix = int(width - 1) - else: - ix = int(round(x - 0.5)) - - if y == 0: - iy = 0 - elif y == height: - iy = int(height - 1) - else: - iy = int(round(y - 0.5)) - - if ix < 0 or iy < 0 or ix >= width or iy >= height: - print(x == width) - print(y == height) - raise ValueError('Coordinates (%0.1f,%0.f) are outside the ' - '%ix%i image' % (x, y, width, height)) - - enu[i] = enu0 + ray_dir[:, i]*depth_map[iy, ix] - - calibrate_from_enu_pts(image_fname, im_pts, enu, save_dir, K_ref, - dist=dist, optimize_k1=optimize_k1, - optimize_k2=optimize_k2, optimize_k3=optimize_k3, - optimize_k4=optimize_k4, - fix_principal_point=fix_principal_point) - - -def calibrate_from_enu_pts(image_fname, im_pts, enu, save_dir, K, - dist=[0, 0, 0, 0], optimize_k1=False, - optimize_k2=False, optimize_k3=False, - optimize_k4=False, fix_principal_point=True): - dist = np.array(dist, dtype=np.float32) - real_image = cv2.imread(image_fname) - - if real_image.ndim == 3: - real_image = real_image[:, :, ::-1] - - height, width = real_image.shape[:2] - - K[0, 2] = width/2 - K[1, 2] = height/2 - - if False: - ref_image = cv2.imread(ref_image_fname)[:, :, ::-1] - plt.figure(); plt.imshow(ref_image) - plt.plot(ref_pts.T[0], ref_pts.T[1], 'bo') - plt.figure(); plt.imshow(real_image) - plt.plot(im_pts.T[0], im_pts.T[1], 'bo') - - if False: - from mpl_toolkits.mplot3d import Axes3D - fig = plt.figure() - fig.add_subplot(111, projection='3d') - plt.plot(enu[:,0], enu[:,1], enu[:,2], 'ro') - plt.xlabel('X Axis') - plt.ylabel('Y Axis') - - - def show_err(rvec, tvec, k, dist, plot=False): - im_pts_ = cv2.projectPoints(enu, rvec, tvec, k, dist)[0] - im_pts_ = np.squeeze(im_pts_) - err = np.sqrt(np.sum((im_pts - im_pts_)**2, 1)) - err = np.mean(err) - - if plot: - plt.imshow(real_image) - plt.plot(im_pts.T[0], im_pts.T[1], 'bo') - plt.plot(im_pts_.T[0], im_pts_.T[1], 'ro') - for i in range(len(im_pts_)): - plt.plot([im_pts[i, 0], im_pts_[i, 0]], - [im_pts[i, 1], im_pts_[i, 1]], 'g-') - - return err - - - if False: - # First pass. - flags = cv2.CALIB_ZERO_TANGENT_DIST - flags = flags | cv2.CALIB_USE_INTRINSIC_GUESS - flags = flags | cv2.CALIB_FIX_PRINCIPAL_POINT - flags = flags | cv2.CALIB_FIX_ASPECT_RATIO - flags = flags | cv2.CALIB_FIX_K1 - flags = flags | cv2.CALIB_FIX_K2 - flags = flags | cv2.CALIB_FIX_K3 - flags = flags | cv2.CALIB_FIX_K4 - flags = flags | cv2.CALIB_FIX_K5 - flags = flags | cv2.CALIB_FIX_K6 - criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30000, - 0.0000001) - ret = cv2.calibrateCamera([enu.astype(np.float32)], - [im_pts.astype(np.float32)], (width, height), - cameraMatrix=K, distCoeffs=dist, flags=flags, - criteria=criteria) - err, K, dist, rvecs, tvecs = ret - print('First pass error:', err) - rvec, tvec = rvecs[0], tvecs[0] - - # Second pass. - flags = cv2.CALIB_ZERO_TANGENT_DIST - flags = flags | cv2.CALIB_USE_INTRINSIC_GUESS - flags = flags | cv2.CALIB_FIX_PRINCIPAL_POINT - flags = flags | cv2.CALIB_FIX_K1 - flags = flags | cv2.CALIB_FIX_K2 - flags = flags | cv2.CALIB_FIX_K3 - flags = flags | cv2.CALIB_FIX_K4 - flags = flags | cv2.CALIB_FIX_K5 - flags = flags | cv2.CALIB_FIX_K6 - - ret = cv2.calibrateCamera([enu.astype(np.float32)], - [im_pts.astype(np.float32)], - (width, height), cameraMatrix=K, - distCoeffs=dist, flags=flags) - err, K, dist, rvecs, tvecs = ret - print('Second pass error:', err) - rvec, tvec = rvecs[0], tvecs[0] - - #if optimize_k1 or optimize_k2 or optimize_k3 or optimize_k4: - if True: - # Third pass. - flags = cv2.CALIB_ZERO_TANGENT_DIST - flags = flags | cv2.CALIB_USE_INTRINSIC_GUESS - - if fix_principal_point: - flags = flags | cv2.CALIB_FIX_PRINCIPAL_POINT - - if not optimize_k1: - flags = flags | cv2.CALIB_FIX_K1 - - if not optimize_k2: - flags = flags | cv2.CALIB_FIX_K2 - - if not optimize_k3: - flags = flags | cv2.CALIB_FIX_K3 - - if not optimize_k4: - flags = flags | cv2.CALIB_FIX_K4 - - flags = flags | cv2.CALIB_FIX_K5 - flags = flags | cv2.CALIB_FIX_K6 - - ret = cv2.calibrateCamera([enu.astype(np.float32)], - [im_pts.astype(np.float32)], - (width, height), cameraMatrix=K, - distCoeffs=dist, flags=flags) - err, K, dist, rvecs, tvecs = ret - print('Third pass error:', err) - rvec, tvec = rvecs[0], tvecs[0] - - - show_err(rvec, tvec, K, dist, plot=True) - - R = cv2.Rodrigues(rvec)[0] - cam_pos = -np.dot(R.T, tvec).ravel() - - save_camera_model(width, height, K, dist, cam_pos, R, real_image, save_dir, - mode=2) - # ------------------------------------------------------------------------ - - -def get_features(image, num_features=10000): - # Find the keypoints and descriptors and match them. - orb = cv2.ORB_create(nfeatures=num_features, edgeThreshold=25, - patchSize=31, nlevels=16, - scoreType=cv2.ORB_FAST_SCORE, fastThreshold=10) - - if image.ndim == 3: - img_gray = cv2.cvtColor(image, cv2.COLOR_RGB2GRAY) - else: - img_gray = image - - img_gray = img_gray.astype(np.float) - img_gray -= np.percentile(img_gray.ravel(), 1) - img_gray[img_gray < 0] = 0 - img_gray /= np.percentile(img_gray.ravel(), 99)/255 - img_gray[img_gray > 225] = 255 - img_gray = np.round(img_gray).astype(np.uint8) - - return orb.detectAndCompute(img_gray, None) - # ------------------------------------------------------------------------ - - -def process_auto(image_fname, ref_camera_fname, save_dir, optimize_k1=False, - optimize_k2=False, optimize_k3=False, optimize_k4=False, - fix_principal_point=True, num_features=10000, homog_thresh=20, - final_thresh=5): - ret = load_static_camera_from_file(ref_camera_fname) - K, dist, R, depth_map, latitude, longitude, altitude = ret[2:] - - real_image = cv2.imread(image_fname) - - height, width = real_image.shape[:2] - - if real_image.ndim == 3: - real_image = real_image[:, :, ::-1] - - ref_image = cv2.imread('%s/ref_view.png' % - os.path.split(ref_camera_fname)[0]) - - if ref_image.ndim == 3: - ref_image = ref_image[:, :, ::-1] - - kp0, des0 = get_features(ref_image, num_features=num_features) - kp1, des1 = get_features(real_image, num_featuresref_camera_fname=num_features) - - bf = cv2.BFMatcher(cv2.NORM_HAMMING, crossCheck=True) - matches = bf.match(des0, des1) - - pts0 = np.float32([kp0[m.queryIdx].pt for m in matches]) - pts1 = np.float32([kp1[m.trainIdx].pt for m in matches]) - - print('Feature matcher found %i matches' % len(matches)) - - if False: - mask = np.logical_and(pts0[:, 1] > 355, - pts0[:, 1] < 649) - pts0 = pts0[mask] - pts1 = pts1[mask] - mask = np.logical_and(pts1[:, 1] > 355, - pts1[:, 1] < 648) - pts0 = pts0[mask] - pts1 = pts1[mask] - - M, mask = cv2.findHomography(pts0, pts1, cv2.RANSAC, homog_thresh) - mask = np.squeeze(mask) > 0 - - pts0 = pts0[mask] - pts1 = pts1[mask] - - print('Homography filtering yielded %i matches' % len(pts0)) - - ray_dir = cv2.undistortPoints(np.expand_dims(pts0, 0), K, dist, None) - ray_dir = np.squeeze(ray_dir, 0).astype(np.float32).T - ray_dir = np.vstack([ray_dir, np.ones(ray_dir.shape[1])]) - - ray_dir = np.dot(R.T, ray_dir) - - d = [] - for pt in pts0: - ix, iy = np.round(pt - 0.5).astype(np.int) - d.append(depth_map[iy, ix]) - - ray_dir = ray_dir*d - - cam_pos = llh_to_enu(latitude, longitude, altitude, latitude0, longitude0, - altitude0) - enu = ray_dir.T + np.atleast_2d(cam_pos) - - if False: - from mpl_toolkits.mplot3d import Axes3D - fig = plt.figure() - fig.add_subplot(111, projection='3d') - plt.plot(enu[:,0], enu[:,1], enu[:,2], 'ro') - plt.xlabel('X Axis') - plt.ylabel('Y Axis') - - # First pass. - K[0, 2] = width/2 - K[1, 2] = height/2 - flags = cv2.CALIB_ZERO_TANGENT_DIST - flags = flags | cv2.CALIB_USE_INTRINSIC_GUESS - flags = flags | cv2.CALIB_FIX_PRINCIPAL_POINT - flags = flags | cv2.CALIB_FIX_K1 - flags = flags | cv2.CALIB_FIX_K2 - flags = flags | cv2.CALIB_FIX_K3 - flags = flags | cv2.CALIB_FIX_K4 - flags = flags | cv2.CALIB_FIX_K5 - flags = flags | cv2.CALIB_FIX_K6 - - while True: - ret = cv2.calibrateCamera([enu.astype(np.float32)], - [pts1.astype(np.float32)], - (width, height), cameraMatrix=K, - distCoeffs=dist, flags=flags) - err, K, dist, rvecs, tvecs = ret - print('Focal lengths', K[0, 0], K[1, 1]) - print('Second pass error:', err) - rvec, tvec = rvecs[0], tvecs[0] - - im_pts_ = cv2.projectPoints(enu, rvec, tvec, K, dist)[0] - im_pts_ = np.squeeze(im_pts_) - err = np.sqrt(np.sum((pts1 - im_pts_)**2, 1)) - - mask = err < max(100, np.percentile(err, 50)) - - if np.all(mask): - break - - enu = enu[mask] - pts0 = pts0[mask] - pts1 = pts1[mask] - - print('Calibration filtering yielded %i matches' % len(pts0)) - - # Second pass. - flags = cv2.CALIB_ZERO_TANGENT_DIST - flags = flags | cv2.CALIB_USE_INTRINSIC_GUESS - - if fix_principal_point: - flags = flags | cv2.CALIB_FIX_PRINCIPAL_POINT - - if not optimize_k1: - flags = flags | cv2.CALIB_FIX_K1 - - if not optimize_k2: - flags = flags | cv2.CALIB_FIX_K2 - - if not optimize_k3: - flags = flags | cv2.CALIB_FIX_K3 - - if not optimize_k4: - flags = flags | cv2.CALIB_FIX_K4 - - flags = flags | cv2.CALIB_FIX_K5 - flags = flags | cv2.CALIB_FIX_K6 - - while True: - ret = cv2.calibrateCamera([enu.astype(np.float32)], - [pts1.astype(np.float32)], - (width, height), cameraMatrix=K, - distCoeffs=dist, flags=flags) - err, K, dist, rvecs, tvecs = ret - print('Second pass error:', err) - rvec, tvec = rvecs[0], tvecs[0] - - im_pts_ = cv2.projectPoints(enu, rvec, tvec, K, dist)[0] - im_pts_ = np.squeeze(im_pts_) - err = np.sqrt(np.sum((pts1 - im_pts_)**2, 1)) - - mask = err < final_thresh - - if np.all(mask): - break - - enu = enu[mask] - pts0 = pts0[mask] - pts1 = pts1[mask] - - R = cv2.Rodrigues(rvec)[0] - cam_pos = -np.dot(R.T, tvec).ravel() - - if True: - plt.figure() - plt.subplot('211') - plt.imshow(ref_image, cmap='gray', interpolation='none') - plt.plot(pts0.T[0], pts0.T[1], 'ro') - plt.subplot('212') - plt.imshow(real_image, cmap='gray', interpolation='none') - plt.plot(pts1.T[0], pts1.T[1], 'bo') - - if False: - save_camera_model(monitor_resolution[0], monitor_resolution[1], K, - np.zeros(4), cam_pos, R, real_image, save_dir, - mode=1) - - save_camera_model(width, height, K, dist, cam_pos, R, real_image, save_dir, - mode=1) - - -def save_camera_model(width, height, K, dist, cam_pos, R, real_image, - save_dir, mode=1, scale=0.5): - ret = vtk_util.render_distored_image(width, height, K, dist, cam_pos, R, - model_reader, return_depth=True, - monitor_resolution=(1080, 1080)) - img, depth, E, N, U = ret - - # ------------------------------------------------------------------------ - - try: - os.makedirs(save_dir) - except OSError: - pass - - cv2.imwrite('%s/ref_view.png' % save_dir, real_image[:, :, ::-1]) - - # Read and undistort image. - cv2.imwrite('%s/rendered_view.jpg' % save_dir, img[:, :, ::-1]) - # ------------------------------------------------------------------- - - latitude, longitude, altitude = enu_to_llh(cam_pos[0], cam_pos[1], - cam_pos[2], latitude0, - longitude0, altitude0) - - # R currently is relative to ENU coordinate system at latitude0, - # longitude0, altitude0, but we need it to be relative to latitude, - # longitude, altitude. - Rp = np.dot(rmat_enu_ecef(latitude0, longitude0), - rmat_ecef_enu(latitude, longitude)) - - filename = '%s/camera_model.yaml' % save_dir - R2 = np.dot(R, Rp) - save_static_camera(filename, height, width, K, dist, R2, - depth, latitude, longitude, altitude) - - filename = '%s/camera_model.krtd' % save_dir - tvec = -np.dot(R, cam_pos).ravel() - write_camera_krtd_file([K, R2, tvec, dist], filename) - - # Save (x, y, z) meters per pixel. - ENU = np.dstack([E, N, U]).astype(np.float32) - filename = '%s/xyz_per_pixel.npy' % save_dir - np.save(filename, ENU, allow_pickle=False) - - depth_image = depth.copy() - depth_image[depth_image > clipping_range[1]*0.9] = 0 - depth_image -= depth_image.min() - depth_image /= depth_image.max()/255 - depth_image = np.round(depth_image).astype(np.uint8) - - depth_image = cv2.applyColorMap(depth_image, cv2.COLORMAP_JET) - cv2.imwrite('%s/depth_vizualization.png' % save_dir, - depth_image[:, :, ::-1]) - - -# points_1667_to_2018-05-14.20-25-00.20-30-00.hospital.G301 -cam_name = 'cam' -ref_image_id = 1 -save_dir_ = '%s/%s' % (save_dir, cam_name) -points_fname = 'points.txt' -image_fname = '%s.jpg' % cam_name -ref_image_fname = '%s.jpg' % ref_image_id -process(image_fname, ref_image_id, points_fname, save_dir_, K=K, - dist=None, optimize_k1=False, optimize_k2=False, optimize_k3=False, - optimize_k4=False) diff --git a/kamera/colmap_processing/scripts/cam_view.py b/kamera/colmap_processing/scripts/cam_view.py deleted file mode 100755 index 5003810..0000000 --- a/kamera/colmap_processing/scripts/cam_view.py +++ /dev/null @@ -1,93 +0,0 @@ -#!/usr/bin/env python -# ckwg +31 -# Copyright 2021 by Kitware, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions are met: -# -# * Redistributions of source code must retain the above copyright notice, -# this list of conditions and the following disclaimer. -# -# * Redistributions in binary form must reproduce the above copyright notice, -# this list of conditions and the following disclaimer in the documentation -# and/or other materials provided with the distribution. -# -# * Neither name of Kitware, Inc. nor the names of any contributors may be used -# to endorse or promote products derived from this software without specific -# prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS ``AS IS'' -# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -# ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHORS OR CONTRIBUTORS BE LIABLE FOR -# ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -# DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -# SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -# OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -''' -Load and print camera parameters from YAML, and optionally render the view given a 3D model in PLY format. -''' -import logging, sys, numpy as np -import matplotlib.pyplot as plt - -def run(args): -### load camera - CamMdlFN = args.input_cam - from colmap_processing.static_camera_model import load_static_camera_from_file - height, width, K, dist, R, depth_map, latitude, longitude, altitude = load_static_camera_from_file(CamMdlFN) -### print camera parameters - print(CamMdlFN) - print('W,H={}'.format([width, height])) - np.savetxt(sys.stdout, K, '%g', '\t', header='K') - vFoV = np.arctan2(height/2, K[1,1])*360/np.pi - print('vFoV={}'.format(vFoV)) - np.savetxt(sys.stdout, R, '%g', '\t', header='R') - print('LLH={}'.format([latitude, longitude, altitude])) - mesh_lat0, mesh_lon0, mesh_h0 = args.LLH0 - from colmap_processing.geo_conversions import llh_to_enu - cam_pos = llh_to_enu(latitude, longitude, altitude, mesh_lat0, mesh_lon0, mesh_h0) - print('cam_pos={}'.format(cam_pos)) - CCCP = np.eye(4) - CCCP[:3,:4] = np.array([R[0],-R[1],-R[2], cam_pos]).T - np.savetxt(sys.stdout, CCCP, '%g', '\t', header='CloudCompare camera pose') - if not args.visual: return 0 -### VTK rendering: limited to monitor resolution (width x height) - monitor_resolution = (1920, 1080) # TODO: param/config - import colmap_processing.vtk_util as vtk_util - model_reader = vtk_util.load_world_model(args.input_mesh) -### render camera view - render_resolution = list(monitor_resolution) - vtk_camera = vtk_util.Camera(render_resolution[0], render_resolution[1], vFoV, cam_pos, R) - img = vtk_camera.render_image(model_reader) - plt.imshow(img) - plt.show() -### save camera view to an image file - import os - CamViewFN = os.path.splitext(CamMdlFN)[0]+'.vtk.jpg' - import cv2 as cv - cv.imwrite(CamViewFN, img[:,:,::-1]) - -def CLI(argv=None): - import argparse - MeshFN = '../data/mesh.ply' - CamModelFN = '../000128.cam.yml' - # LLH0NSR = np.array([42.43722062, -84.02781521, 251.412]) # North Star Reach origin in LLH - LLH0NSR = np.array([42.86453893, -73.77125128, 73]) # KHQ - parser = argparse.ArgumentParser(description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter) - parser.add_argument('-i:c', '--input_cam', metavar='path', default=CamModelFN, - help='path/to/input/camera/model.yml; default=%(default)s') - parser.add_argument('-i:m', '--input_mesh', metavar='path', default=MeshFN, - help='path/to/input/mesh.ply; default=%(default)s') - parser.add_argument('-l', '--log', metavar='level', choices=['CRITICAL', 'ERROR', 'WARNING', 'INFO', 'DEBUG', 'NOTSET'], - default='WARNING', help='logging verbosity level: %(choices)s; default=%(default)s') - parser.add_argument('-o', '--LLH0', metavar='value', default=LLH0NSR, - help='assumed local GPS origin as an explicit vector [latitude,longitude,altitude]; default=%(default)s') - parser.add_argument('-v', '--visual', action='store_true', help='display/save visuals') - args = parser.parse_args(argv) - logging.basicConfig(level=args.log) - run(args) - -if __name__ == '__main__': CLI() diff --git a/kamera/colmap_processing/scripts/camera_pair_test.py b/kamera/colmap_processing/scripts/camera_pair_test.py deleted file mode 100644 index ab0b837..0000000 --- a/kamera/colmap_processing/scripts/camera_pair_test.py +++ /dev/null @@ -1,158 +0,0 @@ -#! /usr/bin/python -""" -ckwg +31 -Copyright 2018 by Kitware, Inc. -All rights reserved. - -Redistribution and use in source and binary forms, with or without -modification, are permitted provided that the following conditions are met: - - * Redistributions of source code must retain the above copyright notice, - this list of conditions and the following disclaimer. - - * Redistributions in binary form must reproduce the above copyright notice, - this list of conditions and the following disclaimer in the documentation - and/or other materials provided with the distribution. - - * Neither name of Kitware, Inc. nor the names of any contributors may be used - to endorse or promote products derived from this software without specific - prior written permission. - -THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS ``AS IS'' -AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHORS OR CONTRIBUTORS BE LIABLE FOR -ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - -============================================================================== - -""" -from __future__ import division, print_function -import numpy as np -import os -import cv2 -import subprocess -import matplotlib.pyplot as plt -import glob -import natsort -import trimesh -import math -import PIL -from osgeo import osr, gdal -from scipy.optimize import fmin, minimize, fminbound - -# Colmap Processing imports. -from colmap_processing.geo_conversions import llh_to_enu -import colmap_processing.vtk_util as vtk_util -from colmap_processing.geo_conversions import enu_to_llh, llh_to_enu, \ - rmat_ecef_enu, rmat_enu_ecef -from colmap_processing.static_camera_model import save_static_camera, \ - load_static_camera_from_file, write_camera_krtd_file, project_to_camera, \ - unproject_from_camera - - -# ---------------------------------------------------------------------------- -camera_model1_fname = 'camera_model.yaml' -camera_model2_fname = 'camera_model.yaml' -ponts_fname = 'points.txt' - -#MUTC. -mesh_fname = '/home/c.ply' - -latitude0 = 0 # degrees -longitude0 = 0 # degrees -altitude0 = 0 # meters above WGS84 ellipsoid - -im_pts1 = np.loadtxt(ponts_fname)[:, :2] -# ---------------------------------------------------------------------------- - - -img1 = cv2.imread(source_img)[:, :, ::-1] -img2 = cv2.imread(dest_img)[:, :, ::-1] - - -# Read model into VTK. -model_reader = vtk_util.load_world_model(mesh_fname) - -# Load camera models. -ret = load_static_camera_from_file(camera_model1_fname) -height1, width1, K1, dist1, R1, depth_map1, latitude1, longitude1, altitude1 = ret -cam_pos1 = llh_to_enu(latitude1, longitude1, altitude1, latitude0, longitude0, - altitude0) -dist1[:] = 0 - -ret = load_static_camera_from_file(camera_model2_fname) -height2, width2, K2, dist2, R2, depth_map2, latitude2, longitude2, altitude2 = ret -cam_pos2 = llh_to_enu(latitude2, longitude2, altitude2, latitude0, longitude0, - altitude0) -dist2[:] = 0 - -# Render views from each camera. -clipping_range = [18, 234] -ret = vtk_util.render_distored_image(width1, height1, K1, dist1, cam_pos1, R1, - model_reader, return_depth=True, - monitor_resolution=(1920, 1080), - clipping_range=clipping_range) -img1, depth1, E1, N1, U1 = ret - -clipping_range = [32, 267] -ret = vtk_util.render_distored_image(width2, height2, K2, dist2, cam_pos2, R2, - model_reader, return_depth=True, - monitor_resolution=(1920, 1080), - clipping_range=clipping_range) -img2, depth2, E2, N, U2 = ret - - -if False: - wrld_pts = np.array([[-15.576996, -81.941727, 1.275864], - [-15.322388, -82.196335, 1.274765], - [-15.322388, -81.941727, 1.271240]]).T - im_pts1 = project_to_camera(wrld_pts, K1, dist1, R1, cam_pos1) - im_pts2 = project_to_camera(wrld_pts, K2, dist2, R2, cam_pos2) - - plt.figure() - plt.imshow(img1) - plt.plot(im_pts1[:, 0], im_pts1[:, 1], 'go') - - plt.figure() - plt.imshow(img2) - plt.plot(im_pts2[:, 0], im_pts2[:, 1], 'go') - - - wrld_pts1 = unproject_from_camera_embree(im_pts, K, dist, R, cam_pos, - embree_scene) - print(wrld_pts1 - wrld_pts) - - - wrld_pts1 = unproject_from_camera(im_pts1, K1, dist1, R1, cam_pos1, - depth_map1) - print(wrld_pts1 - wrld_pts) - wrld_pts2 = unproject_from_camera(im_pts2, K2, dist2, R2, cam_pos2, - depth_map2) - -plt.figure() -plt.imshow(img1) -plt.plot(im_pts1[:, 0], im_pts1[:, 1], 'go') - -plt.figure() -plt.imshow(img2) -pts = [] -for s in np.linspace(0.999, 1.001, 3): - wrld_pts1 = unproject_from_camera(im_pts1, K1, dist1, R1, cam_pos1, - depth_map1*s) - im_pts2 = project_to_camera(wrld_pts1, K2, dist2, R2, cam_pos2) - pts.append(im_pts2) - -pts = np.array(pts) - -for i in range(pts.shape[1]): - im_pts2 = pts[:, i, :] - plt.plot(im_pts2[:, 0], im_pts2[:, 1], 'r-') - -plt.xlim([0, width2]) -plt.ylim([height2, 0]) \ No newline at end of file diff --git a/kamera/colmap_processing/scripts/colmap_manual_registration.py b/kamera/colmap_processing/scripts/colmap_manual_registration.py deleted file mode 100644 index b4085ce..0000000 --- a/kamera/colmap_processing/scripts/colmap_manual_registration.py +++ /dev/null @@ -1,416 +0,0 @@ -#! /usr/bin/python -""" -ckwg +31 -Copyright 2018 by Kitware, Inc. -All rights reserved. - -Redistribution and use in source and binary forms, with or without -modification, are permitted provided that the following conditions are met: - - * Redistributions of source code must retain the above copyright notice, - this list of conditions and the following disclaimer. - - * Redistributions in binary form must reproduce the above copyright notice, - this list of conditions and the following disclaimer in the documentation - and/or other materials provided with the distribution. - - * Neither name of Kitware, Inc. nor the names of any contributors may be used - to endorse or promote products derived from this software without specific - prior written permission. - -THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS ``AS IS'' -AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHORS OR CONTRIBUTORS BE LIABLE FOR -ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - -============================================================================== - -""" -import sys -import sqlite3 -import numpy as np -import glob -import os -from scipy.spatial import distance_matrix -import cv2 -import matplotlib.pyplot as plt -from shutil import copyfile - - -IS_PYTHON3 = sys.version_info[0] >= 3 - -MAX_IMAGE_ID = 2**31 - 1 - -CREATE_CAMERAS_TABLE = """CREATE TABLE IF NOT EXISTS cameras ( - camera_id INTEGER PRIMARY KEY AUTOINCREMENT NOT NULL, - model INTEGER NOT NULL, - width INTEGER NOT NULL, - height INTEGER NOT NULL, - params BLOB, - prior_focal_length INTEGER NOT NULL)""" - -CREATE_DESCRIPTORS_TABLE = """CREATE TABLE IF NOT EXISTS descriptors ( - image_id INTEGER PRIMARY KEY NOT NULL, - rows INTEGER NOT NULL, - cols INTEGER NOT NULL, - data BLOB, - FOREIGN KEY(image_id) REFERENCES images(image_id) ON DELETE CASCADE)""" - -CREATE_IMAGES_TABLE = """CREATE TABLE IF NOT EXISTS images ( - image_id INTEGER PRIMARY KEY AUTOINCREMENT NOT NULL, - name TEXT NOT NULL UNIQUE, - camera_id INTEGER NOT NULL, - prior_qw REAL, - prior_qx REAL, - prior_qy REAL, - prior_qz REAL, - prior_tx REAL, - prior_ty REAL, - prior_tz REAL, - CONSTRAINT image_id_check CHECK(image_id >= 0 and image_id < {}), - FOREIGN KEY(camera_id) REFERENCES cameras(camera_id)) -""".format(MAX_IMAGE_ID) - -CREATE_TWO_VIEW_GEOMETRIES_TABLE = """ -CREATE TABLE IF NOT EXISTS two_view_geometries ( - pair_id INTEGER PRIMARY KEY NOT NULL, - rows INTEGER NOT NULL, - cols INTEGER NOT NULL, - data BLOB, - config INTEGER NOT NULL, - F BLOB, - E BLOB, - H BLOB) -""" - -CREATE_KEYPOINTS_TABLE = """CREATE TABLE IF NOT EXISTS keypoints ( - image_id INTEGER PRIMARY KEY NOT NULL, - rows INTEGER NOT NULL, - cols INTEGER NOT NULL, - data BLOB, - FOREIGN KEY(image_id) REFERENCES images(image_id) ON DELETE CASCADE) -""" - -CREATE_MATCHES_TABLE = """CREATE TABLE IF NOT EXISTS matches ( - pair_id INTEGER PRIMARY KEY NOT NULL, - rows INTEGER NOT NULL, - cols INTEGER NOT NULL, - data BLOB)""" - -CREATE_NAME_INDEX = \ - "CREATE UNIQUE INDEX IF NOT EXISTS index_name ON images(name)" - -CREATE_ALL = "; ".join([ - CREATE_CAMERAS_TABLE, - CREATE_IMAGES_TABLE, - CREATE_KEYPOINTS_TABLE, - CREATE_DESCRIPTORS_TABLE, - CREATE_MATCHES_TABLE, - CREATE_TWO_VIEW_GEOMETRIES_TABLE, - CREATE_NAME_INDEX -]) - - -def image_ids_to_pair_id(image_id1, image_id2): - if image_id1 > image_id2: - image_id1, image_id2 = image_id2, image_id1 - return image_id1 * MAX_IMAGE_ID + image_id2 - - -def pair_id_to_image_ids(pair_id): - image_id2 = pair_id % MAX_IMAGE_ID - image_id1 = (pair_id - image_id2) / MAX_IMAGE_ID - return image_id1, image_id2 - - -def array_to_blob(array): - if IS_PYTHON3: - return array.tostring() - else: - return np.getbuffer(array) - - -def blob_to_array(blob, dtype, shape=(-1,)): - if IS_PYTHON3: - return np.fromstring(blob, dtype=dtype).reshape(*shape) - else: - return np.frombuffer(blob, dtype=dtype).reshape(*shape) - - -class COLMAPDatabase(sqlite3.Connection): - - @staticmethod - def connect(database_path): - return sqlite3.connect(database_path, factory=COLMAPDatabase) - - - def __init__(self, *args, **kwargs): - super(COLMAPDatabase, self).__init__(*args, **kwargs) - - self.create_tables = lambda: self.executescript(CREATE_ALL) - self.create_cameras_table = \ - lambda: self.executescript(CREATE_CAMERAS_TABLE) - self.create_descriptors_table = \ - lambda: self.executescript(CREATE_DESCRIPTORS_TABLE) - self.create_images_table = \ - lambda: self.executescript(CREATE_IMAGES_TABLE) - self.create_two_view_geometries_table = \ - lambda: self.executescript(CREATE_TWO_VIEW_GEOMETRIES_TABLE) - self.create_keypoints_table = \ - lambda: self.executescript(CREATE_KEYPOINTS_TABLE) - self.create_matches_table = \ - lambda: self.executescript(CREATE_MATCHES_TABLE) - self.create_name_index = lambda: self.executescript(CREATE_NAME_INDEX) - - def add_camera(self, model, width, height, params, - prior_focal_length=False, camera_id=None): - params = np.asarray(params, np.float64) - cursor = self.execute( - "INSERT INTO cameras VALUES (?, ?, ?, ?, ?, ?)", - (camera_id, model, width, height, array_to_blob(params), - prior_focal_length)) - return cursor.lastrowid - - def add_image(self, name, camera_id, - prior_q=np.zeros(4), prior_t=np.zeros(3), image_id=None): - cursor = self.execute( - "INSERT INTO images VALUES (?, ?, ?, ?, ?, ?, ?, ?, ?, ?)", - (image_id, name, camera_id, prior_q[0], prior_q[1], prior_q[2], - prior_q[3], prior_t[0], prior_t[1], prior_t[2])) - return cursor.lastrowid - - def add_keypoints(self, image_id, keypoints): - assert(len(keypoints.shape) == 2) - assert(keypoints.shape[1] in [2, 4, 6]) - - keypoints = np.asarray(keypoints, np.float32) - self.execute( - "INSERT INTO keypoints VALUES (?, ?, ?, ?)", - (image_id,) + keypoints.shape + (array_to_blob(keypoints),)) - - def add_descriptors(self, image_id, descriptors): - descriptors = np.ascontiguousarray(descriptors, np.uint8) - self.execute( - "INSERT INTO descriptors VALUES (?, ?, ?, ?)", - (image_id,) + descriptors.shape + (array_to_blob(descriptors),)) - - def add_matches(self, image_id1, image_id2, matches): - assert(len(matches.shape) == 2) - assert(matches.shape[1] == 2) - - if image_id1 > image_id2: - matches = matches[:,::-1] - - pair_id = image_ids_to_pair_id(image_id1, image_id2) - matches = np.asarray(matches, np.uint32) - self.execute( - "INSERT INTO matches VALUES (?, ?, ?, ?)", - (pair_id,) + matches.shape + (array_to_blob(matches),)) - - def add_two_view_geometry(self, image_id1, image_id2, matches, - F=np.eye(3), E=np.eye(3), H=np.eye(3), config=2): - assert(len(matches.shape) == 2) - assert(matches.shape[1] == 2) - - if image_id1 > image_id2: - matches = matches[:,::-1] - - pair_id = image_ids_to_pair_id(image_id1, image_id2) - matches = np.asarray(matches, np.uint32) - F = np.asarray(F, dtype=np.float64) - E = np.asarray(E, dtype=np.float64) - H = np.asarray(H, dtype=np.float64) - self.execute( - "INSERT INTO two_view_geometries VALUES (?, ?, ?, ?, ?, ?, ?, ?)", - (pair_id,) + matches.shape + (array_to_blob(matches), config, - array_to_blob(F), array_to_blob(E), array_to_blob(H))) - - -# ---------------------------------------------------------------------------- -# Open the database. - -image_dir = '*.jpg' -database_path = 'colmap.db' -manual_points_path = 'manual_points/*.txt' -camera_id = 1 - -image_fnames = glob.glob(image_dir) -image_fnames.sort() - - -manual_points_fnames = glob.glob(manual_points_path) -manual_matches = {} -for manual_points_fname in manual_points_fnames: - fname = os.path.splitext(os.path.split(manual_points_fname)[-1])[0] - fname = fname.split('_') - manual_matches[(int(fname[0]),int(fname[2]))] = np.loadtxt(manual_points_fname) - - -if False: - # Draw manual key points. - save_dir = '/' - for key in manual_matches: - image_id1,image_id2 = key - img1 = cv2.imread(image_fnames[image_id1 - 1])[:,:,::-1].copy() - img2 = cv2.imread(image_fnames[image_id2 - 1])[:,:,::-1].copy() - - # These are the manually selected coordinates. - kps = np.round(manual_matches[key]).astype(np.int) - kp1s = kps[:,:2] - kp2s = kps[:,2:] - - for i in range(len(kp1s)): - cv2.circle(img1, (kp1s[i][0],kp1s[i][1]), 5, (255,0,255), 2) - cv2.circle(img2, (kp2s[i][0],kp2s[i][1]), 5, (255,0,255), 2) - - fname1 = os.path.split(image_fnames[image_id1 - 1])[-1] - fname2 = os.path.split(image_fnames[image_id2 - 1])[-1] - cv2.imwrite(save_dir + fname1, img1[:,:,::-1]) - cv2.imwrite(save_dir + fname2, img2[:,:,::-1]) - - -db = COLMAPDatabase.connect(database_path) -cursor = db.cursor() - - -if False: - keep_pairs = set() - # Remove matches with insufficient inliers. - min_num_matches = 20 - cursor.execute("SELECT pair_id, data FROM two_view_geometries") - for row in cursor: - pair_id = row[0] - if row[1] is not None: - inlier_matches = np.fromstring(row[1], - dtype=np.uint32).reshape(-1, 2) - if len(inlier_matches) > min_num_matches: - keep_pairs.add(pair_id) - - all_pairs = [pair_id - for pair_id, _ in db.execute("SELECT pair_id, data FROM matches")] - - for pair_id in all_pairs: - if pair_id not in keep_pairs: - print('Deleting pair:', pair_id) - db.execute("DELETE FROM matches WHERE pair_id=?", (pair_id,)) - - -# Add missing keypoints. -for key in manual_matches: - keypoints = dict((image_id, blob_to_array(data, np.float32, (-1, 2))) - for image_id, data in db.execute( - "SELECT image_id, data FROM keypoints")) - - matches = dict( - (pair_id_to_image_ids(pair_id), - blob_to_array(data, np.uint32, (-1, 2))) - for pair_id, data in db.execute("SELECT pair_id, data FROM matches") - if data is not None) - - image_id1,image_id2 = key - keypoints1 = keypoints[image_id1] - keypoints2 = keypoints[image_id2] - - # These are the manually selected coordinates. - kp1 = manual_matches[key][:,:2] - kp2 = manual_matches[key][:,2:] - - for kp in kp1: - d = np.sqrt(np.sum((kp - keypoints1)**2, 1)) - if d.min() > 2: - keypoints1 = np.vstack([keypoints1,kp]) - - for kp in kp2: - d = np.sqrt(np.sum((kp - keypoints2)**2, 1)) - if d.min() > 2: - keypoints2 = np.vstack([keypoints2,kp]) - - # Remove old set of keypoints - db.execute("DELETE FROM keypoints WHERE image_id=?", (image_id1,)) - db.execute("DELETE FROM keypoints WHERE image_id=?", (image_id2,)) - - db.add_keypoints(image_id1, keypoints1.copy()) - db.add_keypoints(image_id2, keypoints2.copy()) - - -# Rebuild keypoint dictionary. -keypoints = dict((image_id, blob_to_array(data, np.float32, (-1, 2))) - for image_id, data in db.execute( - "SELECT image_id, data FROM keypoints")) - -# Assing manual matches to keypoints. -for key in manual_matches: - image_id1,image_id2 = key - - matches = dict( - (pair_id_to_image_ids(pair_id), - blob_to_array(data, np.uint32, (-1, 2))) - for pair_id, data in db.execute("SELECT pair_id, data FROM matches") - if data is not None) - - try: - matches = matches[(image_id1,image_id2)] - except: - matches = np.zeros((0,2), dtype=np.int) - - keypoints1 = keypoints[image_id1] - keypoints2 = keypoints[image_id2] - - # These are the manually selected coordinates. - kp1 = manual_matches[key][:,:2] - kp2 = manual_matches[key][:,2:] - - if False: - img1 = cv2.imread(image_fnames[image_id1 - 1])[:,:,::-1] - img2 = cv2.imread(image_fnames[image_id2 - 1])[:,:,::-1] - - plt.figure() - plt.imshow(img1) - #plt.plot(keypoints1[:,0], keypoints1[:,1], 'ro') - plt.plot(kp1[:,0], kp1[:,1], 'go') - - plt.figure() - plt.imshow(img2) - #plt.plot(keypoints2[:,0], keypoints2[:,1], 'ro') - plt.plot(kp2[:,0], kp2[:,1], 'go') - - m = matches[(image_id1,image_id2)][:20] - plt.figure() - plt.imshow(img1) - plt.plot(keypoints1[m[:,0],0], keypoints1[m[:,0],1], 'ro') - plt.figure() - plt.imshow(img2) - plt.plot(keypoints2[m[:,0],0], keypoints2[m[:,0],1], 'ro') - - for i in range(len(kp1)): - d = np.sqrt(np.sum((kp1[i] - keypoints1)**2, 1)) - ind1 = np.argmin(d) - assert d[ind1] < 2 - - d = np.sqrt(np.sum((kp2[i] - keypoints2)**2, 1)) - ind2 = np.argmin(d) - assert d[ind2] < 2 - - # Loop to artificially increase confidence. - for _ in range(10): - matches = np.vstack([matches,[ind1,ind2]]) - - pair_id = image_ids_to_pair_id(image_id1, image_id2) - db.execute("DELETE FROM matches WHERE pair_id=?", (pair_id,)) - db.add_matches(image_id1, image_id2, matches.copy()) - - print('Adding manually registered matches to pair:', image_id1, image_id2) - - -# Commit the data to the file. -db.commit() - -# Clean up. - -db.close() \ No newline at end of file diff --git a/kamera/colmap_processing/scripts/create_geotiff.py b/kamera/colmap_processing/scripts/create_geotiff.py index fd43d4f..f77b76a 100644 --- a/kamera/colmap_processing/scripts/create_geotiff.py +++ b/kamera/colmap_processing/scripts/create_geotiff.py @@ -1,37 +1,4 @@ #! /usr/bin/python -""" -ckwg +31 -Copyright 2018 by Kitware, Inc. -All rights reserved. - -Redistribution and use in source and binary forms, with or without -modification, are permitted provided that the following conditions are met: - - * Redistributions of source code must retain the above copyright notice, - this list of conditions and the following disclaimer. - - * Redistributions in binary form must reproduce the above copyright notice, - this list of conditions and the following disclaimer in the documentation - and/or other materials provided with the distribution. - - * Neither name of Kitware, Inc. nor the names of any contributors may be used - to endorse or promote products derived from this software without specific - prior written permission. - -THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS ``AS IS'' -AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHORS OR CONTRIBUTORS BE LIABLE FOR -ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - -============================================================================== - -""" from __future__ import division, print_function import numpy as np import os @@ -46,8 +13,8 @@ from scipy.spatial import ConvexHull from scipy.interpolate import griddata -import colmap_processing.vtk_util as vtk_util -from colmap_processing.geo_conversions import enu_to_llh +import kamera.colmap_processing.vtk_util as vtk_util +from kamera.colmap_processing.geo_conversions import enu_to_llh # ---------------------------------------------------------------------------- diff --git a/kamera/colmap_processing/scripts/create_static_camera_from_sfm_images.py b/kamera/colmap_processing/scripts/create_static_camera_from_sfm_images.py deleted file mode 100644 index e0560fc..0000000 --- a/kamera/colmap_processing/scripts/create_static_camera_from_sfm_images.py +++ /dev/null @@ -1,293 +0,0 @@ -#! /usr/bin/python -""" -ckwg +31 -Copyright 2018 by Kitware, Inc. -All rights reserved. - -Redistribution and use in source and binary forms, with or without -modification, are permitted provided that the following conditions are met: - - * Redistributions of source code must retain the above copyright notice, - this list of conditions and the following disclaimer. - - * Redistributions in binary form must reproduce the above copyright notice, - this list of conditions and the following disclaimer in the documentation - and/or other materials provided with the distribution. - - * Neither name of Kitware, Inc. nor the names of any contributors may be used - to endorse or promote products derived from this software without specific - prior written permission. - -THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS ``AS IS'' -AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHORS OR CONTRIBUTORS BE LIABLE FOR -ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - -============================================================================== - -""" -from __future__ import division, print_function -import numpy as np -import os -import cv2 -import subprocess -import matplotlib.pyplot as plt -import glob -import natsort -import trimesh -import math -import PIL -from osgeo import osr, gdal - -# Colmap Processing imports. -from colmap_processing.geo_conversions import llh_to_enu -from colmap_processing.colmap_interface import read_images_binary, Image, \ - read_points3D_binary, read_cameras_binary, qvec2rotmat -from colmap_processing.database import COLMAPDatabase, pair_id_to_image_ids, blob_to_array -import colmap_processing.vtk_util as vtk_util -from colmap_processing.geo_conversions import enu_to_llh, rmat_ecef_enu, \ - rmat_enu_ecef -from colmap_processing.static_camera_model import save_static_camera, \ - write_camera_krtd_file - - -# ---------------------------------------------------------------------------- -mesh_fname = 'coarse.ply' -save_dir = 'reference_views' - -# Base path to the colmap directory. -image_dir = 'images' - -# Image to turn into a static camera. -image_fname = '102CANON/IMG_8520.JPG' - -# Path to the images.bin file. -images_bin_fname = '%s/images.bin' % image_dir -camera_bin_fname = '%s/cameras.bin' % image_dir -points_3d_bin_fname = '%s/points3D.bin' % image_dir - -if True: - #MUTC. - H_enu = np.identity(4) - latitude0 = 0 # degrees - longitude0 = 0 # degrees - altitude0 = 0 # meters above WGS84 ellipsoid - -H_enu = np.array(H_enu) - -# VTK renderings are limited to monitor resolution (width x height). -monitor_resolution = (2200, 1200) -# ---------------------------------------------------------------------------- - - -# Read model into VTK. -model_reader = vtk_util.load_world_model(mesh_fname) - -# Read in the details of all images. -images = read_images_binary(images_bin_fname) -cameras = read_cameras_binary(camera_bin_fname) - - -def process(image_fname, save_dir): - # Consider image with filename. - fname_to_index = {images[_].name: _ for _ in images} - image_id = fname_to_index[image_fname] - image = images[image_id] - camera = cameras[image.camera_id] - p = camera.params - - # True camera matrix and distortion of the real image. - K0 = np.array([[p[0], 0, p[2]], [0, p[1], p[3]], [0, 0, 1]]) - dist = p[4:] - width = camera.width - height = camera.height - - # Desired camera matrix has isotropic focal length. - K = K0.copy() - K[0, 0] = K[1, 1] = np.min([K0[0, 0], K0[1, 1]]) - K[0, 2] = width/2 - K[1, 2] = height/2 - - # Pose of the real camera. - R = qvec2rotmat(image.qvec) - tvec = image.tvec - cam_pos = np.dot(-R.T, image.tvec) - - # Update camera pose into the ENU coordinate system of the 3-D model. - cam_pos = np.dot(H_enu[:3], np.hstack([cam_pos, 1])) - ret = cv2.decomposeProjectionMatrix(H_enu[:3]) - R_enu = ret[1] - R = np.dot(R, R_enu.T) - - # Define the rendering camera required to capture all of the image but not - # exceeding monitor resolution. - K_ = K.copy() - if (width <= monitor_resolution[0] and height <= monitor_resolution[1] and - np.all(dist == 0) and K_[0, 2] == width/2 and K_[0, 2] == height/2): - needs_warping = False - res_x, res_y = width, height - else: - # Need to find the undistorted camera model that circumscribes the - # field of view of the real camera. - K_[0, 2] = monitor_resolution[0]/2 - K_[1, 2] = monitor_resolution[1]/2 - K_[0, 0] = K_[1, 1] = np.min([K_[0, 0], K_[1, 1]]) - - # Unproject points along 'camera_model' image border and project them - # into the temporary camera defined above to determine how much it must - # be expanded to avoid clipping. - points = np.array([[0, width, width, 0], [0, 0, height, height]], - dtype=np.float32) - ray_dir = cv2.undistortPoints(np.expand_dims(points.T, 0), K, dist, - None) - ray_dir = np.squeeze(ray_dir, 0).astype(np.float32).T - ray_dir = np.vstack([ray_dir, np.ones(ray_dir.shape[1])]) - points2 = cv2.projectPoints(ray_dir.T, np.zeros(3, dtype=np.float32), - np.zeros(3, dtype=np.float32), K_, None)[0] - points2 = np.squeeze(points2, 1).T - points2 = np.abs(points2 - np.atleast_2d(K_[:2, 2]).T) - points3 = np.array([[0, width/2, width, width, width, width/2, 0, 0], - [0, 0, 0, height/2, height, height, height, - height/2]], dtype=np.float32) - points3 = np.abs(points3 - np.atleast_2d(K_[:2, 2]).T) - s = np.min(np.min(points3, 1) / np.max(points2, 1)) - K_[0, 0] = K_[1, 1] = K_[1, 1]*s - needs_warping = True - res_x, res_y = monitor_resolution - - vfov = np.arctan(res_y/2/K_[1, 1])*2*180/np.pi - vtk_camera = vtk_util.Camera(res_x, res_y, vfov, cam_pos, R) - - clipping_range = [1, 2000] - - img = vtk_camera.render_image(model_reader, clipping_range=clipping_range, - diffuse=0.6, ambient=0.6, specular=0.1, - light_color=[1.0, 1.0, 1.0], - light_pos=[0,0,1000]) - depth = vtk_camera.unproject_view(model_reader, - clipping_range=clipping_range)[3] - - #img = cv2.resize(img, (width, height)) - #depth = cv2.resize(depth, (width, height)) - - # Warp the rendered imagery. - X, Y = np.meshgrid(np.arange(width) + 0.5, np.arange(height) + 0.5) - points = np.vstack([X.ravel(), Y.ravel()]) - ray_dir = cv2.undistortPoints(np.expand_dims(points.T, 0), K, None, None) - ray_dir = np.squeeze(ray_dir, 0).astype(np.float32).T - ray_dir = np.vstack([ray_dir, np.ones(ray_dir.shape[1])]) - points2 = cv2.projectPoints(ray_dir.T, np.zeros(3, dtype=np.float32), - np.zeros(3, dtype=np.float32), K_, None)[0] - points2 = np.squeeze(points2, 1).T - X2 = np.reshape(points2[0], X.shape).astype(np.float32) - Y2 = np.reshape(points2[1], Y.shape).astype(np.float32) - img = cv2.remap(img, X2, Y2, cv2.INTER_CUBIC) - #E = cv2.remap(E, X2, Y2, cv2.INTER_CUBIC) - #N = cv2.remap(N, X2, Y2, cv2.INTER_CUBIC) - #U = cv2.remap(U, X2, Y2, cv2.INTER_CUBIC) - depth = cv2.remap(depth, X2, Y2, cv2.INTER_CUBIC) - - # Read and undistort image. - real_image = cv2.imread('%s/%s' % (image_dir, image_fname))[:, :, ::-1] - - if real_image.shape[0] == width: - real_image = np.rot90(real_image, k=3).copy() - - cv2.imwrite('%s/ref_image_distored.png' % save_dir, real_image[:, :, ::-1]) - - # Undistort real imagery. - X, Y = np.meshgrid(np.arange(width) + 0.5, np.arange(height) + 0.5) - points = np.vstack([X.ravel(), Y.ravel()]) - ray_dir = cv2.undistortPoints(np.expand_dims(points.T, 0), K, None, None) - ray_dir = np.squeeze(ray_dir, 0).astype(np.float32).T - ray_dir = np.vstack([ray_dir, np.ones(ray_dir.shape[1])]) - points2 = cv2.projectPoints(ray_dir.T, np.zeros(3, dtype=np.float32), - np.zeros(3, dtype=np.float32), K0, dist)[0] - points2 = np.squeeze(points2, 1).T - X2 = np.reshape(points2[0], X.shape).astype(np.float32) - Y2 = np.reshape(points2[1], Y.shape).astype(np.float32) - real_image = cv2.remap(real_image, X2, Y2, cv2.INTER_CUBIC) - - #real_image = cv2.undistort(real_image, K0, dist, K) - #plt.figure(); plt.imshow(real_image) - - try: - os.makedirs(save_dir) - except OSError: - pass - - cv2.imwrite('%s/ref_image.png' % save_dir, real_image[:, :, ::-1]) - - - # ------------------------------------------------------------------- - # Identify holes in the model and then inpaint them. - hole_mask = depth > clipping_range[-1] - 0.1 - - output = cv2.connectedComponentsWithStats(hole_mask.astype(np.uint8), - 8, cv2.CV_32S) - labels = output[1] - - # Remove components that touch outer boundary. - edge_labels = set(labels[:, 0]) - edge_labels = edge_labels.union(set(labels[0, :])) - edge_labels = edge_labels.union(set(labels[:, -1])) - edge_labels = edge_labels.union(set(labels[-1, :])) - - for i in edge_labels: - labels[labels == i] = 0 - - mask = (labels > 0).astype(np.uint8) - img = cv2.inpaint(img, mask, 3, cv2.INPAINT_NS) - depth = cv2.inpaint(depth.astype(np.float32), mask, 3, cv2.INPAINT_NS) - cv2.imwrite('%s/ref_image_rendered.png' % save_dir, img[:, :, ::-1]) - # ------------------------------------------------------------------- - - # ------------------------------------------------------------------- - # Save visualization of depth map. - depth_map_rgb = depth.copy() - depth_map_rgb -= np.percentile(depth_map_rgb.ravel(), 0.1) - depth_map_rgb[depth_map_rgb < 0] = 0 - r = depth_map_rgb.ravel() - r = r[r < 900] - depth_map_rgb /= np.percentile(r, 99.9)/255 - depth_map_rgb[depth_map_rgb > 255] = 255 - depth_map_rgb = 255 - depth_map_rgb - depth_map_rgb = np.round(depth_map_rgb).astype(np.uint8) - clahe = cv2.createCLAHE(clipLimit=2, tileGridSize=(5,5)) - depth_map_rgb = clahe.apply(depth_map_rgb) - depth_map_rgb = cv2.applyColorMap(depth_map_rgb, cv2.COLORMAP_JET) - - cv2.imwrite('%s/depth_map_visualization.jpg' % save_dir, - depth_map_rgb[:, :, ::-1]) - # ------------------------------------------------------------------- - - - latitude, longitude, altitude = enu_to_llh(cam_pos[0], cam_pos[1], - cam_pos[2], latitude0, - longitude0, altitude0) - - # R currently is relative to ENU coordinate system at latitude0, longitude0, - # altitude0, but we need it to be relative to latitude, longitude, altitude. - Rp = np.dot(rmat_enu_ecef(latitude0, longitude0), - rmat_ecef_enu(latitude, longitude)) - - filename = '%s/camera_model.yaml' % save_dir - dist = [0.0, 0.0, 0.0, 0.0] - save_static_camera(filename, height, width, K, dist, np.dot(R, Rp), depth, - latitude, longitude, altitude) - - filename = '%s/camera_model.krtd' % save_dir - write_camera_krtd_file([K, R, tvec, dist], filename) - - -image_fnames = [images[_].name for _ in images] -for key in images: - image = images[key] - save_dir_ = '%s/%s' % (save_dir, image.id) - process(image.name, save_dir_) diff --git a/kamera/colmap_processing/scripts/downsample_video_collection.py b/kamera/colmap_processing/scripts/downsample_video_collection.py deleted file mode 100644 index aa1b7e5..0000000 --- a/kamera/colmap_processing/scripts/downsample_video_collection.py +++ /dev/null @@ -1,175 +0,0 @@ -import cv2 -import numpy as np -import glob -import matplotlib.pyplot as plt -import os - -img_fnames = glob.glob('images/*.png') -img_fnames.sort() - -def apply_clahe(img, clip_limit): - img = img.astype(np.float) - img -= np.percentile(img.ravel(), 0.01) - img /= np.percentile(img.ravel(), 99.99) - img = np.clip(img, 0, 1) - img = np.round(img*255).astype(np.uint8) - clahe = cv2.createCLAHE(clipLimit=clip_limit, tileGridSize=(16, 16)) - - return clahe.apply(img) - - -for img_fname in img_fnames: - print(img_fname) - img = cv2.imread(img_fname, cv2.IMREAD_UNCHANGED) - if img.ndim == 3: - img = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) - - img = apply_clahe(img, clip_limit=3) - cv2.imwrite(img_fname, img) - - -k = 0 - -img1 = cv2.cvtColor(cv2.imread(img_fnames[0]), cv2.COLOR_BGR2GRAY) - -# Frame rate -frame_rate = 60 # Hz - -# Measure optical flow. -optical_flow_10 = [] -optical_flow_50 = [] -optical_flow_90 = [] -sharpness = [] -while True: - try: - img2 = cv2.imread(img_fnames[k], cv2.IMREAD_UNCHANGED) - if img2.ndim == 3: - img2 = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) - except IndexError: - break - - # Calculate sharpness. - sobelx = cv2.Sobel(img2, cv2.CV_64F, 1, 0).ravel() - sobely = cv2.Sobel(img2, cv2.CV_64F, 0, 1).ravel() - sharpness.append(np.mean(np.sqrt(sobelx**2 + sobely**2))) - - if False: - # Just calculate sharpness - print(k) - k += 1 - continue - - w = 256 - XY = np.meshgrid(np.arange(w//2, img1.shape[1] - w//2, w), - np.arange(w//2, img1.shape[0] - w//2, w)) - pts = np.vstack([XY[0].ravel(),XY[1].ravel()]).T.astype(np.float32) - - # calculate optical flow - lk_params = dict(winSize=(w,w), - maxLevel=2, - criteria=(cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT, 10, 0.03)) - print('Calculating optical flow:', img_fnames[k]) - pts2, st, err = cv2.calcOpticalFlowPyrLK(img1, img2, pts, None, **lk_params) - - delta = np.sqrt(np.sum((pts - pts2)**2, 1)) - - optical_flow_10.append(np.percentile(delta, 10)) - optical_flow_50.append(np.percentile(delta, 50)) - optical_flow_90.append(np.percentile(delta, 90)) - print('Optical flow is', optical_flow_50[-1], 'pixels') - - if False: - if k > 0 and optical_flow_50[-1] < 10: - print('Removing', img_fnames[k]) - os.remove(img_fnames[k]) - else: - img1 = img2 - else: - img1 = img2 - - k += 1 - - -optical_flow_10 = np.array(optical_flow_10) -optical_flow_50 = np.array(optical_flow_50) -optical_flow_90 = np.array(optical_flow_90) -sharpness = np.array(sharpness) -times = np.arange(0, len(optical_flow_10))/float(frame_rate) -plt.plot(times, optical_flow_10) -plt.plot(times, optical_flow_50) -plt.plot(times, optical_flow_90) -plt.plot(times, sharpness/sharpness.max()) - -if False: - ind = np.argsort(optical_flow_50)[::-1][3] - plt.imshow(cv2.imread(img_fnames[ind], cv2.IMREAD_UNCHANGED), 'gray') - -ind = np.nonzero(sharpness < 0.28*sharpness.max())[0] - -#ind = optical_flow > 10 - -# Pick the frame with the local-minimum optical flow over half a second. -window = int(0.1*frame_rate) -L = len(sharpness) -local_max = [] -for i in range(L): - ind_left = i - ind_right = i + window - if ind_right > L: - di = ind_right - L - ind_right -= di - ind_left -= di - - local_max.append(max(sharpness[ind_left:ind_right])) - -local_max = np.array(local_max) - -ind = np.nonzero(local_max > sharpness)[0] - -for i in ind: - try: - os.remove(img_fnames[i]) - print('Removing', img_fnames[i]) - except OSError: - pass - - -# Downsample by a fixed amount taking the sharpest image in each window. -final_size = 1500 -sharpness = np.array(sharpness) -inds = np.round(np.linspace(0, len(sharpness), final_size)).astype(np.int) -delete_fnames = [] -for i in range(len(inds) - 1): - indi = np.arange(inds[i], inds[i+1]) - indi = indi[np.argsort(sharpness[indi])[:-1]] - delete_fnames = delete_fnames + [img_fnames[_] for _ in indi] - -for fname in delete_fnames: - print('Removing', fname) - try: - os.remove(fname) - except OSError: - pass - - - - -# Delete 2/3 frames. -img_fnames = glob.glob('GoPro/*.png') -img_fnames.sort() -k = 0 -k2 = 0 -while True: - if k2 == 1 or k2 == 2: - #print(k) - print('Removing', img_fnames[k]) - os.remove(img_fnames[k]) - - k += 1 - - if k2 == 2: - k2 = 0 - continue - - k2 += 1 - diff --git a/kamera/colmap_processing/scripts/explode_drone_videos.py b/kamera/colmap_processing/scripts/explode_drone_videos.py deleted file mode 100644 index d10af6e..0000000 --- a/kamera/colmap_processing/scripts/explode_drone_videos.py +++ /dev/null @@ -1,154 +0,0 @@ -#! /usr/bin/python -""" -ckwg +31 -Copyright 2018 by Kitware, Inc. -All rights reserved. - -Redistribution and use in source and binary forms, with or without -modification, are permitted provided that the following conditions are met: - - * Redistributions of source code must retain the above copyright notice, - this list of conditions and the following disclaimer. - - * Redistributions in binary form must reproduce the above copyright notice, - this list of conditions and the following disclaimer in the documentation - and/or other materials provided with the distribution. - - * Neither name of Kitware, Inc. nor the names of any contributors may be used - to endorse or promote products derived from this software without specific - prior written permission. - -THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS ``AS IS'' -AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHORS OR CONTRIBUTORS BE LIABLE FOR -ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - -============================================================================== - -""" -from __future__ import division, print_function -import numpy as np -import os -import cv2 -import subprocess -import matplotlib.pyplot as plt -import glob -import natsort - - -def process(video_path, image_prefix, output_dir, opt_flow_thresh=25, - downsample_rate=2): - try: - os.makedirs(output_dir) - except OSError: - pass - - filepattern = '%s/%s_%%05d.png' % (output_dir, image_prefix) - - # Call ffmpeg to explode the video file to an image directory. - subprocess.call(' '.join(['ffmpeg', '-i', video_path, filepattern]), - shell=True) - - # Delete redundant frames. - img_fnames = glob.glob('%s/%s_*.png' % (output_dir, image_prefix)) - img_fnames = natsort.natsorted((img_fnames)) - - img1 = cv2.imread(img_fnames[0]) - - if img1.ndim == 3: - img1 = cv2.cvtColor(img1, cv2.COLOR_BGR2GRAY) - - for i in range(1, len(img_fnames)): - try: - img2 = cv2.imread(img_fnames[i]) - - if img2.ndim == 3: - img2 = cv2.cvtColor(img2, cv2.COLOR_BGR2GRAY) - except: - # Some png are corrupt for some reason. - print('Deleting', img_fnames[i]) - os.remove(img_fnames[i]) - continue - - w = 256 - XY = np.meshgrid(np.arange(w//2, img1.shape[1] - w//2, w), - np.arange(w//2, img1.shape[0] - w//2, w)) - pts = np.vstack([XY[0].ravel(), XY[1].ravel()]).T.astype(np.float32) - - # calculate optical flow - lk_params = dict(winSize = (w,w), - maxLevel = 2, - criteria = (cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT, 10, 0.03)) - print('Calculating optical flow:', img_fnames[i]) - pts2, st, err = cv2.calcOpticalFlowPyrLK(img1, img2, pts, None, - **lk_params) - - delta = np.sqrt(np.sum((pts - pts2)**2, 1)) - - # Only consider valid points. - delta = delta[st.ravel().astype(np.bool)] - - new_key_frame = np.mean(st) < 0.5 - - if ~new_key_frame: - print('Optical flow is', np.percentile(delta, 90), 'pixels') - new_key_frame = np.percentile(delta, 90) > opt_flow_thresh - - if new_key_frame: - img1 = img2 - else: - print('Deleting', img_fnames[i]) - os.remove(img_fnames[i]) - - # Delete redundant frames. - img_fnames = glob.glob('%s/%s_*.png' % (output_dir, image_prefix)) - img_fnames = natsort.natsorted((img_fnames)) - k = 0 - for img_fname in img_fnames: - if k > 0: - os.remove(img_fname) - - k += 1 - - if k == downsample_rate: - k = 0 - - os.remove(video_path) - - -# Path to the video. -video_path = '0006.mp4' - -# Path to the list of videos that are publicly released. -image_prefix = 'test' - -# Directory to save results -output_dir = 'out' - - -for i in range(10,22): - video_path = '%s.mp4' - - try: - base_fname = os.path.splitext(video_path)[0] - os.rename('%s.MP4' % base_fname, '%s.mp4' % base_fname) - except: - pass - - if not os.path.isfile(video_path): - continue - - # Directory to save results - head, tail = os.path.split(video_path) - output_dir = '%s/images/%s' % (head, os.path.splitext(tail)[0]) - - image_prefix = '%s_%s' % (head.split('/')[-1], str(i).zfill(4)) - - process(video_path, image_prefix, output_dir, opt_flow_thresh=23, - downsample_rate=5) diff --git a/kamera/colmap_processing/scripts/extract_image_pose.py b/kamera/colmap_processing/scripts/extract_image_pose.py deleted file mode 100644 index 95d2b16..0000000 --- a/kamera/colmap_processing/scripts/extract_image_pose.py +++ /dev/null @@ -1,87 +0,0 @@ -#! /usr/bin/python -""" -ckwg +31 -Copyright 2018 by Kitware, Inc. -All rights reserved. - -Redistribution and use in source and binary forms, with or without -modification, are permitted provided that the following conditions are met: - - * Redistributions of source code must retain the above copyright notice, - this list of conditions and the following disclaimer. - - * Redistributions in binary form must reproduce the above copyright notice, - this list of conditions and the following disclaimer in the documentation - and/or other materials provided with the distribution. - - * Neither name of Kitware, Inc. nor the names of any contributors may be used - to endorse or promote products derived from this software without specific - prior written permission. - -THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS ``AS IS'' -AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHORS OR CONTRIBUTORS BE LIABLE FOR -ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - -============================================================================== - -""" -from __future__ import division, print_function -import numpy as np -import os -import cv2 -import subprocess -import matplotlib.pyplot as plt -import glob -import natsort -import trimesh -import math -import PIL -from osgeo import osr, gdal -from scipy.optimize import fmin, minimize, fminbound - -# Colmap Processing imports. -from colmap_processing.geo_conversions import llh_to_enu -from colmap_processing.colmap_interface import read_images_binary, Image, \ - read_points3D_binary, read_cameras_binary, qvec2rotmat -from colmap_processing.database import COLMAPDatabase, pair_id_to_image_ids, blob_to_array -import colmap_processing.vtk_util as vtk_util -from colmap_processing.geo_conversions import enu_to_llh, llh_to_enu, \ - rmat_ecef_enu, rmat_enu_ecef -from colmap_processing.static_camera_model import save_static_camera, \ - load_static_camera_from_file, write_camera_krtd_file - - -# ---------------------------------------------------------------------------- -project_dir = '/test' -pose_filename = '%s/image_poses.txt' % project_dir - -# Path to the images.bin file. -images_bin_fname = '%s/images.bin' % project_dir - -images = read_images_binary(images_bin_fname) - -lines = [] -for image_num in images: - image = images[image_num] - R = qvec2rotmat(image.qvec) - tvec = image.tvec - pos = -np.dot(R.T, tvec) - line = [image.name] - line = line + list(pos.ravel()) + list(R.ravel()) - line = [str(_) for _ in line] - lines.append(line) - - -with open(pose_filename, 'w') as f: - t = '# Image Name, Camera Position, Camera Rotation Matrix (unraveled by row).' - f.write(t) - for line in lines: - line = '%s\n' % ', '.join(line) - f.write(line) diff --git a/kamera/colmap_processing/scripts/filter_dense_images.py b/kamera/colmap_processing/scripts/filter_dense_images.py deleted file mode 100644 index 9a6c4e9..0000000 --- a/kamera/colmap_processing/scripts/filter_dense_images.py +++ /dev/null @@ -1,45 +0,0 @@ -from __future__ import division, print_function -import numpy as np -import os -import cv2 -import subprocess -import matplotlib.pyplot as plt -import glob -import natsort - -fname = '/mnt/data/colmap/stereo/patch-match.cfg' -with open(fname, 'r') as fp: - patch_match_lines = fp.readlines() - -fname = '/mnt/data/colmap/stereo/fusion.cfg' -with open(fname, 'r') as fp: - fusion_lines = fp.readlines() - -k = 0 -fnames = [] -while True: - try: - fnames.append(patch_match_lines[k]) - k += 1 - l2 = patch_match_lines[k] - k += 1 - except IndexError: - break - -fnames = natsort.natsorted(fnames) - -ind = np.arange(0, len(fnames), 3) -fnames = [fnames[_] for _ in ind] - -nl2 = '__auto__, 20\n' - -fname = '/mnt/data/colmap/stereo/patch-match2.cfg' -with open(fname, 'w') as fp: - for fn in fnames: - fp.write(fn) - fp.write(nl2) - -fname = '/mnt/data/colmap/stereo/fusion2.cfg' -with open(fname, 'w') as fp: - for fn in fnames: - fp.write(fn) \ No newline at end of file diff --git a/kamera/colmap_processing/scripts/from_landmark_registration_gui/calibrate_static_camera_from_llh_points.py b/kamera/colmap_processing/scripts/from_landmark_registration_gui/calibrate_static_camera_from_llh_points.py deleted file mode 100644 index bc020c3..0000000 --- a/kamera/colmap_processing/scripts/from_landmark_registration_gui/calibrate_static_camera_from_llh_points.py +++ /dev/null @@ -1,327 +0,0 @@ -#! /usr/bin/python -""" -ckwg +31 -Copyright 2018 by Kitware, Inc. -All rights reserved. - -Redistribution and use in source and binary forms, with or without -modification, are permitted provided that the following conditions are met: - - * Redistributions of source code must retain the above copyright notice, - this list of conditions and the following disclaimer. - - * Redistributions in binary form must reproduce the above copyright notice, - this list of conditions and the following disclaimer in the documentation - and/or other materials provided with the distribution. - - * Neither name of Kitware, Inc. nor the names of any contributors may be used - to endorse or promote products derived from this software without specific - prior written permission. - -THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS ``AS IS'' -AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHORS OR CONTRIBUTORS BE LIABLE FOR -ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - -============================================================================== - -""" -from __future__ import division, print_function -import numpy as np -import os -import cv2 -import subprocess -import matplotlib.pyplot as plt -import glob -import natsort -import math -import PIL -from osgeo import osr, gdal -from scipy.optimize import fmin, minimize, fminbound - -# Colmap Processing imports. -from colmap_processing.geo_conversions import llh_to_enu -from colmap_processing.colmap_interface import read_images_binary, Image, \ - read_points3d_binary, read_cameras_binary, qvec2rotmat -from colmap_processing.database import COLMAPDatabase, pair_id_to_image_ids, blob_to_array -import colmap_processing.vtk_util as vtk_util -from colmap_processing.geo_conversions import enu_to_llh, llh_to_enu, \ - rmat_ecef_enu, rmat_enu_ecef -from colmap_processing.static_camera_model import save_static_camera, \ - load_static_camera_from_file, write_camera_krtd_file -from colmap_processing.camera_models import StandardCamera, DepthCamera -from colmap_processing.calibration import calibrate_camera_to_xyz, \ - cam_depth_map_plane -from colmap_processing.platform_pose import PlatformPoseFixed -from colmap_processing.camera_models import quaternion_from_matrix, \ - quaternion_inverse - - -# ---------------------------------------------------------------------------- -# Landmark annotation GUI workspace path. -landmark_gui_workspace = '/home/user/libraries/georegistration_guis/data/workspace' - -# Image Directory. -image_dir = '/home/user/libraries/georegistration_guis/data/frames' - -# Directory to save camera models within. -save_dir = '/home/user/libraries/georegistration_guis/data/camera_models' - -# Define the origin of the easting/northing/up coordinate system. -lat0 = 0 -lon0 = 0 -h0 = 0 - -# VTK renderings are limited to monitor resolution (width x height). -monitor_resolution = (2200, 1200) - -dist = [0, 0, 0, 0] -optimize_k1 = True -optimize_k2 = False -optimize_k3 = False -optimize_k4 = False -fix_principal_point = False -fix_aspect_ratio = True -# ---------------------------------------------------------------------------- - - -image_fnames = [] -for ext in ['.jpg', '.tiff', '.png', '.bmp', '.jpeg']: - image_fnames = image_fnames + glob.glob('%s/*%s' % (image_dir, ext)) - -img_fname_to_cam_id = {} -img_id_to_cam_id = {} -img_cam_id_to_fname = {} -img_fname_to_img_id = {} -camera_models = {} -images = {} -for image_id, fname in enumerate(image_fnames): - image_id = image_id + 1 - img = cv2.imread(fname) - - if img.ndim == 3: - img = img[:, :, ::-1] - - image_fname = os.path.splitext(os.path.split(fname)[1])[0] - - height, width = img.shape[:2] - - # Every image is from a different camera. - camera_id = image_id - - img_fname_to_img_id[image_fname] = image_id - img_cam_id_to_fname[image_id] = image_fname - img_fname_to_cam_id[image_fname] = camera_id - img_id_to_cam_id[image_id] = camera_id - images[image_id] = img - - # Initialize with dummy values. - camera_models[camera_id] = StandardCamera(width, height, np.identity(3), - np.zeros(4), np.zeros(3), - [0, 0, 0, 1]) - -# ---------------------------------------------------------------------------- -# Parse landmark annotation GUI workspace. - -# img_keypoints is a dictionary that accepts the integer image index and -# returns a list with the first element being the indices into landmarks and -# the second element is the associated image coordinates for each landmark. -img_keypoints = {} -for image_id, fname in enumerate(image_fnames): - img_fname = os.path.splitext(os.path.split(fname)[1])[0] - fname = ('%s/%s_image_points.txt' % (landmark_gui_workspace, img_fname)) - try: - points = np.loadtxt(fname) - except (OSError, IOError): - points = np.zeros((0, 3)) - - image_id = img_fname_to_img_id[img_fname] - camera_id = img_fname_to_cam_id[img_fname] - - img_keypoints[image_id] = [points[:, 0].astype(np.int), points[:, 1:]] - -points_fname = '%s/ground_control_points.txt' % landmark_gui_workspace -ret = np.loadtxt(points_fname) -enu_pts = {int(ret[i, 0]): llh_to_enu(ret[i, 1], ret[i, 2], ret[i, 3], lat0, - lon0, h0) - for i in range(len(ret))} - - -def calibrate_camera_id(image_id, save_dir_, fix_aspect_ratio=True, - fix_principal_point=True, fix_k1=True, fix_k2=True, - fix_k3=True, fix_k4=True, fix_k5=True, fix_k6=True): - print(img_cam_id_to_fname[image_id]) - l_ind, im_pts0 = img_keypoints[image_id] - wrld_pts = [] - im_pts = [] - # Only collect keypoints with an associated easting/northing/up position. - for i in range(len(l_ind)): - if l_ind[i] in enu_pts: - if abs(enu_pts[l_ind[i]][2]) < 0.01: - wrld_pts.append(enu_pts[l_ind[i]]) - im_pts.append(im_pts0[i]) - - im_pts = np.array(im_pts) - wrld_pts = np.array(wrld_pts) - ref_image = images[image_id] - cm0 = camera_models[img_id_to_cam_id[image_id]] - height = cm0.height - width = cm0.width - - ret = calibrate_camera_to_xyz(im_pts, wrld_pts, height, width, - fix_aspect_ratio=fix_aspect_ratio, - fix_principal_point=fix_principal_point, - fix_k1=fix_k1, fix_k2=fix_k2, fix_k3=fix_k3, - fix_k4=fix_k4, fix_k5=fix_k5, fix_k6=fix_k6, - plot_results=False, ref_image=ref_image) - - K, dist, rvec, tvec = ret - - # Save camera model specification. Remembering that 'camera_models' considers - # quaterions to be coordinate system rotations not transformations. So, we have - # to invert the standard computer vision rotation matrix to create the - # orientation quaterions. - R = cv2.Rodrigues(rvec)[0] - quat = quaternion_inverse(quaternion_from_matrix(R)) - pos = -np.dot(R.T, tvec).ravel() - platform_pose_provider = PlatformPoseFixed(pos, quat) - cm = StandardCamera(width, height, K, dist, [0, 0, 0], [0, 0, 0, 1], - platform_pose_provider) - - # Assume level plane at z = 0. - plane_point = [0, 0, 0] - plane_normal = [0, 0, 1] - - depth_map = cam_depth_map_plane(cm, plane_point, plane_normal) - - cm = DepthCamera(width, height, K, dist, [0, 0, 0], [0, 0, 0, 1], depth_map, - platform_pose_provider) - - camera_models[img_id_to_cam_id[image_id]] = cm - - # Save camera model. - try: - os.makedirs(save_dir_) - except (OSError, IOError): - pass - - if True: - # Sanity check. - err = np.sqrt(np.sum((im_pts.T - cm.project(wrld_pts.T))**2, axis=0)) - err = np.sort(err) - print('Min / Mean / Max reprojection error: (%0.3f, %0.3f, %0.3f) ' - 'pixels' % (err.min(), np.mean(err), err.max())) - - np.savetxt('%s/projection_pixel_err.txt' % save_dir_, err, fmt='%.3f') - - wrld_pts2 = cm.unproject_to_depth(im_pts.T) - err = np.sqrt(np.sum((wrld_pts2 - wrld_pts.T)**2, axis=0)) - err = np.sort(err) - print('Min / Mean / Max unprojection error: (%0.3f, %0.3f, %0.3f) ' - 'meters' % (err.min(), np.mean(err), err.max())) - - np.savetxt('%s/unprojection_meters_err.txt' % save_dir_, err, - fmt='%.3f') - - fname = '%s/camera_model.yaml' % save_dir_ - cm.save_to_file(fname) - - if ref_image.ndim == 3: - cv2.imwrite('%s/ref_view.jpg' % save_dir_, ref_image[:, :, ::-1]) - cv2.imwrite('%s/undistorted.jpg' % save_dir_, - cv2.undistort(ref_image[:, :, ::-1], K, dist)) - else: - cv2.imwrite('%s/ref_view.jpg' % save_dir_, ref_image) - cv2.imwrite('%s/undistorted.jpg' % save_dir_, - cv2.undistort(ref_image, K, dist)) - - filename = '%s/camera_model.krtd' % save_dir_ - write_camera_krtd_file([K, R, tvec, dist], filename) - - X, Y = np.meshgrid(np.linspace(0.5, cm.width - .5, cm.width), - np.linspace(0.5, cm.height - .5, cm.height)) - X, Y, Z = cm.unproject_to_depth(np.vstack([X.ravel(), Y.ravel()])) - X.shape = (cm.height, cm.width) - Y.shape = (cm.height, cm.width) - Z.shape = (cm.height, cm.width) - - XYZ = np.dstack([X, Y, Z]).astype(np.float32) - filename = '%s/xyz_per_pixel.npy' % save_dir_ - np.save(filename, XYZ, allow_pickle=False) - - plt.figure(num=None, figsize=(15.3, 10.7), dpi=80) - font = {'size' : 26} - plt.rc('font', **font) - plt.rc('axes', linewidth=4) - plt.imshow(ref_image) - im_pts2 = cm.project(wrld_pts.T).T - for i in range(len(im_pts)): - plt.plot(im_pts[i, 0], im_pts[i, 1], 'bo') - plt.plot(im_pts2[i, 0], im_pts2[i, 1], 'ro') - plt.plot([im_pts[i, 0], im_pts2[i, 0]], - [im_pts[i, 1], im_pts2[i, 1]], 'k--') - - plt.savefig('%s/model_pixel_error.pdf' % save_dir_) - - plt.figure(num=None, figsize=(15.3, 10.7), dpi=80) - font = {'size' : 40} - plt.rc('font', **font) - plt.rc('axes', linewidth=4) - wrld_pts2 = cm.unproject_to_depth(im_pts.T).T - for i in range(len(im_pts)): - plt.plot(wrld_pts[i, 0], wrld_pts[i, 1], 'bo') - plt.plot(wrld_pts2[i, 0], wrld_pts2[i, 1], 'ro') - plt.plot([wrld_pts[i, 0], wrld_pts2[i, 0]], - [wrld_pts[i, 1], wrld_pts2[i, 1]], 'k--') - - plt.xlabel('X (meters)', fontsize=40) - plt.ylabel('Y (meters)', fontsize=40) - - plt.savefig('%s/model_meters_error.pdf' % save_dir_) - - -if True: - img_to_process = ['images',] - image_ids = [img_fname_to_cam_id[i] for i in img_to_process] -else: - image_ids = images.keys() - -for image_id in image_ids: - try: - plt.close('all') - save_dir_ = '%s/%s_1' % (save_dir, img_cam_id_to_fname[image_id]) - calibrate_camera_id(image_id, save_dir_, fix_aspect_ratio=True, - fix_principal_point=True, fix_k1=True, fix_k2=True, - fix_k3=True, fix_k4=True, fix_k5=True, fix_k6=True) - - if False: - plt.close('all') - save_dir_ = '%s/%s_2' % (save_dir, img_cam_id_to_fname[image_id]) - calibrate_camera_id(image_id, save_dir_, fix_aspect_ratio=False, - fix_principal_point=True, fix_k1=False, - fix_k2=True, fix_k3=True, fix_k4=True, - fix_k5=True, fix_k6=True) - - if False: - plt.close('all') - save_dir_ = '%s/%s_3' % (save_dir, img_cam_id_to_fname[image_id]) - calibrate_camera_id(image_id, save_dir_, fix_aspect_ratio=False, - fix_principal_point=False, fix_k1=False, - fix_k2=True, fix_k3=True, fix_k4=True, - fix_k5=True, fix_k6=True) - - if False: - plt.close('all') - save_dir_ = '%s/%s_4' % (save_dir, img_cam_id_to_fname[image_id]) - calibrate_camera_id(image_id, save_dir_, fix_aspect_ratio=False, - fix_principal_point=False, fix_k1=False, - fix_k2=False, fix_k3=True, fix_k4=True, - fix_k5=True, fix_k6=True) - except: - pass \ No newline at end of file diff --git a/kamera/colmap_processing/scripts/georegister_data.py b/kamera/colmap_processing/scripts/georegister_data.py deleted file mode 100644 index 9e5de02..0000000 --- a/kamera/colmap_processing/scripts/georegister_data.py +++ /dev/null @@ -1,416 +0,0 @@ -#!/usr/bin/env python -""" -ckwg +31 -Copyright 2019 by Kitware, Inc. -All rights reserved. - -Redistribution and use in source and binary forms, with or without -modification, are permitted provided that the following conditions are met: - - * Redistributions of source code must retain the above copyright notice, - this list of conditions and the following disclaimer. - - * Redistributions in binary form must reproduce the above copyright notice, - this list of conditions and the following disclaimer in the documentation - and/or other materials provided with the distribution. - - * Neither name of Kitware, Inc. nor the names of any contributors may be used - to endorse or promote products derived from this software without specific - prior written permission. - -THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS ``AS IS'' -AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHORS OR CONTRIBUTORS BE LIABLE FOR -ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - -============================================================================== - -This script helps to georegister the 3-D reconstruction coordinate system used -by Colmap. We assume that enough of the scene is jointly visible (e.g., enough -high-altitude views) so that georegistration is a rigid rotation and isotropic -scaling. - -(1) The first step is to geo-register keypoints within images. This is done by -first saving images with keypoints associated with a sparse-reconstructed 3-D -point superimposed. These images should be loaded into the -landmark_registration GUI. The ENU origin should be added as a point with -latitude and longitude both zero. - - -""" -from __future__ import division, print_function, absolute_import -import numpy as np -import cv2 -import glob -import os -import matplotlib.pyplot as plt -from scipy.sparse import lil_matrix -import time -from sklearn.decomposition import PCA -from scipy.spatial import KDTree -import threading -from scipy.optimize import minimize -from mpl_toolkits.mplot3d import Axes3D - -# Colmap Processing imports. -from colmap_processing.geo_conversions import llh_to_enu, enu_to_llh -from colmap_processing.colmap_interface import read_images_binary, Image, \ - read_points3D_binary, read_cameras_binary, qvec2rotmat -from colmap_processing.database import COLMAPDatabase, pair_id_to_image_ids, blob_to_array - - -# ---------------------------------------------------------------------------- -# Base path to the colmap directory. -image_dir = '/media/images0' -colmap_data_dir = '/media/colmap' - -# Path to the images.bin file. -images_bin_fname = '%s/images.bin' % colmap_data_dir - -# Path to the points3D.bin file. -points_3d_bin_fname = '%s/points3D.bin' % colmap_data_dir - -# Path to save images to in order to geo-register. -georegister_data_dir = '%s/georegistration' % colmap_data_dir - -georegistration_result_fname = '%s/georegistration_matrix.txt' % georegister_data_dir - -# Define ENU coordinate system origin. -lat0 = 0 # degrees -lon0 = 0 # degrees -h0 = 0 # meters above WGS84 ellipsoid - - -# Read in the details of all images. -images = read_images_binary(images_bin_fname) - - -# Remove image keypoints without associated reconstructed 3-D point. -for image_num in images: - image = images[image_num] - ind = [_ for _ in range(len(image.xys)) if image.point3D_ids[_] != -1] - xys = image.xys[ind] - point3D_ids = image.point3D_ids[ind] - - images[image_num] = Image(id=image.id, qvec=image.qvec, tvec=image.tvec, - camera_id=image.camera_id, name=image.name, - xys=xys, point3D_ids=point3D_ids) - - -if False: - # Save images with keypoints superimposed. This allows selection of - # pixels near keypoints to be geolocated. - try: - os.makedirs('%s/workspace' % georegister_data_dir) - except OSError: - pass - - for image_num in images: - print(image_num) - image = images[image_num] - img_fname = '%s/%s' % (image_dir, image.name) - img = cv2.imread(img_fname) - - if img.shape[0] > img.shape[1]: - img = np.rot90(img, k=3).copy() - - for i, xy in enumerate(image.xys): - if image.point3D_ids[i] == -1: - continue - - xy = tuple(np.round(xy).astype(np.int)) - cv2.circle(img, xy, 5, color=(0, 0, 255), thickness=1) - - img_fname = '%s/images/%s.jpg' % (georegister_data_dir, image.id) - - try: - os.makedirs(os.path.split(img_fname)[0]) - except OSError: - pass - - img = cv2.imwrite(img_fname, img) - - -pts_3d = read_points3D_binary(points_3d_bin_fname) - - -def show_image(image_id): - image = images[image_id] - img = cv2.imread('%s/%s' % (image_dir, image.name)) - if img.ndim == 3: - img = img[:, :, ::-1] - - plt.imshow(img) - - -def get_xyz_from_image_pt(image_id, im_pt): - """Return model 3-D point from image index and image coordinate of point. - - """ - image = images[image_id] - ind0 = [_ for _ in range(len(image.xys)) if image.point3D_ids[_] != -1] - xys = image.xys[ind0] - d = np.sqrt(np.sum((xys - np.atleast_2d(im_pt))**2, 1)) - ind1 = np.argmin(d) - - print('Distance to select point:', d[ind1], 'pixels') - if d[ind1] > 10: - return None - - xyz = pts_3d[image.point3D_ids[ind0[ind1]]].xyz - return xyz - - -# Load level_points.txt, which encodes all points that are at the same -# elevation. The file should contain an image ID followed by the image -# coordinates of the point. -temp = np.loadtxt('%s/level_points.txt' % georegister_data_dir) -level_pts = [] -for line in temp: - image_id = int(line[0]) - im_pt = line[1:] - - if False: - # Show points plotted on image. - plt.figure() - show_image(image_id) - plt.plot(im_pt[0], im_pt[1], 'ro') - - xyz = get_xyz_from_image_pt(image_id, im_pt) - if xyz is not None: - level_pts.append(xyz) - -level_pts = np.array(level_pts).T - - -# Load point_lower_higher.txt, where the first point has a lower elevation than -# the second. -temp = np.loadtxt('%s/point_lower_higher.txt' % georegister_data_dir) -image_id = int(temp[0][0]) -xyz1 = get_xyz_from_image_pt(image_id, temp[0][1:]) -xyz2 = get_xyz_from_image_pt(image_id, temp[1][1:]) -v_up = xyz2 - xyz1 - -if False: - # Show points plotted on image. - plt.figure() - show_image(image_id) - plt.plot(temp[0][1], temp[0][2], 'ro') - plt.plot(temp[1][1], temp[1][2], 'bo') - - -def level_err(x): - R = cv2.Rodrigues(x[:3])[0] - - if np.dot(R, v_up)[2] < 0: - return 1e10 - - xyz = np.dot(R, level_pts) - #err = max(xyz[2]) - min(xyz[2]) - err = np.std(xyz[2]) - #print(x, err) - return err - - -# Solve for optimal transform. -best_err = np.inf -for _ in range(100): - x = (np.random.rand(3)*2 - 1)*np.pi - - for _ in range(5): - x = minimize(level_err, x, tol=1e-12).x - - err_ = level_err(x) - if err_ < best_err: - best_x = x - best_err = err_ - -print('Final level error:', best_err) - - -# Rotation matrix that puts model right-side up and level. -R = cv2.Rodrigues(best_x)[0] - -if False: - # Show points plotted on image. - xyz = np.dot(R, level_pts) - fig = plt.figure() - ax = fig.add_subplot(111, projection='3d') - plt.plot(xyz[0], xyz[1], xyz[2], '.') - plt.xlabel('Model X') - plt.ylabel('Model Y') - - -# Load in landmark_registration points. -gcp = np.loadtxt('%s/workspace/ground_control_points.txt' % - georegister_data_dir) -with open('%s/workspace/image_list.txt' % georegister_data_dir, 'r') as f: - image_list = f.readlines() - -latlonalt = {int(_[0]): _[1:] for _ in gcp} -model_xyz = {} -for fname in glob.glob('%s/workspace/*_image_points.txt' % - georegister_data_dir): - image_id = int(os.path.split(fname)[1].split('_image_points.txt')[0]) - - pts = np.loadtxt(fname) - pts = np.atleast_2d(pts) - - if False: - # Show points plotted on image. - plt.figure() - show_image(image_id) - - for pt in pts: - im_pt = pt[1:] - xyz = get_xyz_from_image_pt(image_id, im_pt) - print(xyz) - if xyz is not None: - model_xyz[int(pt[0])] = xyz - - if True: - plt.plot(im_pt[0], im_pt[1], 'ro') - - -# The point that is intended to be the origin has latitude and longitude zero. -i = [_ for _ in latlonalt if latlonalt[_][0] == 0 and latlonalt[_][1] == 0][0] -origin_xyz = model_xyz[i] -del model_xyz[i] -del latlonalt[i] - -keys = list(latlonalt.keys()) - -# Make sure there is a landmark for each latitude/longitude. -assert len(set(model_xyz.keys()).difference(set(keys))) == 0 - -keys = list(model_xyz.keys()) - -latlonalt = np.array([latlonalt[_] for _ in keys]) -latlonalt[:, 2] = 0 -model_xyz = np.array([model_xyz[_] for _ in keys]).T - -if False: - fig = plt.figure() - ax = fig.add_subplot(111, projection='3d') - plt.plot(model_xyz[0], model_xyz[1], model_xyz[2], '.') - plt.xlabel('Model X') - plt.ylabel('Model Y') - -# Pick a reasonable origin to start. -lat0, lon0 = np.mean(latlonalt, axis=0)[:2] - -easting_northing = np.array([llh_to_enu(_[0], _[1], 0, lat0, lon0, 0)[:2] - for _ in latlonalt]).T - -if False: - fig = plt.figure() - plt.plot(easting_northing[0], easting_northing[1], '.') - plt.xlabel('Easting (m)') - plt.ylabel('Northing (m)') - -# Create homogenous 2-D leveled version of model points. -model_xyz_leveled = np.dot(R, model_xyz) -model_xyz_leveled[2] = 1 - -if False: - fig = plt.figure() - ax = fig.add_subplot(111, projection='3d') - plt.plot(model_xyz_leveled[0], model_xyz_leveled[1], model_xyz_leveled[2], - '.') - plt.xlabel('Model X') - plt.ylabel('Model Y') - - -def get_m(x): - theta = x[0] - scale = x[1] - tx = x[2] - ty = x[3] - - c = np.cos(theta) - s = np.sin(theta) - - R_ = np.identity(4) - R_[:2, :2] = np.array([[c, -s], [s, c]]) - - S_ = np.identity(4) - S_[:3, :3] *= scale - - T_ = np.identity(4) - T_[0, 3] = -tx - T_[1, 3] = -ty - - return np.dot(T_, np.dot(S_, R_)) - - -def align_err(x): - if x[1] < 0: - return 1e10 - - m = get_m(x) - m = m[:, [0, 1, 3]] - err = easting_northing - np.dot(m, model_xyz_leveled)[:2] - err = np.sqrt(np.mean(np.sum(err**2, axis=0))) - print('Error in meters rms:', err) - return err - - -def plot_err(x): - m = get_m(x) - m = m[:, [0, 1, 3]] - pts1 = np.dot(m, model_xyz_leveled)[:2] - plt.plot(pts1[0], pts1[1], 'ro') - plt.plot(easting_northing[0], easting_northing[1], 'bo') - for i in range(pts1.shape[1]): - plt.plot([pts1[0], easting_northing[0]], - [pts1[1], easting_northing[1]], 'b-') - - -# Solve for optimal transform. -x = np.array([np.random.rand(1)[0]*np.pi*2, 1, 0, 0]) -for _ in range(4): - x = minimize(align_err, x).x - -M2 = get_m(x) -M1 = np.identity(4) -M1[:3, :3] = R -M = np.dot(M2, M1) - -# M now warps from model coordinates to east/northing/up at lat0 and lon0. But, -# we want to update lat0, lon0, and h0 such that origin_xyz is (0, 0, 0). -enu0 = np.dot(M, np.hstack([origin_xyz, 1]))[:3] -lat0, lon0, h0 = enu_to_llh(enu0[0], enu0[1], enu0[2], lat0, lon0, 0) - -if False: - # Define origin_xy - llh_to_enu(enu0[0], enu0[1], enu0[2], lat0, lon0, 0) - -M[:3, 3] -= np.dot(M, np.hstack([origin_xyz, 1]))[:3] - -print('Should be all zeros', np.dot(M, np.hstack([origin_xyz, 1]))[:3]) - -print('Transformation from model coordinates to east/north/up meters relative ' - 'to the origin at latitude=%0.8f, latitude=%0.8f, height=%0.3f meters is' - % (lat0, lon0, h0)) -print(M) - - -with open(georegistration_result_fname, 'w') as f: - f.write('# This registration matrix accepts coordinates in the ' - 'coordinates system used by\n# the structure-from-motion ' - 'reconstruction and returns easting, northing, up\n# coordinates ' - 'in meters. Therefere, it applies to the reconstructed poses of ' - 'the\n# cameras and sparse and dense point reconstructions.\n\n') - for i in range(4): - f.write('%0.10f %0.10f %0.10f %0.10f\n' % (tuple(M[i]))) - - f.write('\n# The origin of the east-north-up coordinate system is located ' - 'at:\nlatitude0 = %0.8f # degrees\nlongitude0 = %0.8f # ' - 'degrees\nheight0 = %0.3f # meters above WGS84 ' - 'ellipsoid\n' % (lat0, lon0, h0)) diff --git a/kamera/colmap_processing/scripts/georegister_data_sparse_3d_pts.py b/kamera/colmap_processing/scripts/georegister_data_sparse_3d_pts.py deleted file mode 100644 index 65574a5..0000000 --- a/kamera/colmap_processing/scripts/georegister_data_sparse_3d_pts.py +++ /dev/null @@ -1,252 +0,0 @@ -#!/usr/bin/env python -""" -ckwg +31 -Copyright 2019 by Kitware, Inc. -All rights reserved. - -Redistribution and use in source and binary forms, with or without -modification, are permitted provided that the following conditions are met: - - * Redistributions of source code must retain the above copyright notice, - this list of conditions and the following disclaimer. - - * Redistributions in binary form must reproduce the above copyright notice, - this list of conditions and the following disclaimer in the documentation - and/or other materials provided with the distribution. - - * Neither name of Kitware, Inc. nor the names of any contributors may be used - to endorse or promote products derived from this software without specific - prior written permission. - -THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS ``AS IS'' -AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHORS OR CONTRIBUTORS BE LIABLE FOR -ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - -============================================================================== - -This script helps to georegister the 3-D reconstruction coordinate system used -by Colmap. We assume that enough of the scene is jointly visible (e.g., enough -high-altitude views) so that georegistration is a rigid rotation and isotropic -scaling. - -(1) The first step is to geo-register keypoints within images. This is done by -first saving images with keypoints associated with a sparse-reconstructed 3-D -point superimposed. These images should be loaded into the -landmark_registration GUI. The ENU origin should be added as a point with -latitude and longitude both zero. - - -""" -from __future__ import division, print_function, absolute_import -import numpy as np -import cv2 -import glob -import os -import matplotlib.pyplot as plt -from scipy.sparse import lil_matrix -import time -from sklearn.decomposition import PCA -from scipy.spatial import KDTree -import threading -from scipy.optimize import minimize -from mpl_toolkits.mplot3d import Axes3D - -# Colmap Processing imports. -from colmap_processing.geo_conversions import llh_to_enu, enu_to_llh -from colmap_processing.colmap_interface import read_images_binary, Image, \ - read_points3D_binary, read_cameras_binary, qvec2rotmat -from colmap_processing.database import COLMAPDatabase, pair_id_to_image_ids, blob_to_array - - -# ---------------------------------------------------------------------------- -# Base path to the colmap directory. -data_dir = '/mnt/data' -result_fname = '%s/georegistration_matrix.txt' % data_dir - - -# Load level_points.txt, which encodes all points that are at the same -# elevation. The file should contain an image ID followed by the image -# coordinates of the point. -level_pts = np.loadtxt('%s/level_points.txt' % data_dir).T - -# Load point_lower_higher.txt, where the first point has a lower elevation than -# the second. -xyz1, xyz2 = np.loadtxt('%s/point_lower_higher.txt' % data_dir) -v_up = xyz2 - xyz1 - -# Rows where first three are model x,y,z coordinates and the last two elements -# are the latitude and longitude in degrees. -tmp = np.loadtxt('%s/points.txt' % data_dir) -latlonalt = tmp[:, 3:] -model_xyz = tmp[:, :3].T -origin_xyz = model_xyz[:, 0] - - -def level_err(x): - R = cv2.Rodrigues(x[:3])[0] - - if np.dot(R, v_up)[2] < 0: - return 1e10 - - xyz = np.dot(R, level_pts) - #err = max(xyz[2]) - min(xyz[2]) - err = np.std(xyz[2]) - #print(x, err) - return err - - -# Solve for optimal transform. -best_err = np.inf -for _ in range(100): - x = (np.random.rand(3)*2 - 1)*np.pi - - for _ in range(5): - x = minimize(level_err, x, tol=1e-12).x - - err_ = level_err(x) - if err_ < best_err: - best_x = x - best_err = err_ - -print('Final level error:', best_err) - - -# Rotation matrix that puts model right-side up and level. -R = cv2.Rodrigues(best_x)[0] - -if False: - # Show points plotted on image. - xyz = np.dot(R, level_pts) - fig = plt.figure() - ax = fig.add_subplot(111, projection='3d') - plt.plot(xyz[0], xyz[1], xyz[2], '.') - plt.xlabel('Model X') - plt.ylabel('Model Y') - - -if False: - fig = plt.figure() - ax = fig.add_subplot(111, projection='3d') - plt.plot(model_xyz[0], model_xyz[1], model_xyz[2], '.') - plt.xlabel('Model X') - plt.ylabel('Model Y') - -# Pick a reasonable origin to start. -lat0, lon0 = np.mean(latlonalt, axis=0)[:2] - -easting_northing = np.array([llh_to_enu(_[0], _[1], 0, lat0, lon0, 0)[:2] - for _ in latlonalt]).T - -if False: - fig = plt.figure() - plt.plot(easting_northing[0], easting_northing[1], '.') - plt.xlabel('Easting (m)') - plt.ylabel('Northing (m)') - -# Create homogenous 2-D leveled version of model points. -model_xyz_leveled = np.dot(R, model_xyz) -model_xyz_leveled[2] = 1 - -if False: - fig = plt.figure() - ax = fig.add_subplot(111, projection='3d') - plt.plot(model_xyz_leveled[0], model_xyz_leveled[1], model_xyz_leveled[2], - '.') - plt.xlabel('Model X') - plt.ylabel('Model Y') - - -def get_m(x): - theta = x[0] - scale = x[1] - tx = x[2] - ty = x[3] - - c = np.cos(theta) - s = np.sin(theta) - - R_ = np.identity(4) - R_[:2, :2] = np.array([[c, -s], [s, c]]) - - S_ = np.identity(4) - S_[:3, :3] *= scale - - T_ = np.identity(4) - T_[0, 3] = -tx - T_[1, 3] = -ty - - return np.dot(T_, np.dot(S_, R_)) - - -def align_err(x): - if x[1] < 0: - return 1e10 - - m = get_m(x) - m = m[:, [0, 1, 3]] - err = easting_northing - np.dot(m, model_xyz_leveled)[:2] - err = np.sqrt(np.mean(np.sum(err**2, axis=0))) - print('Error in meters rms:', err) - return err - - -def plot_err(x): - m = get_m(x) - m = m[:, [0, 1, 3]] - pts1 = np.dot(m, model_xyz_leveled)[:2] - plt.plot(pts1[0], pts1[1], 'ro') - plt.plot(easting_northing[0], easting_northing[1], 'bo') - for i in range(pts1.shape[1]): - plt.plot([pts1[0], easting_northing[0]], - [pts1[1], easting_northing[1]], 'b-') - - -# Solve for optimal transform. -x = np.array([np.random.rand(1)[0]*np.pi*2, 10, 0, 0]) -for _ in range(4): - x = minimize(align_err, x).x - -M2 = get_m(x) -M1 = np.identity(4) -M1[:3, :3] = R -M = np.dot(M2, M1) - -# M now warps from model coordinates to east/northing/up at lat0 and lon0. But, -# we want to update lat0, lon0, and h0 such that origin_xyz is (0, 0, 0). -enu0 = np.dot(M, np.hstack([origin_xyz, 1]))[:3] -lat0, lon0, h0 = enu_to_llh(enu0[0], enu0[1], enu0[2], lat0, lon0, 0) - -if False: - # Define origin_xy - llh_to_enu(enu0[0], enu0[1], enu0[2], lat0, lon0, 0) - -M[:3, 3] -= np.dot(M, np.hstack([origin_xyz, 1]))[:3] - -print('Should be all zeros', np.dot(M, np.hstack([origin_xyz, 1]))[:3]) - -print('Transformation from model coordinates to east/north/up meters relative ' - 'to the origin at latitude=%0.8f, latitude=%0.8f, height=%0.3f meters is' - % (lat0, lon0, h0)) -print(M) - - -with open(result_fname, 'w') as f: - f.write('# This registration matrix accepts coordinates in the ' - 'coordinates system used by\n# the structure-from-motion ' - 'reconstruction and returns easting, northing, up\n# coordinates ' - 'in meters. Therefere, it applies to the reconstructed poses of ' - 'the\n# cameras and sparse and dense point reconstructions.\n\n') - for i in range(4): - f.write('%0.10f %0.10f %0.10f %0.10f\n' % (tuple(M[i]))) - - f.write('\n# The origin of the east-north-up coordinate system is located ' - 'at:\nlatitude0 = %0.8f # degrees\nlongitude0 = %0.8f # ' - 'degrees\nheight0 = %0.3f # meters above WGS84 ' - 'ellipsoid\n' % (lat0, lon0, h0)) diff --git a/kamera/colmap_processing/scripts/get_first_frame_per_video.py b/kamera/colmap_processing/scripts/get_first_frame_per_video.py deleted file mode 100644 index f4393ca..0000000 --- a/kamera/colmap_processing/scripts/get_first_frame_per_video.py +++ /dev/null @@ -1,29 +0,0 @@ -from __future__ import division, print_function -import numpy as np -import os -import cv2 -import subprocess -import matplotlib.pyplot as plt -import glob -import natsort - -base_dir = '/media/in' -out_dir = '/media/out' - - -list_of_files = [] -for (dirpath, dirnames, filenames) in os.walk(base_dir): - for filename in filenames: - if (filename.endswith('.mp4') or filename.endswith('.MP4') - or filename.endswith('.avi')): - list_of_files.append(os.sep.join([dirpath, filename])) - -for fname in list_of_files: - cap = cv2.VideoCapture(fname) - ret, frame = cap.read() - if frame is not None: - print(frame.shape) - - base = os.path.splitext(os.path.split(fname)[1])[0] - fname = '%s/%s.png' % (out_dir, base) - cv2.imwrite(fname, frame) diff --git a/kamera/colmap_processing/scripts/image_to_geotiff.py b/kamera/colmap_processing/scripts/image_to_geotiff.py index abf54db..4d18cac 100644 --- a/kamera/colmap_processing/scripts/image_to_geotiff.py +++ b/kamera/colmap_processing/scripts/image_to_geotiff.py @@ -1,37 +1,4 @@ #! /usr/bin/python -""" -ckwg +31 -Copyright 2018 by Kitware, Inc. -All rights reserved. - -Redistribution and use in source and binary forms, with or without -modification, are permitted provided that the following conditions are met: - - * Redistributions of source code must retain the above copyright notice, - this list of conditions and the following disclaimer. - - * Redistributions in binary form must reproduce the above copyright notice, - this list of conditions and the following disclaimer in the documentation - and/or other materials provided with the distribution. - - * Neither name of Kitware, Inc. nor the names of any contributors may be used - to endorse or promote products derived from this software without specific - prior written permission. - -THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS ``AS IS'' -AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHORS OR CONTRIBUTORS BE LIABLE FOR -ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - -============================================================================== - -""" from __future__ import division, print_function import numpy as np import os @@ -46,8 +13,8 @@ from scipy.spatial import ConvexHull from scipy.interpolate import griddata -import colmap_processing.vtk_util as vtk_util -from colmap_processing.geo_conversions import enu_to_llh +import kamera.colmap_processing.vtk_util as vtk_util +from kamera.colmap_processing.geo_conversions import enu_to_llh src_fname = '' diff --git a/kamera/colmap_processing/scripts/manual_cal_landmark_gui.py b/kamera/colmap_processing/scripts/manual_cal_landmark_gui.py deleted file mode 100644 index 376f855..0000000 --- a/kamera/colmap_processing/scripts/manual_cal_landmark_gui.py +++ /dev/null @@ -1,168 +0,0 @@ -#! /usr/bin/python -""" -ckwg +31 -Copyright 2018 by Kitware, Inc. -All rights reserved. - -Redistribution and use in source and binary forms, with or without -modification, are permitted provided that the following conditions are met: - - * Redistributions of source code must retain the above copyright notice, - this list of conditions and the following disclaimer. - - * Redistributions in binary form must reproduce the above copyright notice, - this list of conditions and the following disclaimer in the documentation - and/or other materials provided with the distribution. - - * Neither name of Kitware, Inc. nor the names of any contributors may be used - to endorse or promote products derived from this software without specific - prior written permission. - -THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS ``AS IS'' -AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHORS OR CONTRIBUTORS BE LIABLE FOR -ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - -============================================================================== - -""" -import sys -import sqlite3 -import numpy as np -import glob -import os -from scipy.spatial import distance_matrix -import cv2 -import matplotlib.pyplot as plt - -# Colmap Processing imports. -from colmap_processing.geo_conversions import llh_to_enu -from colmap_processing.database import COLMAPDatabase, blob_to_array, \ - pair_id_to_image_ids - - -# ---------------------------------------------------------------------------- -# Landmark annotation GUI workspace path. -landmark_gui_workspace = '/media/workspace' - -# Colmap directory where all of the Colmap files will be generated. All images -# should be placed in a sub-directory 'images'. -colmap_workspace_path = '/media/colmap' - -# Camera model type. -model = 'OPENCV' -# ---------------------------------------------------------------------------- - - -db = COLMAPDatabase('%s/colmap_database.db' % colmap_workspace_path) -db.create_tables() - -image_dir = '%s/images0' % colmap_workspace_path - -image_fnames = [] -for ext in ['.jpg', '.tiff', '.png', '.bmp', '.jpeg']: - image_fnames = image_fnames + glob.glob('%s/*%s' % (image_dir, ext)) - - -img_fname_to_cam_id = {} -img_fname_to_img_id = {} -for image_id, fname in enumerate(image_fnames): - image_id = image_id + 1 - img = cv2.imread(fname, 0) - image_fname = os.path.splitext(os.path.split(fname)[1])[0] - - height, width = img.shape[:2] - - if model == 'OPENCV': - f = 1.501825029214076494e+03 - k1 = -3.539975553110535356e-01 - k2 = 1.221648902040088081e-01 - params = [f, f, width/2.0, height/2.0, k1, k2, 0, 0] - model_ind = 4 - else: - raise Exception('Need to implement for model \'%s\'' % model) - - # Every image is from a different camera. - camera_id = image_id - - img_fname_to_img_id[image_fname] = image_id - img_fname_to_cam_id[image_fname] = camera_id - - db.add_camera(model_ind, width, height, params, prior_focal_length=1500, - camera_id=camera_id) - db.add_image(os.path.split(fname)[1], camera_id, image_id=image_id) - - -# Parse landmark annotation GUI workspace. -img_keypoints_to_landmark = {} -for image_id, fname in enumerate(image_fnames): - img_fname = os.path.splitext(os.path.split(fname)[1])[0] - fname = ('%s/%s_image_points.txt' % (landmark_gui_workspace, img_fname)) - try: - points = np.loadtxt(fname) - except (OSError, IOError): - points = np.zeros((0, 3)) - - image_id = img_fname_to_img_id[img_fname] - camera_id = img_fname_to_cam_id[img_fname] - - db.add_keypoints(image_id, points[:, 1:]) - - img_keypoints_to_landmark[image_id] = points[:, 0].astype(np.int).tolist() - - -img_w_matches = list(img_keypoints_to_landmark.keys()) -for ii in range(len(img_w_matches) - 1): - image_id1 = img_w_matches[ii] - matches1 = img_keypoints_to_landmark[image_id1] - for jj in range(ii + 1, len(img_w_matches)): - image_id2 = img_w_matches[jj] - matches2 = img_keypoints_to_landmark[image_id2] - - matches = [] - for i in range(len(matches1)): - try: - j = matches2.index(matches1[i]) - matches.append([i, j]) - except ValueError: - pass - - if len(matches) > 0: - matches = np.array(matches, dtype=np.int) - db.add_matches(image_id1, image_id2, matches) - db.add_two_view_geometry(image_id1, image_id2, matches) - -db.commit() -db.close() - - -db = COLMAPDatabase('/media/colmap_database.db') - - -# Read and check cameras. -rows = db.execute("SELECT * FROM cameras") -for row in rows: - camera_id, model, width, height, params, prior = row - params = blob_to_array(params, np.float64) - print('Camera ID: %s Model: %s Resolution (%i, %i) Params: %s' % - (camera_id, model, width, height, params)) - -# Read and check images. -rows = db.execute("SELECT * FROM images") -for row in rows: - camera_id, name, image_id = row[:3] - print('Camera ID: %s Name: %s Image ID: %s' % - (camera_id, name, image_id)) - -# Read and check matches. -db.get_all_pair_id() -db.get_match_dictionary() -db.get_keypoint_from_image_dict() - -db.close() diff --git a/kamera/colmap_processing/scripts/point_warping_test.py b/kamera/colmap_processing/scripts/point_warping_test.py deleted file mode 100644 index 0936486..0000000 --- a/kamera/colmap_processing/scripts/point_warping_test.py +++ /dev/null @@ -1,329 +0,0 @@ -from __future__ import division, print_function -import numpy as np -import os -import cv2 -import matplotlib.pyplot as plt -from osgeo import osr, gdal -import PIL - -# Colmap Processing imports. -from colmap_processing.geo_conversions import llh_to_enu -from colmap_processing.colmap_interface import read_images_binary, Image, \ - read_points3D_binary, read_cameras_binary, qvec2rotmat -from colmap_processing.database import COLMAPDatabase, pair_id_to_image_ids, blob_to_array -import colmap_processing.vtk_util as vtk_util -from colmap_processing.geo_conversions import enu_to_llh, llh_to_enu, \ - rmat_ecef_enu, rmat_enu_ecef -from colmap_processing.static_camera_model import save_static_camera, \ - load_static_camera_from_file, write_camera_krtd_file - -camera_model1 = '/home/user/camera_model1.yaml' -camera_model2 = '/home/user/camera_model2.yaml' -base_layer_fname = '/home/user/3d_models/base_layer.tif' -ponts_fname = '/home/user/points.txt' - -latitude0 = 0 # degrees -longitude0 = 0 # degrees -height0 = 0 - - -def load_camera_model(camera_model): - ret = load_static_camera_from_file(camera_model) - K, d, R, depth_map, latitude, longitude, altitude = ret[2:] - img_fname = '%s/ref_view.png' % os.path.split(camera_model)[0] - image = cv2.imread(img_fname) - return image, K, d, R, depth_map, latitude, longitude, altitude - -image1, K1, d1, R1, depth_map1, latitude1, longitude1, altitude1 = load_camera_model(camera_model1) -image2, K2, d2, R2, depth_map2, latitude2, longitude2, altitude2 = load_camera_model(camera_model2) - - -def unproject_from_camera(im_pts, K, d, R, depth_map, latitude, longitude, altitude): - # Unproject rays into the camera coordinate system. - ray_dir = np.ones((3, len(im_pts)), dtype=np.float) - ray_dir0 = cv2.undistortPoints(np.expand_dims(im_pts, 0), K, d, R=None) - ray_dir[:2] = np.squeeze(ray_dir0, 0).T - - enu0 = llh_to_enu(latitude, longitude, altitude, latitude0, longitude0, - height0) - enu0 = np.array(enu0) - - # Rotate rays into the local east/north/up coordinate system. - ray_dir = np.dot(R.T, ray_dir) - - height, width = depth_map.shape - enu = np.zeros((len(im_pts), 3)) - for i in range(im_pts.shape[0]): - x, y = im_pts[i] - if x == 0: - ix = 0 - elif x == width: - ix = int(width - 1) - else: - ix = int(round(x - 0.5)) - - if y == 0: - iy = 0 - elif y == height: - iy = int(height - 1) - else: - iy = int(round(y - 0.5)) - - if ix < 0 or iy < 0 or ix >= width or iy >= height: - print(x == width) - print(y == height) - raise ValueError('Coordinates (%0.1f,%0.f) are outside the ' - '%ix%i image' % (x, y, width, height)) - - enu[i] = enu0 + ray_dir[:, i]*depth_map[iy, ix] - - return enu - - -def project_to_camera(wrld_pts, K, d, R, depth_map, latitude, longitude, altitude): - # Unproject rays into the camera coordinate system. - cam_pos = llh_to_enu(latitude, longitude, altitude, latitude0, longitude0, - height0) - tvec = -np.dot(R, cam_pos).ravel() - rvec = cv2.Rodrigues(R)[0] - im_pts = cv2.projectPoints(wrld_pts, rvec, tvec, K, d)[0] - im_pts = np.squeeze(im_pts) - - return im_pts - - -class GeoImage(object): - """Representation of a georeferenced image. - - """ - def __init__(self): - self._dem = None - - @staticmethod - def load_geotiff(fname): - self = GeoImage() - - if fname is None: - return - - if not os.path.isfile(fname): - print('Could not open file \'%s\'' % fname) - return - - ds = gdal.Open(fname, gdal.GA_ReadOnly) - if ds.RasterCount > 1: - raw_image = np.zeros((ds.RasterYSize,ds.RasterXSize,3), dtype=np.uint8) - for i in range(3): - raw_image[:,:,i] = ds.GetRasterBand(i+1).ReadAsArray() - else: - band = ds.GetRasterBand(1) - raw_image = band.ReadAsArray() - - self._array = raw_image - - # Create lat/lon coordinate system. - wgs84_wkt = """ - GEOGCS["WGS 84", - DATUM["WGS_1984", - SPHEROID["WGS 84",6378137,298.257223563, - AUTHORITY["EPSG","7030"]], - AUTHORITY["EPSG","6326"]], - PRIMEM["Greenwich",0, - AUTHORITY["EPSG","8901"]], - UNIT["degree",0.01745329251994328, - AUTHORITY["EPSG","9122"]], - AUTHORITY["EPSG","4326"]]""" - wgs84_cs = osr.SpatialReference() - wgs84_cs.ImportFromWkt(wgs84_wkt) - - image_cs= osr.SpatialReference() - image_cs.ImportFromWkt(ds.GetProjectionRef()) - - # Create a transform object to convert between projection of the image - # and WGS84 coordinates. - self._to_lla_tform = osr.CoordinateTransformation(image_cs, wgs84_cs) - self._from_lla_tform = osr.CoordinateTransformation(wgs84_cs, image_cs) - - # See documentation for gdal GetGeoTransform(). - geo_tform_mat = np.identity(3) - c = ds.GetGeoTransform() - geo_tform_mat[0,0] = c[1] - geo_tform_mat[0,1] = c[2] - geo_tform_mat[0,2] = c[0] - geo_tform_mat[1,0] = c[4] - geo_tform_mat[1,1] = c[5] - geo_tform_mat[1,2] = c[3] - self.geo_tform_mat = geo_tform_mat[:2] - self.geo_inv_tform_mat = np.linalg.inv(geo_tform_mat)[:2] - - # Try to load DEM if available. - try: - fname, ext = os.path.splitext(fname) - depth_map = np.asarray(PIL.Image.open('%s.dem.tif' % fname)) - self._dem = depth_map - print('Loaded') - except (OSError, IOError): - self._dem = None - - return self - - @property - def array(self): - return self._array - - def get_lon_lat_from_im_pt(self, im_pt): - """Return latitude and longitude for image point. - - :param pos: Raw image coordinates of the geotiff that were clicked. - :type pos: 2-array - - :return: Longitude (degrees) and latitude (degrees) associated with - the clicked point. - :rtype: 3-array - - """ - # Convert from image coordinates to the coordinates of the projection - # encoded in the geotiff. - Xp,Yp = np.dot(self.geo_tform_mat, [im_pt[0],im_pt[1],1]) - lon,lat,_ = self._to_lla_tform.TransformPoint(Xp, Yp) - - if self._dem is not None: - height, width = self._dem.shape[:2] - x, y = im_pt - if x == 0: - ix = 0 - elif x == width: - ix = int(width - 1) - else: - ix = int(round(x - 0.5)) - - if y == 0: - iy = 0 - elif y == height: - iy = int(height - 1) - else: - iy = int(round(y - 0.5)) - - h = self._dem[iy, ix] - - return lon, lat, h - - return lon, lat - - def get_im_pt_from_lon_lat(self, lon, lat): - """Return image coordinates for latitude and longitude. - - :param lon: Longitude (degrees). - :type lon: float - - :param lat: Latitude (degrees). - :type lat: float - - :return: Raw image coordinates of the geotiff. - :rtype: 2-array - - """ - Xp,Yp,_ = self._from_lla_tform.TransformPoint(lon, lat) - - # Convert from image coordinates to the coordinates of the projection - # encoded in the geotiff. - im_pt = np.dot(self.geo_inv_tform_mat, [Xp,Yp,1]) - return im_pt - - -base_layer = GeoImage.load_geotiff(base_layer_fname) - -# ---------------------------------------------------------------------------- -# Clicked points in both camera1 and base layer. -res = np.loadtxt('/home/user/geo_points_test.txt') -im_pts0 = res[:, :2] -llh0 = res[:, 2:] - -wrld_pts = np.array([llh_to_enu(_[0], _[1], _[2], latitude0, longitude0, height0) - for _ in llh0]) -im_pts = project_to_camera(wrld_pts, K1, d1, R1, depth_map1, latitude1, longitude1, altitude1) -plt.imshow(image1) -#plt.plot(im_pts0[:, 0], im_pts0[:, 1], 'go') -plt.plot(im_pts[:, 0], im_pts[:, 1], 'ro') - - -# Project from camera 1 into the world. -wrld_pts1 = unproject_from_camera(im_pts0, K1, d1, R1, depth_map1, latitude1, longitude1, altitude1) -im_pts = project_to_camera(wrld_pts, K1, d1, R1, depth_map1, latitude1, longitude1, altitude1) -plt.plot(im_pts[:, 0], im_pts[:, 1], 'bo') - -# Compare 3-D points selected from base-layer image versus unprojected from -# camera. -plt.figure() -plt.plot(wrld_pts[:, 0], wrld_pts[:, 1], 'go') -plt.plot(wrld_pts1[:, 0], wrld_pts1[:, 1], 'bo') - - - - - - - - -# Project from camera 2 into the world. -wrld_pts = unproject_from_camera(im_pts0, K1, d1, R1, depth_map1, latitude1, longitude1, altitude1) -llh = [enu_to_llh(_[0], _[1], _[2], latitude0, longitude0, height0) - for _ in wrld_pts] -base_pts1 = [base_layer.get_im_pt_from_lon_lat(llh[1], llh[0]) for llh in llh] -base_pts1 = np.array(base_pts1) -base_pts0 = [base_layer.get_im_pt_from_lon_lat(llh[1], llh[0]) for llh in llh0] -base_pts0 = np.array(base_pts0) - -plt.figure() -plt.imshow(base_layer.array) -plt.plot(base_pts1[:, 0], base_pts1[:, 1], 'go') -plt.plot(base_pts0[:, 0], base_pts0[:, 1], 'ro') - - -# ---------------------------------------------------------------------------- - -points = np.loadtxt(ponts_fname) -pts1 = points[:, :2] -pts2 = points[:, 2:] - -# Project from camera 1 into the world. -wrld_pts1 = unproject_from_camera(pts1, K1, d1, R1, depth_map1, latitude1, longitude1, altitude1) - -# Project from camera 2 into the world. -wrld_pts2 = unproject_from_camera(pts2, K2, d2, R2, depth_map2, latitude2, longitude2, altitude2) - -plt.close('all') -plt.figure() -plt.imshow(base_layer.array) - -llh1 = [enu_to_llh(_[0], _[1], _[2], latitude0, longitude0, height0) - for _ in wrld_pts1] -base_pts1 = [base_layer.get_im_pt_from_lon_lat(llh[1], llh[0]) for llh in llh1] -base_pts1 = np.array(base_pts1) -plt.plot(base_pts1[:, 0], base_pts1[:, 1], 'go') - -# Plot points from image 2 on base layer. -llh2 = [enu_to_llh(_[0], _[1], _[2], latitude0, longitude0, height0) - for _ in wrld_pts2] -base_pts2 = [base_layer.get_im_pt_from_lon_lat(llh[1], llh[0]) for llh in llh2] -base_pts2 = np.array(base_pts2) -plt.plot(base_pts2[:, 0], base_pts2[:, 1], 'ro') - - - -plt.subplot(1, 2, 1) -plt.imshow(image1) -plt.plot(pts1[:, 0], pts1[:, 1], 'go') - - -#wrld_pts = unproject_from_camera(pts1, K1, d1, R1, depth_map1, latitude1, longitude1, altitude1) -im_pts = project_to_camera(wrld_pts, K1, d1, R1, depth_map1, latitude1, longitude1, altitude1) -plt.plot(im_pts[:, 0], im_pts[:, 1], 'ro') - -plt.subplot(1, 2, 2) -plt.imshow(image2) -plt.plot(pts2[:, 0], pts2[:, 1], 'go') - -# Project from camera 1 onto camera 2. -im_pts = project_to_camera(wrld_pts, K2, d2, R2, depth_map2, latitude2, longitude2, altitude2) -plt.plot(im_pts[:, 0], im_pts[:, 1], 'ro') \ No newline at end of file diff --git a/kamera/colmap_processing/scripts/prune_snapshot_dir.py b/kamera/colmap_processing/scripts/prune_snapshot_dir.py deleted file mode 100755 index b4843ab..0000000 --- a/kamera/colmap_processing/scripts/prune_snapshot_dir.py +++ /dev/null @@ -1,16 +0,0 @@ -import glob -import shutil -import time - -snap_shot_dir = '/mnt/data/colmap/snapshots/*' -num_to_keep = 10 - -while True: - dirs = glob.glob(snap_shot_dir) - dirs = sorted(dirs) - print('Found %i subdirectories' % len(dirs)) - for d in dirs[:-num_to_keep]: - print('Deleting \'%s\'' % d) - shutil.rmtree(d) - - time.sleep(10) \ No newline at end of file diff --git a/kamera/colmap_processing/scripts/pyembree_example.py b/kamera/colmap_processing/scripts/pyembree_example.py deleted file mode 100644 index b6088bd..0000000 --- a/kamera/colmap_processing/scripts/pyembree_example.py +++ /dev/null @@ -1,102 +0,0 @@ -from __future__ import division, print_function -import numpy as np -import os -import cv2 -import subprocess -import matplotlib.pyplot as plt -import glob -import natsort -import trimesh -import math -import PIL -from osgeo import osr, gdal - - -from pyembree import rtcore_scene as rtcs -from pyembree.mesh_construction import TriangleMesh - - -# Determine the bounds of the model. -mesh_fname = 'coarse.ply' -mesh = trimesh.load(mesh_fname) -embree_scene = rtcs.EmbreeScene() -vertices = mesh.vertices.astype(np.float32) -faces = mesh.faces -embree_mesh = TriangleMesh(embree_scene, vertices, faces) - - -height, width, K, R, dist, cam_pos = height1, width1, K1, R1, dist1, cam_pos1 -#height, width, K, R, dist, cam_pos = height2, width2, K2, R2, dist2, cam_pos2 - -X, Y = np.meshgrid(np.arange(width, dtype=np.float32) + 0.5, - np.arange(height, dtype=np.float32) + 0.5) -points = np.vstack([X.ravel(), Y.ravel()]) -ray_dir = cv2.undistortPoints(np.expand_dims(points.T, 0), K, dist, None) -ray_dir = np.squeeze(ray_dir, 0).T -ray_dir = np.vstack([ray_dir, np.ones(ray_dir.shape[1])]) -ray_dir = np.dot(R.T, ray_dir).astype(np.float32) -ray_dir /= np.sqrt(np.sum(ray_dir**2, 0)) - -origins = np.zeros((ray_dir.shape[1], 3), dtype=np.float32) -origins[:, 0] = cam_pos[0] -origins[:, 1] = cam_pos[1] -origins[:, 2] = cam_pos[2] -res = embree_scene.run(origins, ray_dir.T, output=1) - -ray_inter = res['geomID'] >= 0 -depth = np.zeros(ray_dir.shape[1], dtype=np.float32) -depth[~ray_inter] = np.nan -print('Intersection coordinates') -primID = res['primID'][ray_inter] -u = res['u'][ray_inter] -v = res['v'][ray_inter] -w = 1 - u - v - -inters = np.atleast_2d(w).T * vertices[faces[primID][:, 0]] + \ - np.atleast_2d(u).T * vertices[faces[primID][:, 1]] + \ - np.atleast_2d(v).T * vertices[faces[primID][:, 2]] - -# Vector from the intersection point back to the camera. -rays = inters - cam_pos - -# Dot product with the z-axis of the camera is the depth. -depth[ray_inter] = np.dot(rays, R[2]) -depth = np.reshape(depth, (height, width)) - - - -depth_map1 = depth - - - - -def unproject_from_camera_embree(im_pts, K, dist, R, cam_pos, embree_scene): - # Unproject rays into the camera coordinate system. - ray_dir = np.ones((3, len(im_pts)), dtype=np.float32) - ray_dir0 = cv2.undistortPoints(np.expand_dims(im_pts, 0), K, dist, R=None) - ray_dir[:2] = np.squeeze(ray_dir0, 0).T - ray_dir = np.dot(R.T, ray_dir).astype(np.float32).T - - origins = np.zeros((ray_dir.shape[1], 3), dtype=np.float32) - origins[:, 0] = cam_pos[0] - origins[:, 1] = cam_pos[1] - origins[:, 2] = cam_pos[2] - res = embree_scene.run(origins, ray_dir, output=1) - - ray_inter = res['geomID'] >= 0 - depth = np.zeros(ray_dir.shape[1], dtype=np.float32) - depth[~ray_inter] = np.nan - primID = res['primID'][ray_inter] - u = res['u'][ray_inter] - v = res['v'][ray_inter] - w = 1 - u - v - - points = np.zeros((ray_dir.shape[1], 3), dtype=np.float32) - points[~ray_inter] = np.nan - - points[ray_inter] = np.atleast_2d(w).T * vertices[faces[primID][:, 0]] + \ - np.atleast_2d(u).T * vertices[faces[primID][:, 1]] + \ - np.atleast_2d(v).T * vertices[faces[primID][:, 2]] - - - return points.T \ No newline at end of file diff --git a/kamera/colmap_processing/scripts/reduce_image_set.py b/kamera/colmap_processing/scripts/reduce_image_set.py deleted file mode 100644 index 95bea99..0000000 --- a/kamera/colmap_processing/scripts/reduce_image_set.py +++ /dev/null @@ -1,519 +0,0 @@ -#!/usr/bin/env python -""" -ckwg +31 -Copyright 2019 by Kitware, Inc. -All rights reserved. - -Redistribution and use in source and binary forms, with or without -modification, are permitted provided that the following conditions are met: - - * Redistributions of source code must retain the above copyright notice, - this list of conditions and the following disclaimer. - - * Redistributions in binary form must reproduce the above copyright notice, - this list of conditions and the following disclaimer in the documentation - and/or other materials provided with the distribution. - - * Neither name of Kitware, Inc. nor the names of any contributors may be used - to endorse or promote products derived from this software without specific - prior written permission. - -THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS ``AS IS'' -AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHORS OR CONTRIBUTORS BE LIABLE FOR -ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - -============================================================================== - -""" -from __future__ import division, print_function, absolute_import -import numpy as np -import matplotlib.pyplot as plt -from scipy.sparse import lil_matrix -import time -from sklearn.decomposition import PCA -from scipy.spatial import KDTree -import threading -from numba import jit - -# Colmap Processing imports. -from colmap_processing.geo_conversions import llh_to_enu -from colmap_processing.colmap_interface import read_images_binary, Image, \ - read_points3D_binary, read_cameras_binary, qvec2rotmat -from colmap_processing.database import COLMAPDatabase, pair_id_to_image_ids, blob_to_array - - -# ---------------------------------------------------------------------------- -# Base path to the colmap directory. -image_dir = 'test' - -# Path to the images.bin file. -images_bin_fname = 'images.bin' -images_bin_fname = 'images.bin' - -camera_bin_fname = 'cameras.bin' -camera_bin_fname = 'cameras.bin' - -# Path to the points3D.bin file. -points_3d_bin_fname = 'points3D.bin' -points_3d_bin_fname = 'points3D.bin' - -# Path to .db file. -database_fname = 'database.db' - -# Reduced point cloud. -reduced_point_cloud = 'small.asc' - -# Read in the details of all images. -images = read_images_binary(images_bin_fname) - -cameras = read_cameras_binary(camera_bin_fname) - -# Get position of camera corresponding to each image. -cam_pos = {} -for image_id in images: - image = images[image_id] - R = qvec2rotmat(image.qvec) - cam_pos[image_id] = np.dot(-R.T, image.tvec) - - -# Remove image keypoints without associated reconstructed 3-D point. -image_ids = list(images.keys()) -for image_num in images: - image = images[image_num] - ind = [_ for _ in range(len(image.xys)) if image.point3D_ids[_] != -1] - xys = image.xys[ind] - point3D_ids = image.point3D_ids[ind] - - images[image_num] = Image(id=image.id, qvec=image.qvec, tvec=image.tvec, - camera_id=image.camera_id, name=image.name, - xys=xys, point3D_ids=point3D_ids) - -pts_3d = read_points3D_binary(points_3d_bin_fname) - -if False: - pts = np.array([pts_3d[_].xyz for _ in pts_3d]) - rgb = [pts_3d[_] for _ in pts_3d] - pca = PCA(n_components=2) - pts_2d = pca.fit_transform(pts).T - plt.plot(pts_2d[0], pts_2d[1], '.', c=rgb) - -if True: - # Filter down 3d pts. - pts_3d_keys = list(pts_3d.keys()) - pts = np.array([pts_3d[_].xyz for _ in pts_3d_keys]) - red_pts = np.loadtxt(reduced_point_cloud)[:, :3] - - kdtree = KDTree(pts) - - inds = set() - for i, red_pt in enumerate(red_pts): - print('Processing', i + 1, 'of', len(red_pts)) - inds.add(kdtree.query(red_pt, distance_upper_bound=0.1)[1]) - - pts_3d = {pts_3d_keys[_]: pts_3d[pts_3d_keys[_]] for _ in inds} - pts_3d_id = set(pts_3d.keys()) - - images0 = images - images = {} - num_points_in_image = [] - for key in images0: - image = images0[key] - #xys = image.xys - point3D_ids = image.point3D_ids - ind = [i for i in range(len(point3D_ids)) - if point3D_ids[i] in pts_3d_id] - #xys = [xys[_] for _ in ind] - point3D_ids = [point3D_ids[_] for _ in ind] - s = len(ind) - if s > 20: - images[image.id] = Image(id=image.id, qvec=image.qvec, - tvec=image.tvec, - camera_id=image.camera_id, - name=image.name, xys=xys, - point3D_ids=point3D_ids) - num_points_in_image.append(s) -# ---------------------------------------------------------------------------- - - -# ---------------------------------------------------------------------------- -if False: - L = len(images) - num_matches = np.zeros((L, L), np.int) - pt3d_ids = [set(images[_].point3D_ids) for _ in images] - for i in range(L): - print('Processing %i/%i' % (i + 1, L)) - for j in range(i + 1, L): - s = len(pt3d_ids[i].intersection(pt3d_ids[j])) - num_matches[i, j] = s - - m = num_matches + num_matches.T - - -# Start by picking the view with the most 3-D points -ind = np.argsort([len(images[key].point3D_ids) for key in image_ids]) -image_id0s = [image_ids[_] for _ in ind] - - -def get_inv_depth_accuracy_array(i, j, inv_depth_accuracy): - ind1 = min([i, j]) - ind2 = max([i, j]) - return inv_depth_accuracy[:, ind1*num_images + ind2] - - -for image_id0 in image_id0s: - s0 = set(images[image_id0].point3D_ids) - num_matches = [] - for image_id in image_ids: - if image_id0 == image_id: - num_matches.append(0) - continue - - num_matches.append(len(set(images[image_id].point3D_ids).intersection(s0))) - - num_matches = np.array(num_matches) - inds = np.argsort(num_matches)[::-1] - inds = inds[num_matches[inds] > 0.95 * num_matches[inds[0]]] - - if len(inds) == 1: - continue - - raise Exception() - image_ids_to_consider = [image_ids[_] for _ in inds] - - # We want to consider all other images in 'image_ids_to_consider' to see if - # we can pick two element from this set and when considered with image_id0, - # one is sufficiently redundant to the other two. - - das = [] - for i in image_ids_to_consider: - das.append(get_inv_depth_accuracy_array(image_id0, i, depth_accuracy)) - - sum((das[0] < das[1]).toarray())/sum((das[0] > 0).toarray()) - - das = np.array(das) - - - - da1 = depth_accuracy[:, ind1*num_images + ind2].toarray() - depth_accuracy - - - num_matches - - num_matches = [sum([i in pts_3d[pt3d_id].image_ids for pt3d_id in pts_3d]) - for i in image_ids] -# ---------------------------------------------------------------------------- - - -# ---------------------------------------------------------------------------- -# Consider which points are visible from which cameras. -image_ids = list(images.keys()) - -all_id = [] -for i in images: - all_id = all_id + list(images[i].point3D_ids) - -all_id = list(set(all_id)) -all_id.sort() - -pt3d_id_map = {all_id[_]: _ for _ in range(len(all_id))} -visibility = lil_matrix((len(all_id), len(images)), dtype=np.bool) -L = len(images) -for i in range(L): - print('Considering results for image %i/%i' % (i + 1, L)) - image = images[image_ids[i]] - ids = np.array([pt3d_id_map[_] for _ in image.point3D_ids]) - visibility[ids, i] = 1 - -visibility = visibility.tocsc() -counts = np.sum(visibility, axis=0).tolist()[0] - - -removed_image = [] -num_points_after_removal = [] -while True: - # visibility[i, j] indicates where the ith 3-D point is visible by images[j]. - c0 = sum(np.array(np.sum(visibility, axis=1).tolist()).ravel() > 3) - without_i = [] - L = visibility.shape[1] - for i in range(L): - print('Considering results without image %i/%i' % (i + 1, L)) - mask = np.ones(L, np.bool) - mask[i] = 0 - c = sum(np.array(np.sum(visibility[:, mask], axis=1).tolist()).ravel() > 3) - without_i.append(c) - - ind = np.argmax(without_i) - num_missed_pts = c0 - without_i[ind] - - print('Removing image %s drops from %i->%i 3-D points visible ' - '(difference=%i) ' % (str(images[image_ids[ind]].id), without_i[ind], - c0, num_missed_pts)) - - if num_missed_pts > 5000: - break - - # Remove image ind. - removed_image.append(images[image_ids[ind]]) - num_points_after_removal.append(without_i[ind]) - - del images[image_ids[ind]] - image_ids = list(images.keys()) - mask = np.ones(L, np.bool) - mask[ind] = 0 - visibility = visibility[:, mask] -# ---------------------------------------------------------------------------- - - -# ---------------------------------------------------------------------------- -def calculate_pair_point_inv_accuracy(images, min_d_acc=0.001, max_d_acc=0.1, - show_timing=False): - """Return 3-D point's inverse depth accuracy for each image pair. - - :param image_ids: - - :param inv_depth_accuracy: Sparse matrix of size num_3d_points x - (num_images^2) Matrix. For each ith 3-D point considered (indices - aren't related to pts_3d keys) and images with 'images' keys j and k, - where j < k, inv_depth_accuracy[i, j*num_images + k] is the depth - accuracy constrained by the pairing of images j and k. - :type inv_depth_accuracy: scipy csc sparse matrix - - """ - - num_images = len(images) - image_ids = list(images.keys()) - image_ids_set = set(image_ids) - image_id_map = {image_ids[_]: _ for _ in range(len(image_ids))} - - # For each 3-D point, we consider the depth accuracy from each two-view - # geometry. - L = len(pts_3d) - inv_depth_accuracy = lil_matrix((L, num_images*num_images), dtype=np.float32) - t0 = time.time() - for ii, pt_3d_id in enumerate(pts_3d): - pt_3d = pts_3d[pt_3d_id] - image_ids_ = pt_3d.image_ids - image_ids_ = image_ids_set.intersection(set(image_ids_)) - image_ids_ = list(image_ids_) - image_ids_.sort() - - if len(image_ids_) == 0: - continue - - # Unit vectors pointing from cameras to point. - v = [] - - for image_id in image_ids_: - cam_pos1 = cam_pos[image_id] - v.append(pt_3d.xyz - cam_pos1) - - # Normalize. - v = np.array(v) - d = np.sqrt(np.sum(v**2, axis=1)) - v = v / np.atleast_2d(d).T - - dp = np.dot(v, v.T) - dp = np.maximum(dp, -1) - dp = np.minimum(dp, 1) - cos_theta = dp # radians - - # Camera focal length - f = [cameras[images[image_id].camera_id].params[0] - for image_id in image_ids_] - - image_ids_ = [image_id_map[_] for _ in image_ids_] - - for i in range(len(image_ids_)): - ind1 = image_ids_[i] - for j in range(i + 1, len(image_ids_)): - ind2 = image_ids_[j] - - # If theta is the angle between cameras in radians, and one - # camera with focal length f nominally located at d from the - # point changes it image-space position by one pixel, the ray - # angle from that camera changes by 1/f, so the depth estimate - # will change by cos(theta)*d/f - d_acc1 = cos_theta[i, j]*d[i]/f[i] - d_acc2 = cos_theta[i, j]*d[j]/f[j] - d_acc = max([d_acc1, d_acc2]) - - d_acc = max([min_d_acc, d_acc]) - - if d_acc > max_d_acc: - continue - - if False: - jj = np.ravel_multi_index((ind1, ind2), - (num_images, num_images)) - else: - jj = ind1*num_images + ind2 - - inv_depth_accuracy[ii, jj] = 1/d_acc - - if show_timing: - # Time spend so far. - time_so_far = time.time() - t0 - time_per_iter = time_so_far/(ii + 1) - iter_left = L - ii - 1 - print('Calculating pair depth accuracy....time left:', - time_per_iter*iter_left/60, 'minutes') - - inv_depth_accuracy = inv_depth_accuracy.tocsc() - return image_ids, inv_depth_accuracy - - -@jit(nopython=True) -def get_mask_numba(inot, num_images): - # Calculate the masks that should be applied to pair_angles to only - # consider results coming from images that are not inot. - mask = [1 for _ in range(num_images**2)] - - for i in range(num_images): - if i == inot: - for j in range(i + 1, num_images): - mask[i*num_images + j] = 0 - else: - for j in range(i + 1, num_images): - if j == inot: - mask[i*num_images + j] = 0 - - return mask - - -def get_mask(inot, num_images): - # Calculate the masks that should be applied to pair_angles to only - # consider results coming from images that are not inot. - mask = np.ones(num_images**2, dtype=np.bool) - - for i in range(num_images): - if i == inot: - for j in range(i + 1, num_images): - mask[i*num_images + j] = 0 - else: - for j in range(i + 1, num_images): - if j == inot: - mask[i*num_images + j] = 0 - - return mask - - -def get_mask0(inot, num_images): - # Calculate the masks that should be applied to pair_angles to only - # consider results coming from images that are not inot. - mask = np.ones(num_images*num_images, dtype=np.bool) - for i in range(num_images): - for j in range(i + 1, num_images): - if i == inot or j == inot: - ij = np.ravel_multi_index((i, j), (num_images, num_images)) - mask[ij] = 0 - ij = np.ravel_multi_index((j, i), (num_images, num_images)) - mask[ij] = 0 - - return mask - - -class ThreadedProcessing(object): - def __init__(self, images, inv_depth_accuracy, num_threads=1, - show_timing=False): - self.inv_depth_accuracy = inv_depth_accuracy - self.scores_without_image = np.zeros(len(images)) - self.images = images - self.num_images = len(images) - self.num_threads = num_threads - self.finished = np.zeros(self.num_images, dtype=np.bool) - self.show_timing = show_timing - - def process_all_images(self): - t0 = time.time() - thread_pool_dict = {} - for inot in range(self.num_images): - if self.num_threads > 1: - # Multi-threaded. - while True: - for key in list(thread_pool_dict.keys()): - if self.finished[key]: - del thread_pool_dict[key] - - if len(thread_pool_dict) < self.num_threads: - break - - time.sleep(.001) - - thread = threading.Thread(target=self.process_one_image, - args=(inot,)) - thread.start() - thread_pool_dict[inot] = thread - else: - # Single-threaded. - self.process_one_image(inot) - - if self.show_timing: - # Time spend so far. - time_so_far = time.time() - t0 - time_per_iter = time_so_far/(inot + 1) - iter_left = self.num_images - inot - 1 - print('Time left:', time_per_iter*iter_left/60, 'minutes') - - if self.show_timing: - print('Processing took:', (time.time() - t0)/60, 'minutes') - - def get_mask(self, inot): - # Calculate the masks that should be applied to pair_angles to only - # consider results coming from images that are not inot. - mask = np.ones(self.num_images*self.num_images, dtype=np.bool) - for i in range(self.num_images): - for j in range(i + 1, self.num_images): - if i == inot or j == inot: - ij = np.ravel_multi_index((i, j), (self.num_images, - self.num_images)) - mask[ij] = 0 - ij = np.ravel_multi_index((j, i), (self.num_images, - self.num_images)) - mask[ij] = 0 - - return mask - - def process_one_image(self, inot): - tic = time.time() - mask = get_mask(inot, self.num_images) - #print('Calculating mask took:', time.time() - tic) - tic = time.time() - without_i = np.max(inv_depth_accuracy[:, mask], axis=1) - without_i = without_i.data - self.scores_without_image[inot] = sum(without_i) - #print('Analzing pair angles took:', time.time() - tic) - self.finished[inot] = True - - -# Analyze scores when one image is left out. -scores_after_removal = [] -removed_images = [] -while len(images) > 100: - print('Processing current image list of size=%i' % len(images)) - image_ids, inv_depth_accuracy = calculate_pair_point_inv_accuracy(images, - min_d_acc=0.001, - max_d_acc=0.1) - threaded_processing = ThreadedProcessing(images, inv_depth_accuracy, - num_threads=1) - threaded_processing.process_all_images() - ind = np.argmax(threaded_processing.scores_without_image) - - # Remove image ind. - removed_images.append(images[image_ids[ind]]) - scores_after_removal.append(threaded_processing.scores_without_image[ind]) - del images[image_ids[ind]] - - perc = scores_after_removal[-1]/max(scores_after_removal)*100 - print('After reducing to %i iamges, the depth accuracy score is %0.5f%% of ' - 'that from the full image set' % (len(images), perc)) - -plt.plot(np.array(scores_after_removal)/max(scores_after_removal)) -# ---------------------------------------------------------------------------- \ No newline at end of file diff --git a/kamera/colmap_processing/scripts/reg_cams.sh b/kamera/colmap_processing/scripts/reg_cams.sh deleted file mode 100755 index 910bdf8..0000000 --- a/kamera/colmap_processing/scripts/reg_cams.sh +++ /dev/null @@ -1,35 +0,0 @@ -#!/usr/bin/env bash -#2021 (C) Eugene.Borovikov@Kitware.com: register camera images to an existing model, e.g. to calibrate them, assuming colmap is installed an configured. - -BaseDN=$1 # base directory name -echo BaseDN=$BaseDN - -VocabFN=$2 # SIFT vocabulary file name -echo VocabFN=$VocabFN - -echo colmap feature_extractor \ - --database_path $BaseDN/colmap/database.db \ - --image_path $BaseDN/images \ - --image_list_path $BaseDN/images/cam.lst \ - --ImageReader.camera_model OPENCV - -# vocab-tree matcher -echo colmap vocab_tree_matcher \ - --database_path $BaseDN/colmap/database.db \ - --VocabTreeMatching.vocab_tree_path $VocabFN \ - --VocabTreeMatching.match_list_path $BaseDN/images/cam.lst - -# alternative exhaustive_matcher -echo colmap exhaustive_matcher \ - --database_path $BaseDN/colmap/database.db - -InMdlPath=$BaseDN/colmap/sparse/org -OutMdlPath=$InMdlPath\_cam - -echo mkdir -p $OutMdlPath -echo cp $InMdlPath/project.ini $OutMdlPath - -echo colmap image_registrator \ - --database_path $BaseDN/colmap/database.db \ - --input_path $InMdlPath \ - --output_path $OutMdlPath diff --git a/kamera/colmap_processing/scripts/render_view.py b/kamera/colmap_processing/scripts/render_view.py deleted file mode 100644 index 87c065f..0000000 --- a/kamera/colmap_processing/scripts/render_view.py +++ /dev/null @@ -1,142 +0,0 @@ -#! /usr/bin/python -""" -ckwg +31 -Copyright 2018 by Kitware, Inc. -All rights reserved. - -Redistribution and use in source and binary forms, with or without -modification, are permitted provided that the following conditions are met: - - * Redistributions of source code must retain the above copyright notice, - this list of conditions and the following disclaimer. - - * Redistributions in binary form must reproduce the above copyright notice, - this list of conditions and the following disclaimer in the documentation - and/or other materials provided with the distribution. - - * Neither name of Kitware, Inc. nor the names of any contributors may be used - to endorse or promote products derived from this software without specific - prior written permission. - -THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS ``AS IS'' -AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHORS OR CONTRIBUTORS BE LIABLE FOR -ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - -============================================================================== - -""" -from __future__ import division, print_function -import numpy as np -import os -import cv2 -import subprocess -import matplotlib.pyplot as plt -import glob -import natsort -import math -import PIL -from osgeo import osr, gdal - -# Colmap Processing imports. -from colmap_processing.geo_conversions import llh_to_enu -from colmap_processing.colmap_interface import read_images_binary, Image, \ - read_points3D_binary, read_cameras_binary, qvec2rotmat -from colmap_processing.database import COLMAPDatabase, pair_id_to_image_ids, blob_to_array -import colmap_processing.vtk_util as vtk_util -from colmap_processing.geo_conversions import enu_to_llh, llh_to_enu, \ - rmat_ecef_enu, rmat_enu_ecef -from colmap_processing.static_camera_model import load_static_camera_from_file -from colmap_processing.vtk_util import render_distored_image - - - -# ---------------------------------------------------------------------------- -save_dir = '/media/data/test' -location = 'test' - - -if location == 'test': - # Meshed 3-D model used to render an synthetic view for sanity checking and - # to produce the depth map. - mesh_fname = 'mesh.ply' - mesh_lat0 = 0 # degrees - mesh_lon0 = 0 # degrees - mesh_h0 = 73 # meters above WGS84 ellipsoid -else: - raise Exception('Unrecognized location \'%s\'' % location) - -# VTK renderings are limited to monitor resolution (width x height). -monitor_resolution = (1080, 1080) - -clipping_range = [1, 2000] -# ---------------------------------------------------------------------------- - - -# Read model into VTK. -try: - model_reader - assert prev_loaded_fname == mesh_fname -except: - model_reader = vtk_util.load_world_model(mesh_fname) - prev_loaded_fname = mesh_fname - - -# -------------------------------- Define Camera ----------------------------- -if False: - # Manually specific camera. - res_x = 1920 - res_y = 1080 - pos = [-36.75 - 5, 39.25, 11.74] - pan = -180 + 90 + 90 - tilt = -45 - vfov = 90 - - vtk_camera = vtk_util.CameraPanTilt(res_x, res_y, vfov, pos, pan, tilt) -else: - # Read existing camera from file. - camera_fname = 'camera_model.yaml' - ret = load_static_camera_from_file(camera_fname) - height, width, K, dist, R, depth_map, latitude, longitude, altitude = ret - cam_pos = llh_to_enu(latitude, longitude, altitude, mesh_lat0, mesh_lon0, - mesh_h0) - - # R currently is relative to ENU coordinate system at latitude0, - # longitude0, altitude0, but we need it to be relative to latitude, - # longitude, altitude. - Rp = np.dot(rmat_enu_ecef(mesh_lat0, mesh_lon0), - rmat_ecef_enu(latitude, longitude)) - R = np.dot(R, Rp) - -ret = vtk_util.render_distored_image(width, height, K, dist, cam_pos, R, - model_reader, return_depth=True, - monitor_resolution=(1080, 1080), - clipping_range=clipping_range) -rendered_view, depth, E, N, U = ret - - -if False: - im = PIL.Image.fromarray(depth, mode='F') # float32 - depth_map_fname = '%s/camera_model_depth_map.tif' % save_dir - im.save(depth_map_fname) - - depth_image = depth.copy() - depth_image[depth_image > clipping_range[1]*0.9] = 0 - depth_image -= depth_image.min() - depth_image /= depth_image.max()/255 - depth_image = np.round(depth_image).astype(np.uint8) - - depth_image = cv2.applyColorMap(depth_image, cv2.COLORMAP_JET) - cv2.imwrite('%s/depth_vizualization.png' % save_dir, - depth_image[:, :, ::-1]) - -plt.imshow(rendered_view) - - -cv2.imwrite('%s/rendered.jpg' % save_dir, rendered_view[:, :, ::-1]) diff --git a/kamera/colmap_processing/scripts/split_database.py b/kamera/colmap_processing/scripts/split_database.py deleted file mode 100644 index 00dbf59..0000000 --- a/kamera/colmap_processing/scripts/split_database.py +++ /dev/null @@ -1,181 +0,0 @@ -#! /usr/bin/python -""" -ckwg +31 -Copyright 2021 by Kitware, Inc. -All rights reserved. - -Redistribution and use in source and binary forms, with or without -modification, are permitted provided that the following conditions are met: - - * Redistributions of source code must retain the above copyright notice, - this list of conditions and the following disclaimer. - - * Redistributions in binary form must reproduce the above copyright notice, - this list of conditions and the following disclaimer in the documentation - and/or other materials provided with the distribution. - - * Neither name of Kitware, Inc. nor the names of any contributors may be used - to endorse or promote products derived from this software without specific - prior written permission. - -THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS ``AS IS'' -AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHORS OR CONTRIBUTORS BE LIABLE FOR -ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - -============================================================================== - -""" -from __future__ import division, print_function -import numpy as np -import os -import cv2 -import matplotlib.pyplot as plt -import json -import itertools -import copy - -# Colmap Processing imports. -import colmap_processing.colmap_interface as colmap_interface -from colmap_processing.database import COLMAPDatabase, pair_id_to_image_ids, \ - blob_to_array - - -# ---------------------------------------------------------------------------- -# Colmap directory. -colmap_dir = '/mnt/data/colmap' - -# Source database. -source_db_fname = '/mnt/data/colmap/database.db' - -# Path to the sparse dir containing images.bin, cameras.bin, and points3D.bin. -sparse_dir = '%s/sparse/0' % colmap_dir - -sparse_out_dir = '%s/sparse' % colmap_dir -# ---------------------------------------------------------------------------- - -# Read in the details of all images. -cameras, images, points3D = colmap_interface.read_model(sparse_dir) - -source_db = COLMAPDatabase.connect(source_db_fname) -kp_dict = source_db.get_keypoint_from_image_dict() -descr_dict = source_db.get_descriptors_from_image_dict() -match_dict = source_db.get_match_dictionary() -two_view_geoms = source_db.get_all_two_view_geometry() - - -base_fname_to_camera = {} -base_fname_to_camera_id = {} -camera_id_to_base_fname = {} -name_to_camera = {} -base_fname_to_src_images = {} -for image in images: - img_base = images[image].name.split('/')[0] - camera_id = images[image].camera_id - camera = cameras[camera_id] - - if img_base not in base_fname_to_camera_id: - base_fname_to_camera_id[img_base] = camera_id - - assert base_fname_to_camera_id[img_base] == camera_id - - camera_id_to_base_fname[camera_id] = img_base - base_fname_to_camera[img_base] = camera - - if img_base not in base_fname_to_src_images: - base_fname_to_src_images[img_base] = [] - - base_fname_to_src_images[img_base].append(images[image]) - - -new_db = {} -for name in camera_id_to_base_fname.values(): - fname = '%s/%s.db' % (colmap_dir, name) - new_db[name] = db = COLMAPDatabase.connect(fname) - db.create_tables() - - camera = base_fname_to_camera[name] - db.add_camera(colmap_interface.CAMERA_MODEL_NAMES_TO_IND[camera.model], - camera.width, camera.height, camera.params, camera_id=1) - -for name in base_fname_to_src_images: - db = new_db[name] - - # Add images and keypoints and descriptors into the database. - for image in base_fname_to_src_images[name]: - db.add_image(image.name, 1, image.qvec, image.tvec, image.id) - db.add_keypoints(image.id, kp_dict[image.id]) - db.add_descriptors(image.id, descr_dict[image.id]) - - # Add images and keypoints and descriptors into the database. - image_ids = set(db.get_keypoint_from_image_dict().keys()) - for pair in match_dict: - image_id1 = int(pair[0]) - image_id2 = int(pair[1]) - if image_id1 in image_ids and image_id2 in image_ids: - db.add_matches(image_id1, image_id2, match_dict[pair]) - - for i in range(len(two_view_geoms[0])): - pair_id = two_view_geoms[0][i] - image_ids = two_view_geoms[1][i] - - image_id1 = int(image_ids[0]) - image_id2 = int(image_ids[1]) - if image_id1 in image_ids and image_id2 in image_ids: - inlier_matches = two_view_geoms[2][i] - F = two_view_geoms[3][i] - E = two_view_geoms[4][i] - H = two_view_geoms[5][i] - config = two_view_geoms[6][i] - - db.add_two_view_geometry(image_id1, image_id2, inlier_matches, F, - E, H, config) - - -for name in new_db: - db = new_db[name] - db.commit() - db.close() - - -if False: - fname = '/mnt/data/database.db' - db = COLMAPDatabase.connect(fname) - - rows = db.execute("SELECT * FROM cameras") - camera_id, model, width, height, params, prior = next(rows) - params = blob_to_array(params, np.float64) - - -for name in base_fname_to_camera: - outdir_ = '%s/%s' % (sparse_out_dir, name) - camera = base_fname_to_camera[name] - - try: - os.makedirs(outdir_) - except (OSError, IOError): - pass - - camera = base_fname_to_camera[name] - camera = colmap_interface.Camera(1, camera.model, camera.width, - camera.height, camera.params) - - images_ = dict([(image.id, image) - for image in base_fname_to_src_images[name]]) - - points3D_ = {} - - point3D_ids = set() - for image in base_fname_to_src_images[name]: - point3D_ids = point3D_ids.union(set(image.point3D_ids)) - - points3D_ = {ind:points3D[ind] for ind in point3D_ids if ind >= 0} - - colmap_interface.write_model({1:camera}, images_, points3D_, outdir_, - ext=".bin") \ No newline at end of file diff --git a/kamera/colmap_processing/scripts/static_cameras_from_colmap.py b/kamera/colmap_processing/scripts/static_cameras_from_colmap.py deleted file mode 100644 index 0657b52..0000000 --- a/kamera/colmap_processing/scripts/static_cameras_from_colmap.py +++ /dev/null @@ -1,615 +0,0 @@ -#! /usr/bin/python -""" -ckwg +31 -Copyright 2018 by Kitware, Inc. -All rights reserved. - -Redistribution and use in source and binary forms, with or without -modification, are permitted provided that the following conditions are met: - - * Redistributions of source code must retain the above copyright notice, - this list of conditions and the following disclaimer. - - * Redistributions in binary form must reproduce the above copyright notice, - this list of conditions and the following disclaimer in the documentation - and/or other materials provided with the distribution. - - * Neither name of Kitware, Inc. nor the names of any contributors may be used - to endorse or promote products derived from this software without specific - prior written permission. - -THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS ``AS IS'' -AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHORS OR CONTRIBUTORS BE LIABLE FOR -ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - -============================================================================== - -""" -from __future__ import division, print_function -import numpy as np -import os -import cv2 -import subprocess -import matplotlib.pyplot as plt -from mpl_toolkits.mplot3d import Axes3D -import glob -import natsort -import trimesh -import math -import PIL -from osgeo import osr, gdal -from scipy.optimize import fmin, minimize, fminbound - -# Colmap Processing imports. -from colmap_processing.geo_conversions import llh_to_enu -from colmap_processing.colmap_interface import read_images_binary, Image, \ - read_points3D_binary, read_cameras_binary, qvec2rotmat -from colmap_processing.database import COLMAPDatabase, pair_id_to_image_ids, blob_to_array -import colmap_processing.vtk_util as vtk_util -from colmap_processing.geo_conversions import enu_to_llh, llh_to_enu, \ - rmat_ecef_enu, rmat_enu_ecef -from colmap_processing.static_camera_model import save_static_camera, \ - load_static_camera_from_file, write_camera_krtd_file - - -# ---------------------------------------------------------------------------- -save_dir = 'static_camera_models' - -# Base path to the colmap directory containing 'cameras.bin' 'images.bin'. -image_dir = 'all' - -# Path to the images.bin file. -images_bin_fname = '%s/images.bin' % image_dir -camera_bin_fname = '%s/cameras.bin' % image_dir -points_3d_bin_fname = '%s/points3D.bin' % image_dir - -if True: - mesh_fname = 'coarse.ply' - - latitude0 = 0 # degrees - longitude0 = 0 # degrees - altitude0 = 0 # meters above WGS84 ellipsoid -# ---------------------------------------------------------------------------- - - -# Read in the details of all images. -images = read_images_binary(images_bin_fname) -cameras = read_cameras_binary(camera_bin_fname) -pts_3d = read_points3D_binary(points_3d_bin_fname) - - -if False: - # Save off camera positions to be used to obtain geo-registration matrix. - keys = list(images.keys()) - times = [float(os.path.splitext(images[key].name)[0])/1e6 for key in keys] - times = np.array(times) - inds = np.argsort(times) - times = times[inds] - keys = [keys[ind] for ind in inds] - tvecs = [images[key].tvec for key in keys] - rmat = [qvec2rotmat(images[key].qvec) for key in keys] - cam_pos = [np.dot(rmat[i].T, -tvecs[i]) for i in range(len(tvecs))] - cam_pos = np.array(cam_pos).T - - fig = plt.figure() - ax = fig.add_subplot(111, projection='3d') - plt.plot(cam_pos[0], cam_pos[1], cam_pos[2]) - rmat_ravel = np.array([r.ravel() for r in rmat]).T - data = np.vstack([times, cam_pos, rmat_ravel]).T - np.savetxt('cam_pos.txt', data) - - - - - - - - - - - - -def process(image_fname, ref_image_id, points_fname, save_dir, K, - dist=[0, 0, 0, 0,], optimize_k1=False, optimize_k2=False, - optimize_k3=False, optimize_k4=False): - - - colmap_image = images[ref_image_id] - camera = cameras[colmap_image.camera_id] - - point_pairs = np.loadtxt(points_fname) - - if False: - point_pairs = point_pairs[3:] - - ref_pts = point_pairs[:, :2] - im_pts = point_pairs[:, 2:].astype(np.float32) - - enu = [get_xyz_from_image_pt(ref_image_id, im_pt) for im_pt in ref_pts] - enu = np.array(enu).astype(np.float32) - - calibrate_from_enu_pts(image_fname, im_pts, enu, save_dir, K, dist=dist, - optimize_k1=optimize_k1, optimize_k2=optimize_k2, - optimize_k3=optimize_k3, optimize_k4=optimize_k4) - - -def calibrate_from_llh_file(image_fname, points_fname, save_dir, - K, dist=[0, 0, 0, 0], optimize_k1=False, - optimize_k2=False, optimize_k3=False, - optimize_k4=False, fix_principal_point=True): - ret = np.loadtxt(points_fname) - im_pts = ret[:, :2] - llh = ret[:, 2:] - enu = [llh_to_enu(_[0], _[1], _[2], latitude0, longitude0, altitude0) - for _ in llh] - enu = np.array(enu) - calibrate_from_enu_pts(image_fname, im_pts, enu, save_dir, K, dist=dist, - optimize_k1=optimize_k1, optimize_k2=optimize_k2, - optimize_k3=optimize_k3, optimize_k4=optimize_k4, - fix_principal_point=fix_principal_point) - - -def calibrate_manual_clicked_reference(image_fname, points_fname, - ref_camera_fname, save_dir, - dist=[0, 0, 0, 0], optimize_k1=False, - optimize_k2=False, optimize_k3=False, - optimize_k4=False, - fix_principal_point=True): - """points_fname from reference to image. - - """ - ret = np.loadtxt(points_fname) - ref_pts = ret[:, :2] - im_pts = ret[:, 2:] - - ret = load_static_camera_from_file(ref_camera_fname) - K_ref, dist_ref, R, depth_map, latitude, longitude, altitude = ret[2:] - height, width = depth_map.shape - - enu0 = llh_to_enu(latitude, longitude, altitude, latitude0, longitude0, - altitude0) - enu0 = np.array(enu0) - enu = np.zeros((len(ref_pts), 3)) - - # Unproject rays into the camera coordinate system. - ray_dir = np.ones((3, len(ref_pts)), dtype=np.float) - ray_dir0 = cv2.undistortPoints(np.expand_dims(ref_pts, 0), - K_ref, dist_ref, R=None) - ray_dir[:2] = np.squeeze(ray_dir0, 0).T - - # Rotate rays into the local east/north/up coordinate system. - ray_dir = np.dot(R.T, ray_dir) - ray_dir /= np.sqrt(np.sum(ray_dir**2, 0)) - - for i in range(ref_pts.shape[0]): - x, y = ref_pts[i] - if x == 0: - ix = 0 - elif x == width: - ix = int(width - 1) - else: - ix = int(round(x - 0.5)) - - if y == 0: - iy = 0 - elif y == height: - iy = int(height - 1) - else: - iy = int(round(y - 0.5)) - - if ix < 0 or iy < 0 or ix >= width or iy >= height: - print(x == width) - print(y == height) - raise ValueError('Coordinates (%0.1f,%0.f) are outside the ' - '%ix%i image' % (x, y, width, height)) - - enu[i] = enu0 + ray_dir[:, i]*depth_map[iy, ix] - - calibrate_from_enu_pts(image_fname, im_pts, enu, save_dir, K_ref, - dist=dist, optimize_k1=optimize_k1, - optimize_k2=optimize_k2, optimize_k3=optimize_k3, - optimize_k4=optimize_k4, - fix_principal_point=fix_principal_point) - - -def calibrate_from_enu_pts(image_fname, im_pts, enu, save_dir, K, - dist=[0, 0, 0, 0], optimize_k1=False, - optimize_k2=False, optimize_k3=False, - optimize_k4=False, fix_principal_point=True): - dist = np.array(dist, dtype=np.float32) - real_image = cv2.imread(image_fname) - - if real_image.ndim == 3: - real_image = real_image[:, :, ::-1] - - height, width = real_image.shape[:2] - - K[0, 2] = width/2 - K[1, 2] = height/2 - - if False: - ref_image = cv2.imread(ref_image_fname)[:, :, ::-1] - plt.figure(); plt.imshow(ref_image) - plt.plot(ref_pts.T[0], ref_pts.T[1], 'bo') - plt.figure(); plt.imshow(real_image) - plt.plot(im_pts.T[0], im_pts.T[1], 'bo') - - if False: - from mpl_toolkits.mplot3d import Axes3D - fig = plt.figure() - fig.add_subplot(111, projection='3d') - plt.plot(enu[:,0], enu[:,1], enu[:,2], 'ro') - plt.xlabel('X Axis') - plt.ylabel('Y Axis') - - - def show_err(rvec, tvec, k, dist, plot=False): - im_pts_ = cv2.projectPoints(enu, rvec, tvec, k, dist)[0] - im_pts_ = np.squeeze(im_pts_) - err = np.sqrt(np.sum((im_pts - im_pts_)**2, 1)) - err = np.mean(err) - - if plot: - plt.imshow(real_image) - plt.plot(im_pts.T[0], im_pts.T[1], 'bo') - plt.plot(im_pts_.T[0], im_pts_.T[1], 'ro') - for i in range(len(im_pts_)): - plt.plot([im_pts[i, 0], im_pts_[i, 0]], - [im_pts[i, 1], im_pts_[i, 1]], 'g-') - - return err - - - if False: - # First pass. - flags = cv2.CALIB_ZERO_TANGENT_DIST - flags = flags | cv2.CALIB_USE_INTRINSIC_GUESS - flags = flags | cv2.CALIB_FIX_PRINCIPAL_POINT - flags = flags | cv2.CALIB_FIX_ASPECT_RATIO - flags = flags | cv2.CALIB_FIX_K1 - flags = flags | cv2.CALIB_FIX_K2 - flags = flags | cv2.CALIB_FIX_K3 - flags = flags | cv2.CALIB_FIX_K4 - flags = flags | cv2.CALIB_FIX_K5 - flags = flags | cv2.CALIB_FIX_K6 - criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30000, - 0.0000001) - ret = cv2.calibrateCamera([enu.astype(np.float32)], - [im_pts.astype(np.float32)], (width, height), - cameraMatrix=K, distCoeffs=dist, flags=flags, - criteria=criteria) - err, K, dist, rvecs, tvecs = ret - print('First pass error:', err) - rvec, tvec = rvecs[0], tvecs[0] - - # Second pass. - flags = cv2.CALIB_ZERO_TANGENT_DIST - flags = flags | cv2.CALIB_USE_INTRINSIC_GUESS - flags = flags | cv2.CALIB_FIX_PRINCIPAL_POINT - flags = flags | cv2.CALIB_FIX_K1 - flags = flags | cv2.CALIB_FIX_K2 - flags = flags | cv2.CALIB_FIX_K3 - flags = flags | cv2.CALIB_FIX_K4 - flags = flags | cv2.CALIB_FIX_K5 - flags = flags | cv2.CALIB_FIX_K6 - - ret = cv2.calibrateCamera([enu.astype(np.float32)], - [im_pts.astype(np.float32)], - (width, height), cameraMatrix=K, - distCoeffs=dist, flags=flags) - err, K, dist, rvecs, tvecs = ret - print('Second pass error:', err) - rvec, tvec = rvecs[0], tvecs[0] - - #if optimize_k1 or optimize_k2 or optimize_k3 or optimize_k4: - if True: - # Third pass. - flags = cv2.CALIB_ZERO_TANGENT_DIST - flags = flags | cv2.CALIB_USE_INTRINSIC_GUESS - - if fix_principal_point: - flags = flags | cv2.CALIB_FIX_PRINCIPAL_POINT - - if not optimize_k1: - flags = flags | cv2.CALIB_FIX_K1 - - if not optimize_k2: - flags = flags | cv2.CALIB_FIX_K2 - - if not optimize_k3: - flags = flags | cv2.CALIB_FIX_K3 - - if not optimize_k4: - flags = flags | cv2.CALIB_FIX_K4 - - flags = flags | cv2.CALIB_FIX_K5 - flags = flags | cv2.CALIB_FIX_K6 - - ret = cv2.calibrateCamera([enu.astype(np.float32)], - [im_pts.astype(np.float32)], - (width, height), cameraMatrix=K, - distCoeffs=dist, flags=flags) - err, K, dist, rvecs, tvecs = ret - print('Third pass error:', err) - rvec, tvec = rvecs[0], tvecs[0] - - - show_err(rvec, tvec, K, dist, plot=True) - - R = cv2.Rodrigues(rvec)[0] - cam_pos = -np.dot(R.T, tvec).ravel() - - save_camera_model(width, height, K, dist, cam_pos, R, real_image, save_dir, - mode=2) - # ------------------------------------------------------------------------ - - -def get_features(image, num_features=10000): - # Find the keypoints and descriptors and match them. - orb = cv2.ORB_create(nfeatures=num_features, edgeThreshold=25, - patchSize=31, nlevels=16, - scoreType=cv2.ORB_FAST_SCORE, fastThreshold=10) - - if image.ndim == 3: - img_gray = cv2.cvtColor(image, cv2.COLOR_RGB2GRAY) - else: - img_gray = image - - img_gray = img_gray.astype(np.float) - img_gray -= np.percentile(img_gray.ravel(), 1) - img_gray[img_gray < 0] = 0 - img_gray /= np.percentile(img_gray.ravel(), 99)/255 - img_gray[img_gray > 225] = 255 - img_gray = np.round(img_gray).astype(np.uint8) - - return orb.detectAndCompute(img_gray, None) - # ------------------------------------------------------------------------ - - -def process_auto(image_fname, ref_camera_fname, save_dir, optimize_k1=False, - optimize_k2=False, optimize_k3=False, optimize_k4=False, - fix_principal_point=True, num_features=10000, homog_thresh=20, - final_thresh=5): - ret = load_static_camera_from_file(ref_camera_fname) - K, dist, R, depth_map, latitude, longitude, altitude = ret[2:] - - real_image = cv2.imread(image_fname) - - height, width = real_image.shape[:2] - - if real_image.ndim == 3: - real_image = real_image[:, :, ::-1] - - ref_image = cv2.imread('%s/ref_view.png' % - os.path.split(ref_camera_fname)[0]) - - if ref_image.ndim == 3: - ref_image = ref_image[:, :, ::-1] - - kp0, des0 = get_features(ref_image, num_features=num_features) - kp1, des1 = get_features(real_image, num_featuresref_camera_fname=num_features) - - bf = cv2.BFMatcher(cv2.NORM_HAMMING, crossCheck=True) - matches = bf.match(des0, des1) - - pts0 = np.float32([kp0[m.queryIdx].pt for m in matches]) - pts1 = np.float32([kp1[m.trainIdx].pt for m in matches]) - - print('Feature matcher found %i matches' % len(matches)) - - if False: - mask = np.logical_and(pts0[:, 1] > 355, - pts0[:, 1] < 649) - pts0 = pts0[mask] - pts1 = pts1[mask] - mask = np.logical_and(pts1[:, 1] > 355, - pts1[:, 1] < 648) - pts0 = pts0[mask] - pts1 = pts1[mask] - - M, mask = cv2.findHomography(pts0, pts1, cv2.RANSAC, homog_thresh) - mask = np.squeeze(mask) > 0 - - pts0 = pts0[mask] - pts1 = pts1[mask] - - print('Homography filtering yielded %i matches' % len(pts0)) - - ray_dir = cv2.undistortPoints(np.expand_dims(pts0, 0), K, dist, None) - ray_dir = np.squeeze(ray_dir, 0).astype(np.float32).T - ray_dir = np.vstack([ray_dir, np.ones(ray_dir.shape[1])]) - - ray_dir = np.dot(R.T, ray_dir) - - d = [] - for pt in pts0: - ix, iy = np.round(pt - 0.5).astype(np.int) - d.append(depth_map[iy, ix]) - - ray_dir = ray_dir*d - - cam_pos = llh_to_enu(latitude, longitude, altitude, latitude0, longitude0, - altitude0) - enu = ray_dir.T + np.atleast_2d(cam_pos) - - if False: - from mpl_toolkits.mplot3d import Axes3D - fig = plt.figure() - fig.add_subplot(111, projection='3d') - plt.plot(enu[:,0], enu[:,1], enu[:,2], 'ro') - plt.xlabel('X Axis') - plt.ylabel('Y Axis') - - # First pass. - K[0, 2] = width/2 - K[1, 2] = height/2 - flags = cv2.CALIB_ZERO_TANGENT_DIST - flags = flags | cv2.CALIB_USE_INTRINSIC_GUESS - flags = flags | cv2.CALIB_FIX_PRINCIPAL_POINT - flags = flags | cv2.CALIB_FIX_K1 - flags = flags | cv2.CALIB_FIX_K2 - flags = flags | cv2.CALIB_FIX_K3 - flags = flags | cv2.CALIB_FIX_K4 - flags = flags | cv2.CALIB_FIX_K5 - flags = flags | cv2.CALIB_FIX_K6 - - while True: - ret = cv2.calibrateCamera([enu.astype(np.float32)], - [pts1.astype(np.float32)], - (width, height), cameraMatrix=K, - distCoeffs=dist, flags=flags) - err, K, dist, rvecs, tvecs = ret - print('Focal lengths', K[0, 0], K[1, 1]) - print('Second pass error:', err) - rvec, tvec = rvecs[0], tvecs[0] - - im_pts_ = cv2.projectPoints(enu, rvec, tvec, K, dist)[0] - im_pts_ = np.squeeze(im_pts_) - err = np.sqrt(np.sum((pts1 - im_pts_)**2, 1)) - - mask = err < max(100, np.percentile(err, 50)) - - if np.all(mask): - break - - enu = enu[mask] - pts0 = pts0[mask] - pts1 = pts1[mask] - - print('Calibration filtering yielded %i matches' % len(pts0)) - - # Second pass. - flags = cv2.CALIB_ZERO_TANGENT_DIST - flags = flags | cv2.CALIB_USE_INTRINSIC_GUESS - - if fix_principal_point: - flags = flags | cv2.CALIB_FIX_PRINCIPAL_POINT - - if not optimize_k1: - flags = flags | cv2.CALIB_FIX_K1 - - if not optimize_k2: - flags = flags | cv2.CALIB_FIX_K2 - - if not optimize_k3: - flags = flags | cv2.CALIB_FIX_K3 - - if not optimize_k4: - flags = flags | cv2.CALIB_FIX_K4 - - flags = flags | cv2.CALIB_FIX_K5 - flags = flags | cv2.CALIB_FIX_K6 - - while True: - ret = cv2.calibrateCamera([enu.astype(np.float32)], - [pts1.astype(np.float32)], - (width, height), cameraMatrix=K, - distCoeffs=dist, flags=flags) - err, K, dist, rvecs, tvecs = ret - print('Second pass error:', err) - rvec, tvec = rvecs[0], tvecs[0] - - im_pts_ = cv2.projectPoints(enu, rvec, tvec, K, dist)[0] - im_pts_ = np.squeeze(im_pts_) - err = np.sqrt(np.sum((pts1 - im_pts_)**2, 1)) - - mask = err < final_thresh - - if np.all(mask): - break - - enu = enu[mask] - pts0 = pts0[mask] - pts1 = pts1[mask] - - R = cv2.Rodrigues(rvec)[0] - cam_pos = -np.dot(R.T, tvec).ravel() - - if True: - plt.figure() - plt.subplot('211') - plt.imshow(ref_image, cmap='gray', interpolation='none') - plt.plot(pts0.T[0], pts0.T[1], 'ro') - plt.subplot('212') - plt.imshow(real_image, cmap='gray', interpolation='none') - plt.plot(pts1.T[0], pts1.T[1], 'bo') - - if False: - save_camera_model(monitor_resolution[0], monitor_resolution[1], K, - np.zeros(4), cam_pos, R, real_image, save_dir, - mode=1) - - save_camera_model(width, height, K, dist, cam_pos, R, real_image, save_dir, - mode=1) - - -def save_camera_model(width, height, K, dist, cam_pos, R, real_image, - save_dir, mode=1, scale=0.5): - ret = vtk_util.render_distored_image(width, height, K, dist, cam_pos, R, - model_reader, return_depth=True, - monitor_resolution=(1920, 1080)) - img, depth, E, N, U = ret - - # ------------------------------------------------------------------------ - - try: - os.makedirs(save_dir) - except OSError: - pass - - cv2.imwrite('%s/ref_view.png' % save_dir, real_image[:, :, ::-1]) - - # Read and undistort image. - cv2.imwrite('%s/rendered_view.png' % save_dir, img[:, :, ::-1]) - # ------------------------------------------------------------------- - - latitude, longitude, altitude = enu_to_llh(cam_pos[0], cam_pos[1], - cam_pos[2], latitude0, - longitude0, altitude0) - - # R currently is relative to ENU coordinate system at latitude0, - # longitude0, altitude0, but we need it to be relative to latitude, - # longitude, altitude. - Rp = np.dot(rmat_enu_ecef(latitude0, longitude0), - rmat_ecef_enu(latitude, longitude)) - - filename = '%s/camera_model.yaml' % save_dir - R2 = np.dot(R, Rp) - save_static_camera(filename, height, width, K, dist, R2, - depth, latitude, longitude, altitude) - - filename = '%s/camera_model.krtd' % save_dir - tvec = -np.dot(R, cam_pos).ravel() - write_camera_krtd_file([K, R2, tvec, dist], filename) - - # Save (x, y, z) meters per pixel. - ENU = np.dstack([E, N, U]).astype(np.float32) - filename = '%s/xyz_per_pixel.npy' % save_dir - np.save(filename, ENU, allow_pickle=False) - - depth_image = depth.copy() - depth_image[depth_image > clipping_range[1]*0.9] = 0 - depth_image -= depth_image.min() - depth_image /= depth_image.max()/255 - depth_image = np.round(depth_image).astype(np.uint8) - - depth_image = cv2.applyColorMap(depth_image, cv2.COLORMAP_JET) - cv2.imwrite('%s/depth_vizualization.png' % save_dir, - depth_image[:, :, ::-1]) - - -cam_name = 'test' -save_dir_ = '%s/%s' % (save_dir, cam_name) -points_fname = 'points.txt' -image_fname = '%s.jpg' % cam_name -ref_image_fname = '%s.jpg' % ref_image_id -process(image_fname, ref_image_id, points_fname, save_dir_, K=K_reolink, - dist=None, optimize_k1=False, optimize_k2=False, optimize_k3=False, - optimize_k4=False) \ No newline at end of file diff --git a/kamera/colmap_processing/scripts/stretch_contrast.py b/kamera/colmap_processing/scripts/stretch_contrast.py deleted file mode 100644 index d999d89..0000000 --- a/kamera/colmap_processing/scripts/stretch_contrast.py +++ /dev/null @@ -1,39 +0,0 @@ -import glob -import cv2 -import numpy as np -import matplotlib.pyplot as plt - -fnames = glob.glob('/mnt/data/*.JPG') - -stretch_percentiles = [0.1, 99.9] -monochrome = False -clip_limit = 3.5 -median_blur_diam = 0 - - -for fname in fnames: - print('Processing image \'%s\'' % fname) - img = cv2.imread(fname, cv2.IMREAD_UNCHANGED) - if img.ndim == 3 and monochrome: - img = img[:, :, 0] - - img = img.astype(np.float) - img -= np.percentile(img.ravel(), stretch_percentiles[0]) - img[img < 0] = 0 - img /= np.percentile(img.ravel(), stretch_percentiles[1])/255 - img[img > 255] = 255 - img = np.round(img).astype(np.uint8) - - clahe = cv2.createCLAHE(clipLimit=clip_limit, tileGridSize=(8,8)) - - if img.ndim == 3: - hsv = cv2.cvtColor(img, cv2.COLOR_RGB2HSV) - hsv[:, :, 2] = clahe.apply(hsv[:, :, 2]) - img = cv2.cvtColor(hsv, cv2.COLOR_HSV2RGB) - else: - img = clahe.apply(img) - - if median_blur_diam > 0: - img = cv2.medianBlur(img, median_blur_diam) - - cv2.imwrite(fname, img) diff --git a/kamera/colmap_processing/scripts/texture_colmap_3d_model.py b/kamera/colmap_processing/scripts/texture_colmap_3d_model.py deleted file mode 100644 index b196602..0000000 --- a/kamera/colmap_processing/scripts/texture_colmap_3d_model.py +++ /dev/null @@ -1,530 +0,0 @@ -from __future__ import division, print_function -import os -import numpy as np -import cv2 -import time -import matplotlib.pyplot as plt -from mpl_toolkits import mplot3d -import glob -import copy -import open3d as o3d -from scipy.interpolate import interp2d, RectBivariateSpline, interpn -from SetCoverPy import setcover -import scipy - - -# ADAPT imports. -import colmap_processing.camera_models as camera_models -from colmap_processing.world_models import WorldModelMesh -from colmap_processing.colmap_interface import read_images_binary, \ - read_points3D_binary, read_cameras_binary, qvec2rotmat, \ - standard_cameras_from_colmap -import colmap_processing.vtk_util as vtk_util -from colmap_processing.colmap_interface import Image as ColmapImage -from colmap_processing.platform_pose import PlatformPoseInterp -from colmap_processing.rotations import quaternion_from_matrix, \ - quaternion_matrix - -%matplotlib auto - -# Meshed model generated from 3-D points generated by Colmap. -meshed_model = 'test.ply' - -# This should be the result of undistort function. -image_dir = 'images' - -# This should be the model generated by the undistort function. -sparse_recon_subdir = 'sparse' - - -# --------------------- Read Existing Colmap Reconstruction ------------------ -# Read in the Colmap details of all images. -images_bin_fname = '%s/images.bin' % sparse_recon_subdir -colmap_images = read_images_binary(images_bin_fname) -camera_bin_fname = '%s/cameras.bin' % sparse_recon_subdir -colmap_cameras = read_cameras_binary(camera_bin_fname) - -sfm_cms = standard_cameras_from_colmap(colmap_cameras, colmap_images) - -render_resolution = (1000, 1000) -clipping_range = [.1, 30] - -# A ray is launched from each image at the model, and if it intersections -# within this distance from the mesh surface, it is considered a match. -vertex_eps = 1e-2 - -max_image_width = 1000 - -# Maximum number of pixels of texture per unit of model length. -max_text_res = 600 -# ---------------------------------------------------------------------------- - -colmap_images_inds = sorted(list(colmap_images.keys())) - -if False: - colmap_images_inds = colmap_images_inds[40:45] - -model_reader = vtk_util.load_world_model(meshed_model) -world_mesh = WorldModelMesh(meshed_model) -mesh = o3d.io.read_triangle_mesh(meshed_model, True) -vertices = np.asarray(mesh.vertices) -vertices_colors = np.asarray(mesh.vertex_colors) -triangles = np.asarray(mesh.triangles) - -tria_edge_len = [] -for i, j, k in triangles: - tria_edge_len.append([np.sqrt(np.sum((vertices[i] - vertices[j])**2)), - np.sqrt(np.sum((vertices[j] - vertices[k])**2)), - np.sqrt(np.sum((vertices[i] - vertices[k])**2))]) -tria_edge_len = np.array(tria_edge_len) - -triangle_covarage = np.zeros((len(triangles), len(colmap_images_inds)), - dtype=np.float32) -uv_per_image = {} - -# Figure out which images provide the best coverage for each triangle. -for image_id_ind, image_id in enumerate(colmap_images_inds): - print('Processing \'triangle_covarage\' for image:', image_id) - image = colmap_images[image_id] - - cm = sfm_cms[image.camera_id] - - # Get depth map. - P = cm.get_camera_pose(image_id) - R = P[:3, :3] - cam_pos = -np.dot(R.T, P[:, 3]) - - viz_vert_ind = np.arange(len(vertices)) - - im_pts = cm.project(vertices.T, image_id) - ind = np.all(im_pts >= 0, axis=0) - ind = np.logical_and(ind, im_pts[0] <= cm.width) - ind = np.logical_and(ind, im_pts[1] <= cm.height) - im_pts = im_pts[:, ind] - viz_vert_ind = viz_vert_ind[ind] - - if im_pts.shape[1] == 0: - continue - - if False: - if True: - ret = vtk_util.render_distored_image(cm.width, cm.height, cm.K, - cm.dist, cam_pos, R, model_reader, - return_depth=True, - monitor_resolution=(1000, 1000), - clipping_range=[3, 7], - fill_holes=False) - - #plt.imshow(ret[0]) - #plt.imshow(ret[1]) - - if False: - plt.imshow(ret[0]) - plt.plot(im_pts[0], im_pts[1], 'ro') - - true_depth = ret[1] - - x = np.linspace(0.5, true_depth.shape[1] - 0.5, true_depth.shape[1]) - y = np.linspace(0.5, true_depth.shape[0] - 0.5, true_depth.shape[0]) - - if False: - # Sanity check - X, Y = np.meshgrid(x, y) - xi = np.vstack([X.ravel(), Y.ravel()]).T - - z = interpn((x, y), true_depth.T, xi, method='linear', - bounds_error=False, fill_value=np.nan) - - Z = np.reshape(z, X.shape) - - d1 = interpn((x, y), true_depth.T, im_pts.T, method='linear', - bounds_error=False, fill_value=np.nan) - - # This is the depth that each so-far visible vertex is from the camera. - d2 = np.dot(R[2], vertices[viz_vert_ind].T - np.atleast_2d(cam_pos).T) - - ind = d1 > d2 - vertex_eps - else: - # Determine which vertices are unocculded. - ray_pos, ray_dir = cm.unproject(im_pts, image_id) - - # Sometimes embree misses vertex. - d1 = world_mesh.intersect_rays(ray_pos, ray_dir) - np.atleast_2d(cam_pos).T - d1 = np.sqrt(np.sum(d1**2, axis=0)) - d2 = vertices[viz_vert_ind].T - np.atleast_2d(cam_pos).T - d2 = np.sqrt(np.sum(d2**2, axis=0)) - - # If it is - ind = np.abs(d2 - d1) < vertex_eps - - im_pts = im_pts[:, ind] - viz_vert_ind = viz_vert_ind[ind] - - if False: - img = cv2.imread('%s/%s' % (image_dir, image.name))[:, :, ::-1].copy() - s = set(viz_vert_ind) - for i, j, k in triangles: - if (i in s) + (j in s) + (k in s) >= 2: - wrld_pts = np.vstack([vertices[i], vertices[j], vertices[k]]).T - im_pts = np.round(cm.project(wrld_pts, image_id)).astype(int) - cv2.drawContours(img, [im_pts.T], 0, 255, thickness=-1) - - plt.imshow(img) - - viz_vert = np.zeros(len(vertices), dtype=bool) - viz_vert[viz_vert_ind] = True - - inv_map = {viz_vert_ind[i]: i for i in range(len(viz_vert_ind))} - - # These are the triangles that are visible. - ind = np.where(np.all(viz_vert[triangles], axis=1))[0] - - im_pts = im_pts.T - for ind_ in ind: - i, j, k = triangles[ind_] - i, j, k = inv_map[i], inv_map[j], inv_map[k] - - tria_edge_len_ = np.array([np.sqrt(np.sum((im_pts[i] - im_pts[j])**2)), - np.sqrt(np.sum((im_pts[j] - im_pts[k])**2)), - np.sqrt(np.sum((im_pts[i] - im_pts[k])**2))]) - triangle_covarage[ind_, image_id_ind] = np.min(tria_edge_len_/tria_edge_len[ind_]) - - -# triangle_covarage[i, j] shows the pixel resolution coverage for image j by -# triangle i. - - -# Any images that are covering a particular triangle with higher resolution -# per unit model length than 'max_text_res', it is unnecassarily high -# resolution, so we clamp to 'max_text_res'. -triangle_covarage = np.minimum(max_text_res, triangle_covarage) - -# Remove any images that don't contribute to the mesh. -ind = np.any(triangle_covarage, axis=0) -colmap_images_inds = np.array(colmap_images_inds)[ind] -triangle_covarage = triangle_covarage[:, ind] - - -if False: - # Sanity check - image_id_ind = 65 - image_id = colmap_images_inds[image_id_ind] - image = colmap_images[image_id] - img = cv2.imread('%s/%s' % (image_dir, image.name))[:, :, ::-1].copy() - - for tri_ind in np.where(triangle_covarage[:, image_id_ind] > 0)[0]: - i, j, k = triangles[tri_ind] - wrld_pts = np.vstack([vertices[i], vertices[j], vertices[k]]).T - im_pts = np.round(cm.project(wrld_pts, image_id)).astype(int) - cv2.drawContours(img, [im_pts.T], 0, 255, thickness=-1) - - plt.imshow(img) - - -# Start by taking the best individual choice. -tri_cov_map = np.argmax(triangle_covarage, axis=1) - - -# Map length number of triangles indicating which image (Colmap image index) -# will support its text. -if False: - # If one image covers a triangle at 50% resolution of another image, it is - # a good enough replacement. - sufficient_fract = 0.5 - ind = np.max(triangle_covarage, axis=1) > 0 - triangle_covarage_ = triangle_covarage[ind] - a_matrix = triangle_covarage_ > np.atleast_2d(np.max(triangle_covarage_, axis=1)*sufficient_fract).T - - # Identify any redundant conditions. - a_matrix = np.array(list(set([tuple(a) for a in a_matrix]))) - - g = setcover.SetCover(a_matrix, np.ones(a_matrix.shape[1]), maxiters=1) - solution, time_used = g.SolveSCP() - - colmap_images_inds = np.array(colmap_images_inds)[g.s] - triangle_covarage = triangle_covarage[:, g.s] -elif True: - # Any case where an image provides this fraction of the resolution coverage - # as the best covering image will be accepted as a subsitute. - sufficient_fract = 0.75 - - # a_matrix[i, j] indicates whether image j is suitable to cover triangle i. - # The image index j, here, is the index into `colmap_images_inds` to get - # the actual Colmap image index. - a_matrix = triangle_covarage > np.atleast_2d(np.max(triangle_covarage, axis=1)*sufficient_fract).T - - # Start by taking the best individual choice. - tri_cov_map = np.argmax(triangle_covarage, axis=1) - - triangles_by_vertex = {i: [] for i in range(len(vertices))} - for i, tri in enumerate(triangles): - for v in tri: - triangles_by_vertex[v].append(i) - - for i in triangles_by_vertex: - triangles_by_vertex[i] = list(set(triangles_by_vertex[i])) - - - # 'a_matrix' is a boolean array (num_triangles x num_images) where - # a_matrix[i, j] encodes whether triangles[j] is satisfactorily covered by - # image colmap_images_inds[j]. - changed = 0 - while True: - changed0 = changed - for i in range(len(triangles)): - tris = set() - for tri in triangles[i]: - tris = tris.union(set(triangles_by_vertex[tri])) - - tris.remove(i) - - # 'tris' are the triangle indices of all adjacent triangles. - - neighbor_images = [tri_cov_map[tri] for tri in tris if a_matrix[i, tri_cov_map[tri]]] - - if len(neighbor_images) == 0: - continue - - best_neighbor = scipy.stats.mode(neighbor_images, keepdims=True)[0][0] - tri_cov_map[i] = best_neighbor - - changed += 1 - - if changed0 == changed: - break - - print('Changed', changed) - print('Texture will pull from', len(set(tri_cov_map)), 'images') - - -if False: - # Sanity check - image_id_ind = 0 - image_id = colmap_images_inds[image_id_ind] - image = colmap_images[image_id] - img = cv2.imread('%s/%s' % (image_dir, image.name))[:, :, ::-1].copy() - - #plt.plot(np.sort(triangle_covarage[triangle_covarage > 0])) - - # Isolate a triangle. - for tri_ind in range(len(triangles)): - if triangle_covarage[tri_ind, image_id_ind] > 0: - i, j, k = triangles[tri_ind] - wrld_pts = np.vstack([vertices[i], vertices[j], vertices[k]]).T - im_pts = np.round(cm.project(wrld_pts, image_id)).astype(int) - if (np.all(im_pts[0] > 2050) and np.all(im_pts[0] < 2100) and - np.all(im_pts[1] > 1650) and np.all(im_pts[1] < 1723)): - print(tri_ind) - - - - mask = np.zeros(img.shape[:2], dtype=np.uint8) - for tri_ind in range(len(triangles)): - if triangle_covarage[tri_ind, image_id_ind] > 0: - v = int(np.round(triangle_covarage[tri_ind, image_id_ind]*255/max_text_res)) - i, j, k = triangles[tri_ind] - wrld_pts = np.vstack([vertices[i], vertices[j], vertices[k]]).T - im_pts = np.round(cm.project(wrld_pts, image_id)).astype(int) - cv2.drawContours(mask, [im_pts.T], 0, color=v, thickness=-1) - - plt.imshow(mask) - - for tri_ind in range(len(triangles)): - if tri_cov_map[tri_ind] == image_id_ind: - i, j, k = triangles[tri_ind] - wrld_pts = np.vstack([vertices[i], vertices[j], vertices[k]]).T - im_pts = np.round(cm.project(wrld_pts, image_id)).astype(int) - cv2.drawContours(img, [im_pts.T], 0, 255, thickness=-1) - - plt.imshow(img) - - -# Create the mask for each image so we know how much to include in the texture. -images_for_texture = {} -for image_id_ind, image_id in enumerate(colmap_images_inds): - print('Processing texture for image:', image_id) - image = colmap_images[image_id] - - ind = np.where(tri_cov_map == image_id_ind)[0] - - if len(ind) == 0: - continue - - img = cv2.imread('%s/%s' % (image_dir, image.name))[:, :, ::-1].copy() - - #raise Exception() - - cm = sfm_cms[image.camera_id] - - if False: - # Draw triangles. - image_mask = np.zeros((cm.height, cm.width), dtype=np.uint8) - - for i, j, k in triangles[ind]: - wrld_pts = np.vstack([vertices[i], vertices[j], vertices[k]]).T - im_pts = np.round(cm.project(wrld_pts, image_id)).astype(int) - cv2.drawContours(image_mask, [im_pts.T], 0, 255, thickness=4) - - indh = np.where(np.any(image_mask, axis=0))[0] - indv = np.where(np.any(image_mask, axis=1))[0] - - # left, right, top, bottom - left, right, top, bottom = indh[0], indh[-1], indv[0], indv[-1] - else: - vertices[list(set(triangles[ind].ravel()))] - im_pts = cm.project(vertices[list(set(triangles[ind].ravel()))].T, image_id) - left = int(np.floor(min(im_pts[0]))) - right = int(np.ceil(max(im_pts[0]))) - top = int(np.floor(min(im_pts[1]))) - bottom = int(np.ceil(max(im_pts[1]))) - - left -= 2 - right += 2 - top -= 2 - bottom += 2 - - if True: - # Use entire image. - left = 0 - right = cm.width - top = 0 - bottom = cm.height - - left = max([left, 0]) - top = max([top, 0]) - right = min([right, cm.width]) - bottom = min([bottom, cm.height]) - - in_w, in_h = right - left, bottom - top - - s = max_image_width/in_w - s = min([s, 1]) - final_height = int(np.ceil(in_h*s)) - final_width = max_image_width - - x = (right + left)/2.0 - y = (top + bottom)/2.0 - M = np.array([[1/s, 0, x - final_width/2/s], - [0, 1/s, y - final_height/2/s], - [0, 0, 1]]) - - if s < 0.75: - flags = cv2.INTER_AREA | cv2.WARP_INVERSE_MAP - else: - flags = cv2.INTER_CUBIC | cv2.WARP_INVERSE_MAP - - warped_img = cv2.warpAffine(img, M[:2], (final_width, final_height), - borderValue=(0, 0, 0), flags=flags) - - images_for_texture[image_id] = [np.linalg.inv(M), warped_img] - - -if True: - # Only works when all images are the same resolution - shape0 = None - for image_id in images_for_texture: - if shape0 is None: - shape0 = images_for_texture[image_id][1].shape - else: - assert images_for_texture[image_id][1].shape == shape0 - - h, w = shape0[:2] - L = len(images_for_texture) - #L = 310 - if L == 1: - nrows = 1 - ncols = 1 - else: - # Do something silly and inefficient for now. - best_nrows = None - best_ncols = None - min_max_res = np.inf - for nrows in range(1, L): - for ncols in range(1, L): - if nrows*ncols < L: - continue - - curr = max(nrows*h, ncols*w) - if curr < min_max_res: - min_max_res = curr - best_nrows = nrows - best_ncols = ncols - - nrows = best_nrows - ncols = best_ncols - - offsets = [] - for i in range(nrows): - for j in range(ncols): - offsets.append([i*h, j*w]) - - texture = np.zeros((int(nrows*h), int(ncols*w), 3), dtype=np.uint8) - - # Make the texture - final_homographies = {} - for i, image_id in enumerate(images_for_texture): - #print('Processing image:', image_id) - # M is a homography that takes raw-image coordinates and returns the - # reduced-resolution image coordinates. - M, img = images_for_texture[image_id] - - dy, dx = offsets[i] - - M = np.dot([[1, 0, dx], [0, 1, dy], [0, 0, 1]], M)[:2] - - texture[dy:dy+img.shape[0], dx:dx+img.shape[1], :] = img - final_homographies[image_id] = M - - -# v_uv is a 3*num_triangles x 2 array providing normalized texture coordinates -# for each triangle. -v_uv = np.zeros((3*len(triangles), 2), dtype=np.float32) -for image_id_ind, image_id in enumerate(colmap_images_inds): - print('Generating UV for Image:', image_id) - try: - M = final_homographies[image_id] - except KeyError: - continue - - image = colmap_images[image_id] - cm = sfm_cms[image.camera_id] - - if False: - # Sanity check - image = colmap_images[image_id] - img = cv2.imread('%s/%s' % (image_dir, image.name))[:, :, ::-1].copy() - plt.imshow(img) - - # Indices of the triangles we care about. - ind = np.where(tri_cov_map == image_id_ind)[0] - - for ind_ in ind: - i, j, k = triangles[ind_] - wrld_pts = np.vstack([vertices[i], vertices[j], vertices[k]]).T - im_pts = cm.project(wrld_pts, image_id) - - if (np.any(im_pts < 0) or np.any(im_pts[0] > cm.width) - or np.any(im_pts[1] > cm.height)): - continue - - im_pts = np.vstack([im_pts, [1, 1, 1]]) - im_pts = np.dot(M, im_pts) - - v_uv[ind_*3, 0] = im_pts[0, 0]/texture.shape[1] - v_uv[ind_*3 + 1, 0] = im_pts[0, 1]/texture.shape[1] - v_uv[ind_*3 + 2, 0] = im_pts[0, 2]/texture.shape[1] - - v_uv[ind_*3, 1] = im_pts[1, 0]/texture.shape[0] - v_uv[ind_*3 + 1, 1] = im_pts[1, 1]/texture.shape[0] - v_uv[ind_*3 + 2, 1] = im_pts[1, 2]/texture.shape[0] - - -mesh.triangle_uvs = o3d.utility.Vector2dVector(v_uv) -mesh.textures=[o3d.geometry.Image(texture)] - -#o3d.visualization.draw_geometries([mesh]) - -fname = 'test.obj' -print('Saving: \'%s\'' % fname) -o3d.io.write_triangle_mesh(fname, mesh, write_vertex_colors=False, - write_triangle_uvs=True, compressed=True) diff --git a/kamera/colmap_processing/scripts/xfm4x4_model.py b/kamera/colmap_processing/scripts/xfm4x4_model.py deleted file mode 100755 index 0d9c97e..0000000 --- a/kamera/colmap_processing/scripts/xfm4x4_model.py +++ /dev/null @@ -1,99 +0,0 @@ -#!/usr/bin/env python -# ckwg +31 -# Copyright 2021 by Kitware, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions are met: -# -# * Redistributions of source code must retain the above copyright notice, -# this list of conditions and the following disclaimer. -# -# * Redistributions in binary form must reproduce the above copyright notice, -# this list of conditions and the following disclaimer in the documentation -# and/or other materials provided with the distribution. -# -# * Neither name of Kitware, Inc. nor the names of any contributors may be used -# to endorse or promote products derived from this software without specific -# prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS ``AS IS'' -# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -# ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHORS OR CONTRIBUTORS BE LIABLE FOR -# ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -# DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -# SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -# OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -# ============================================================================== -''' -Eugene_Borovikov@Kitware.com: read COLMAP model, apply a 4x4 similarity transform and save the transformed model, e.g. geo-registration. -''' -import argparse, logging, os, numpy as np -from colmap_processing.colmap_interface import read_model, write_model, rotmat2qvec, Image, Point3D - - -def sRt(M): - U = M[:3,:3] - s = np.linalg.norm(U[0,:]) - R = U/s - t = M[:3,3] - return s,R,t - - -def xfm_img(img, s, R, t): - Ri = img.qvec2rotmat() - U = Ri.dot(R.T) - qi = rotmat2qvec(U) - ti = s*img.tvec - U.dot(t) - return Image(id=img.id, qvec=qi, tvec=ti, - camera_id=img.camera_id, name=img.name, - xys=img.xys, point3D_ids=img.point3D_ids) - - -def xfm_pt(pt, s, R, t): - xyz = s*R.dot(pt.xyz)+t - return Point3D(id=pt.id, xyz=xyz, rgb=pt.rgb, - error=pt.error, image_ids=pt.image_ids, - point2D_idxs=pt.point2D_idxs) - - -def run(args): -### load transform - M = np.loadtxt(args.xfm) - logging.info('xfm={}'.format(M)) -### load model - cameras, images, points3D = read_model(path=args.input_path, ext=args.input_ext) - logging.info('num_cameras={}'.format(len(cameras))) - logging.info('num_images={}'.format(len(images))) - logging.info('num_points3D={}'.format(len(points3D))) -### transform - s,R,t = sRt(M) - images = {ID: xfm_img(img, s, R, t) for ID, img in images.items()} - points3D = {ID: xfm_pt(pt, s, R, t) for ID, pt in points3D.items()} -### output - if not os.path.isdir(args.output_path): os.makedirs(args.output_path) - write_model(cameras, images, points3D, path=args.output_path, ext=args.output_ext) - logging.info('written model{} to {}'.format(args.output_ext, args.output_path)) - - -def CLI(argv=None): - parser = argparse.ArgumentParser(description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter) - parser.add_argument('-i:p', '--input_path', metavar='path', - help='path/to/colmap/model/folder; default=%(default)s') - parser.add_argument('-i:x', '--input_ext', metavar='ext', choices=['.bin','.txt'], default='.bin', help='input model format: %(choices)s; default=%(default)s') - parser.add_argument('-l', '--log', metavar='level', choices=['CRITICAL', 'ERROR', 'WARNING', 'INFO', 'DEBUG', 'NOTSET'], - default='WARNING', help='logging verbosity level: %(choices)s; default=%(default)s') - parser.add_argument('-o:p', '--output_path', metavar='path', - help='path/to/output/folder; default=%(default)s') - parser.add_argument('-o:x', '--output_ext', metavar='ext', default='.bin', help='output model format; default=%(default)s') - parser.add_argument('-x', '--xfm', metavar='path', - help='path/to/4x4/similarity/transform; default=%(default)s') - args = parser.parse_args(argv) - logging.basicConfig(level=args.log) - run(args) - - -if __name__ == '__main__': CLI() diff --git a/kamera/colmap_processing/slam.py b/kamera/colmap_processing/slam.py deleted file mode 100644 index aaa4fed..0000000 --- a/kamera/colmap_processing/slam.py +++ /dev/null @@ -1,1386 +0,0 @@ - -#!/usr/bin/env python -""" -ckwg +31 -Copyright 2020 by Kitware, Inc. -All rights reserved. - -Redistribution and use in source and binary forms, with or without -modification, are permitted provided that the following conditions are met: - - * Redistributions of source code must retain the above copyright notice, - this list of conditions and the following disclaimer. - - * Redistributions in binary form must reproduce the above copyright notice, - this list of conditions and the following disclaimer in the documentation - and/or other materials provided with the distribution. - - * Neither name of Kitware, Inc. nor the names of any contributors may be used - to endorse or promote products derived from this software without specific - prior written permission. - -THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS ``AS IS'' -AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHORS OR CONTRIBUTORS BE LIABLE FOR -ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - -============================================================================== - -Library handling projection operations of a standard camera model. - -Note: the image coordiante system has its origin at the center of the top left -pixel. - -""" -from __future__ import division, print_function, absolute_import -import numpy as np -import cv2 -import copy -import os -import time -from random import shuffle -import bisect -from math import sqrt -import gtsam -from gtsam import (Cal3_S2, DoglegOptimizer, GenericProjectionFactorCal3_S2, - Marginals, NonlinearFactorGraph, PinholeCameraCal3_S2, - Point3, Pose3, PriorFactorPoint3, PriorFactorPose3, Rot3, - Values) -from gtsam import symbol_shorthand -L = symbol_shorthand.L -X = symbol_shorthand.X -Y = symbol_shorthand.Y -V = symbol_shorthand.V -W = symbol_shorthand.W -BIAS_KEY = 1234567 -DUMMY_BIAS_KEY = 12345678 - -try: - import matplotlib.pyplot as plt -except ImportError: - pass - -# Repository imports. -from colmap_processing.platform_pose import PlatformPoseInterp -from colmap_processing.calibration import horn -from colmap_processing.rotations import quaternion_from_matrix, \ - quaternion_matrix -from colmap_processing.colmap_interface import read_images_binary, \ - read_points3D_binary, read_cameras_binary, standard_cameras_from_colmap -from colmap_processing.colmap_interface import Image as ColmapImage - - -def fit_pinhole_camera(cm): - """Solve for a best-matching pinhole (distortion-free) camera model. - - For SLAM problems where we already have an accurate model for the camera's - intrinsic parameters, we only want to optimize camera pose and world - geometry at runtime. Therefore, it is convenient to warp image coordinates - within the original image to appear as if they came from a pinhole - (distrortion-free) camera. Since we might do reprojection error - calculations, we want to maintain the size of a 1x1 pixel projected into - the new pinhole camera to be as closer to 1x1. - - Parameters - ---------- - cm : camera_models.StandardCamera - Camera model with distortion. - - Returns - ------- - cm_pinhole : camera_models.StandardCamera - Camera model without distortion. - """ - cm_pinhole = copy.deepcopy(cm) - cm_pinhole.dist = 0 - im_pts = cm.points_along_image_border(1000) - - ray_pos, ray_dir = cm.unproject(im_pts, 0) - im_pts2 = cm_pinhole.project(ray_pos + ray_dir, 0) - - #plt.plot(im_pts[0],im_pts[1]); plt.plot(im_pts2[0],im_pts2[1]) - - dx1 = - min([min(im_pts2[0]), 0]) - dy1 = - min([min(im_pts2[1]), 0]) - dx2 = max([max(im_pts2[0]) - cm.width, 0]) - dy2 = max([max(im_pts2[1]) - cm.height, 0]) - cm_pinhole.cx = cm_pinhole.cx + dx1 - cm_pinhole.cy = cm_pinhole.cy + dy1 - cm_pinhole.width = cm_pinhole.width + int(np.ceil(dx1 + dx2)) - cm_pinhole.height = cm_pinhole.height + int(np.ceil(dy1 + dy2)) - - return cm_pinhole - - -def draw_keypoints(image, pts, radius=2, color=(255, 0, 0), - thickness=1, copy=False): - """ - :param image: Image/ - :type image: Numpy image - - :param pts: Keypoints to be drawn in the image. - :type pts: Numpy array num_pts x 2 - """ - pts = np.round(pts).astype(int) - - if len(color) == 3 and image.ndim == 2: - image = cv2.cvtColor(image, cv2.COLOR_GRAY2BGR) - elif copy: - image = image.copy() - - for pt in pts: - cv2.circle(image, (pt[0], pt[1]), radius, color, thickness) - - return image - - -def map_to_pinhole_problem(cm, im_pts_at_time): - """Map Colmap reconstruction to appear as if it were a pinhole camera. - - GTSAM can support a camera model including distortion. However, with the - camera model being known ahead of time, it is more computationally - efficient to pre-undistort the image points as if they came from a - particular pinhole camera. This function determines and returns the - appropriate pinhole camera model (i.e., distortion coefficients are zero), - and then it undistorts and returns the image feature points. When used - within a SfM pose optimization problem, this would yield the same poses as - the original distorted camera/points, only faster. - - sfm_cm : camera_models.StandardCamera - Camera model potentially with non-zero distortion coefficients. - - im_pts_at_time : dict - Dictionary taking image time and returning the tuple - (im_pts, point3D_ind), where 'im_pts' is a 2 x N array of image - coordinates and that are associated with wrld_pts[:, point3D_ind]. - - Returns - ------- - sfm_cm : camera_models.StandardCamera - Camera model potentially with zero distortion coefficients. - - im_pts_at_time : dict - Dictionary taking image time and returning the tuple - (im_pts, point3D_ind), where 'im_pts' is a 2 x N array of image - coordinates and that are associated with wrld_pts[:, point3D_ind]. - - """ - cm_pinhole = fit_pinhole_camera(cm) - - for t in im_pts_at_time: - im_pts, point3D_ind = im_pts_at_time[t] - ray_pos, ray_dir = cm.unproject(im_pts, t) - im_pts2 = cm_pinhole.project(ray_pos + ray_dir*100, t) - im_pts_at_time[t] = im_pts2, point3D_ind - - return cm_pinhole, im_pts_at_time - - -def reprojection_error(cm, im_pts_at_time, wrld_pts, wrld_pts_to_score=None, - plot_results=False): - """Calculate mean reprojection error in pixels. - - wrld_pts_to_score : None | bool array-like - Boolean array of length equal to the number of world points (columns of - wrld_pts) indicating which world points to consider when calculating - the error. If None, all will be scored. - """ - err = [] - image_times = sorted(list(im_pts_at_time.keys())) - N2 = 0 - N4 = 0 - N10 = 0 - N20 = 0 - N = 0 - point_dist = [] - for t in image_times: - ret = im_pts_at_time[t] - if ret is None: - continue - - im_pts, point3D_ind = ret - - if im_pts.shape[1] == 0: - continue - - if wrld_pts_to_score is not None: - ind = wrld_pts_to_score[point3D_ind] - im_pts = im_pts[:, ind] - point3D_ind = point3D_ind[ind] - if len(point3D_ind) == 0: - continue - - err_ = np.sqrt(np.sum((im_pts - cm.project(wrld_pts[:, point3D_ind], t))**2, axis=0)) - N2 += sum(err_ < 2) - N4 += sum(err_ < 4) - N10 += sum(err_ < 10) - N20 += sum(err_ < 20) - N += len(err_) - err.append(err_) - - ray_pos, ray_dir = cm.unproject(im_pts, t, normalize_ray_dir=True) - point_dir = wrld_pts[:, point3D_ind] - ray_pos - d = np.sum(ray_dir*point_dir, axis=0) - point_dist.append(d) - - point_dist = np.hstack(point_dist) - err = np.hstack(err) - - print('Error %0.1f%% < 2 pixels, %0.1f%% < 4 pixels, %0.1f%% < 10 pixels, ' - '%0.1f%% < 20 pixels' % (100*N2/N, 100*N4/N, 100*N10/N, 100*N20/N)) - - print('Point distance from camera %0.1f (0%%), %0.1f (25%%), ' - '%0.1f (50%%), %0.1f (75%%), %0.1f (100%%)' % - (np.percentile(point_dist, 0), - np.percentile(point_dist, 25), - np.percentile(point_dist, 50), - np.percentile(point_dist, 75), - np.percentile(point_dist, 100))) - - if plot_results: - plt.figure(num=None, figsize=(15.3, 10.7), dpi=80) - plt.rc('font', **{'size': 20}) - plt.rc('axes', linewidth=4) - plt.plot(np.sort(err), '.') - plt.xlabel('Percentile', fontsize=40) - plt.ylabel('Image Mean Error (pixels)', fontsize=40) - - return np.mean(err) - - -def show_solution_errors(cm, cm_ins, im_pts_at_time, wrld_pts): - """Calculate mean reprojection error in pixels. - """ - image_times = sorted(list(im_pts_at_time.keys())) - err2 = [] - err10 = [] - err20 = [] - err50 = [] - err70 = [] - gps_err = [] - for t in image_times: - im_pts, point3D_ind = im_pts_at_time[t] - err_ = np.sqrt(np.sum((im_pts - cm.project(wrld_pts[:, point3D_ind], t))**2, axis=0)) - err2.append(np.percentile(err_, 2)) - err10.append(np.percentile(err_, 10)) - err20.append(np.percentile(err_, 20)) - err50.append(np.percentile(err_, 50)) - err70.append(np.percentile(err_, 70)) - - pos0, quat0 = cm.platform_pose_provider.pose(t) - pos1 = cm_ins.platform_pose_provider.pos(t) - gps_err.append(pos0 - pos1) - - gps_err = np.array(gps_err).T - - plt.figure(num=None, figsize=(15.3, 10.7), dpi=80) - plt.rc('font', **{'size': 20}) - plt.rc('axes', linewidth=4) - plt.subplot(2, 1, 1) - plt.semilogy(err2, '.', label='2 percentile') - plt.semilogy(err10, '.', label='10 percentile') - plt.semilogy(err20, '.', label='20 percentile') - plt.semilogy(err50, '.', label='50 percentile') - plt.semilogy(err70, '.', label='70 percentile') - plt.xlabel('Image', fontsize=30) - plt.legend(fontsize=14) - plt.ylabel('Reprojection Error (pixels)', fontsize=30) - plt.subplot(2, 1, 2) - plt.plot(gps_err[0], label='X') - plt.plot(gps_err[1], label='Y') - plt.plot(gps_err[2], label='Z') - plt.xlabel('Image', fontsize=30) - plt.legend(fontsize=14) - plt.ylabel('Distance From\nINS Solution (m)', fontsize=30) - plt.tight_layout() - - -def show_reproj_error_on_images(cm, im_pts_at_time, wrld_pts, image_names, - image_dir, out_dir, wrld_pts_to_show=None, - radius=6, thickness=2): - """Show reprojection error on images. - - wrld_pts_to_show : None | bool array-like - Boolean array of length equal to the number of world points (columns of - wrld_pts) indicating which world points to consider when calculating - the error. If None, all will be shown. - """ - image_times = sorted(list(im_pts_at_time.keys())) - for t in image_times: - ret = im_pts_at_time[t] - if ret is None: - continue - - im_pts, point3D_ind = ret - - if wrld_pts_to_show is not None: - ind = wrld_pts_to_show[point3D_ind] - im_pts = im_pts[:, ind] - point3D_ind = point3D_ind[ind] - - if len(point3D_ind) == 0: - continue - - im_pts2 = cm.project(wrld_pts[:, point3D_ind], t) - - img = cv2.imread('%s/%s' % (image_dir, image_names[t]))[:, :, ::-1].copy() - - for i in range(im_pts.shape[1]): - pt1 = np.round(im_pts[:, i]).astype(int) - pt2 = np.round(im_pts2[:, i]).astype(int) - cv2.circle(img, (pt1[0], pt1[1]), radius, (255, 0, 0), thickness) - cv2.circle(img, (pt2[0], pt2[1]), radius, (0, 0, 255), thickness) - cv2.line(img, (pt1[0], pt1[1]), (pt2[0], pt2[1]), (0, 255, 0), - thickness) - - base = os.path.splitext(os.path.split(image_names[t])[1])[0] - cv2.imwrite('%s/%s.jpg' % (out_dir, base), img[:, :, ::-1]) - - -def rescale_sfm_to_ins(cm, ins, wrld_pts): - """Rescale the SfM result via similarity transform to match INS outputs. - - The structure from motion (SfM) solution is self consistent but only - defined up to an arbitrary similarity transform. We are going to optimize - the results of the SfM to best match the INS ouputs. But, a good initial - alignment can be done by solving for the similarity transform that warps - the SfM results as close as possible to the INS results. - - Parameters - ---------- - cm : camera_models.StandardCamera - Structure from motion camera model. - - ins : platform_pose.PlatformPose - Representation of the INS-reported pose as function of time. - - wrld_pts : 3 x N array - 3-D coordinates of world points that are correlated with image points. - - Returns - ------- - cm_out : camera_models.StandardCamera - Structure from motion camera model. - - wrld_pts_out : 3 x N array - 3-D coordinates of world points that are correlated with image points. - - """ - ppp = cm.platform_pose_provider - - # Rescale using the ins. - pts1 = [] - pts2 = [] - for i, t in enumerate(ppp.times): - P = cm.get_camera_pose(t) - R = P[:, :3] - pts1.append(np.dot(-R.T, P[:, 3])) - pts2.append(ins.pose(t)[0]) - - pts1 = np.array(pts1).T - pts2 = np.array(pts2).T - s, R, trans = horn(pts1, pts2, fit_scale=True, fit_translation=True) - - wrld_pts_out = np.dot(R, s*wrld_pts) + np.atleast_2d(trans).T - - cm_out = copy.deepcopy(cm) - ppp2 = PlatformPoseInterp(ppp.lat0, ppp.lon0, ppp.h0) - for i in range(len(ppp.times)): - t = ppp.times[i] - pos, quat = ppp.pose(t) - pos = np.dot(R, s*pos) + trans - R0 = quaternion_matrix(quat)[:3, :3] - R1 = np.dot(R, R0) - quat = quaternion_from_matrix(R1) - ppp2.add_to_pose_time_series(t, pos, quat) - - cm_out.platform_pose_provider = ppp2 - - return cm_out, wrld_pts_out - - -def read_colmap_results(recon_dir, use_camera_id=None, max_images=None, - max_image_pts=None, min_track_len=None): - """Read colmap bin data and return camera and image with 3d point pairs. - - This function assumes that the image filenames encode the integer number of - microseconds since the Unix epoch. - - Parameters - ---------- - recon_dir : str - Direction in which to find colmap 'images.bin', 'cameras.bin', and - 'points3D.bin' files. - - use_camera_id : int | None - If Colmap didn't use a single camera for all images, we can force to - use one camera model for all images. This sets the index of the desired - camera to use. - - :max_images: int | None - Sets maximum number of images to consider. Reducing this value allows - experimentation with smaller problems that are quicker to process. - - :max_image_pts: int | None - Sets the maximum number of image feature points to consider per image. - If set, each image that exceeds this number will have a randsom subset - of this size returned. - - :min_track_len: int - Only consider features that were tracked for this minimum number of - images. - - Returns - ------- - sfm_cm : camera_models.StandardCamera - Camera model that accepts - - im_pts_at_time : dict - Dictionary taking image time and returning the tuple - (im_pts, point3D_ind), where 'im_pts' is a 2 x N array of image - coordinates and that are associated with wrld_pts[:, point3D_ind]. - - wrld_pts : 3 x N array - 3-D coordinates of world points that are correlated with image points. - - image_names : dict - Dictionary taking image time (s) and returning image name. - - - """ - # ------------------- Read Existing Colmap Reconstruction ---------------- - # Read in the Colmap details of all images. - images_bin_fname = '%s/images.bin' % recon_dir - colmap_images = read_images_binary(images_bin_fname) - camera_bin_fname = '%s/cameras.bin' % recon_dir - colmap_cameras = read_cameras_binary(camera_bin_fname) - points_bin_fname = '%s/points3D.bin' % recon_dir - points3d = read_points3D_binary(points_bin_fname) - - if max_images is not None: - colmap_images0 = colmap_images - colmap_images = {} - for t in sorted(list(colmap_images0.keys()))[:max_images]: - colmap_images[t] = colmap_images0[t] - - if use_camera_id is None: - keys = list(colmap_cameras.keys()) - if len(keys) > 1: - raise Exception('Found multiply camera models %s, need to pick ' - 'which one to use for all images' % str(keys)) - - use_camera_id = keys[0] - - image_times = {} - for ind in colmap_images: - colmap_images[ind] = ColmapImage(colmap_images[ind].id, - colmap_images[ind].qvec, - colmap_images[ind].tvec, - use_camera_id, - colmap_images[ind].name, - colmap_images[ind].xys, - colmap_images[ind].point3D_ids) - img_fname = os.path.split(colmap_images[ind].name)[1] - image_times[ind] = float(os.path.splitext(img_fname)[0])/1000000 - - sfm_cm = standard_cameras_from_colmap(colmap_cameras, colmap_images, - image_times)[use_camera_id] - - wrld_pts = [points3d[i].xyz if i in points3d else None - for i in range(max(points3d.keys()) + 1)] - - track_len = np.zeros(len(wrld_pts), dtype=int) - for image_num in colmap_images: - image = colmap_images[image_num] - point3D_ind = image.point3D_ids - track_len[point3D_ind] += 1 - - used_3d = np.zeros(len(wrld_pts), dtype=bool) - for image_num in colmap_images: - image = colmap_images[image_num] - point3D_ind = image.point3D_ids - point3D_ind = image.point3D_ids - ind = point3D_ind != -1 - point3D_ind = point3D_ind[ind] - - if min_track_len is not None: - ind = track_len[point3D_ind] >= min_track_len - point3D_ind = point3D_ind[ind] - - if max_image_pts is not None and len(point3D_ind) > max_image_pts: - ind = np.argsort([track_len[i] for i in point3D_ind])[::-1][:max_image_pts] - point3D_ind = point3D_ind[ind] - - used_3d[point3D_ind] = True - - im_pts_at_time = {} - image_names = {} - for image_num in colmap_images: - image = colmap_images[image_num] - xys = image.xys - point3D_ind = image.point3D_ids - ind = point3D_ind != -1 - point3D_ind = point3D_ind[ind] - xys = xys[ind] - ind = used_3d[point3D_ind] - point3D_ind = point3D_ind[ind] - xys = xys[ind] - - img_fname = os.path.split(image.name)[1] - t = float(os.path.splitext(img_fname)[0])/1000000 - im_pts_at_time[t] = (xys.T, point3D_ind) - image_names[t] = image.name - - # Remove ununsed 3d points. - ind = np.where(used_3d)[0] - orig_map = np.full(len(wrld_pts), -1, dtype=int) - orig_map[ind] = range(len(ind)) - wrld_pts = np.array([wrld_pts[i] for i in ind]).T - - for t in im_pts_at_time: - xys, point3D_ind = im_pts_at_time[t] - ind = point3D_ind >= 0 - xys = xys[:, ind] - point3D_ind = point3D_ind[ind] - point3D_ind = orig_map[point3D_ind] - im_pts_at_time[t] = xys, point3D_ind - - return sfm_cm, im_pts_at_time, wrld_pts, image_names - - -def stereo_pair_marginalize_pts(K, im_pts1, im_pts2, pixel_sigma=3, max_sep=1000, - max_viz_dist=1e4): - im_pts1 = im_pts1.astype(np.float32) - im_pts2 = im_pts2.astype(np.float32) - E, mask = cv2.findEssentialMat(im_pts1.T, im_pts2.T, K, threshold=3) - retval, R, t, mask, points3d0 = cv2.recoverPose(E, im_pts1.T, im_pts2.T, K, - distanceThresh=1e10, - mask=mask) - mask = mask.ravel() > 0 - im_pts1 = im_pts1[:, mask] - im_pts2 = im_pts2[:, mask] - points3d0 = points3d0[:, mask] - - cam2_pos = -np.dot(R.T, t).ravel() - - if True: - # This produces triangulation that has a balanced consistency with both - # camera views. - P1 = np.hstack([K, np.zeros((3, 1))]) - t *= max_sep - P2 = np.hstack([np.dot(K, R), t]) - points3d0 = cv2.triangulatePoints(P1, P2, im_pts1, im_pts2) - - # Do a chirality check for the points. - points3d0 = points3d0/points3d0[3] - points3d0[:2] = points3d0[:2]*np.sign(points3d0[2]) - - xyz = np.dot(np.hstack([R, t]), points3d0) - mask = xyz[2] > 0 - im_pts1 = im_pts1[:, mask] - im_pts2 = im_pts2[:, mask] - points3d0 = points3d0[:, mask] - - if False: - N = 300 - im_pts1 = im_pts1[:, :N] - im_pts2 = im_pts2[:, :N] - points3d0 = points3d0[:, :N] - - if False: - # Sanity check. - P1 = np.hstack([K, np.zeros((3, 1))]) - P2 = np.hstack([np.dot(K, R), t]) - im_pts1_ = np.dot(P1, points3d0) - im_pts2_ = np.dot(P2, points3d0) - im_pts1_ = im_pts1_[:2]/im_pts1_[2] - im_pts2_ = im_pts2_[:2]/im_pts2_[2] - - N = im_pts1.shape[1] - Sigma = np.diag([pixel_sigma**2, pixel_sigma**2]) - inv_sigma = np.linalg.inv(Sigma) - dx = im_pts1 - im_pts1_ - err1 = sum([np.dot(np.dot(dx_, inv_sigma), dx_)/2 for dx_ in dx.T]) - dx = im_pts2 - im_pts2_ - err2 = sum([np.dot(np.dot(dx_, inv_sigma), dx_)/2 for dx_ in dx.T]) - err = err1 + err2 - print('Total error: ', err) - - #plt.plot(im_pts2[0], im_pts2[1], 'go') - #plt.plot(im_pts2_[0], im_pts2_[1], 'bo') - print(sqrt(np.mean(np.sum((im_pts1 - im_pts1_)**2, 0)))) - print(sqrt(np.mean(np.sum((im_pts2 - im_pts2_)**2, 0)))) - - points3d = (points3d0[:3]/points3d0[3]).T - - graph = NonlinearFactorGraph() - - gtsam_camera = Cal3_S2(K[0, 0], K[1, 1], 0.0, K[0, 2], K[1, 2]) - - poses = [Pose3(Rot3(np.identity(3)), [0, 0, 0]), - Pose3(Rot3(R.T), cam2_pos)] - - if True: - factor = gtsam.NonlinearEqualityPose3(X(0), poses[0]) - else: - pose_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array(np.ones(6)*1e-6)) - factor = PriorFactorPose3(X(0), poses[0], pose_noise) - - graph.push_back(factor) - - #factor = PriorFactorPose3(X(1), poses[1], pose_noise) - #graph.push_back(factor) - - if True: - pose_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([1e6, 1e6, 1e6, - max_sep*50, - max_sep*50, - max_sep*50])) - factor = PriorFactorPose3(X(1), poses[1], pose_noise) - graph.push_back(factor) - - measurement_noise1 = gtsam.noiseModel.Isotropic.Sigma(2, pixel_sigma) - measurement_noise2 = gtsam.noiseModel.Isotropic.Sigma(2, pixel_sigma) - - for j in range(im_pts1.shape[1]): - factor = GenericProjectionFactorCal3_S2(im_pts1[:, j], - measurement_noise1, - X(0), L(j), - gtsam_camera, True, True) - graph.push_back(factor) - factor = GenericProjectionFactorCal3_S2(im_pts2[:, j], - measurement_noise2, - X(1), L(j), - gtsam_camera, True, True) - graph.push_back(factor) - - initial_estimate = Values() - for i, pose in enumerate(poses): - initial_estimate.insert(X(i), pose) - - max_viz_dist_noise = gtsam.noiseModel.Diagonal.Sigmas([max_viz_dist, - max_viz_dist, - max_viz_dist]) - for j in range(len(points3d)): - initial_estimate.insert(L(j), points3d[j]) - - if True: - # Constrain the points are nearby. - factor = PriorFactorPoint3(L(j), points3d[j], - max_viz_dist_noise) - graph.push_back(factor) - - params = gtsam.LevenbergMarquardtParams() - #params.setMaxIterations(1000) - #params.setAbsoluteErrorTol(1e-16) - #params.setRelativeErrorTol(1e-16) - optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial_estimate, - params) - - #err0 = optimizer.error() - result = optimizer.optimize() - - marginals = gtsam.Marginals(graph, result) - pose_cov = marginals.marginalCovariance(X(1)) - pose = result.atPose3(X(1)) - - if False: - # Sanity check. - wrld_pts_ = np.array([result.atPoint3(L(i)) - for i in range(len(points3d))]).T - wrld_pts_ = np.vstack([wrld_pts_, np.ones(wrld_pts_.shape[1])]) - R_ = pose.rotation().matrix().T - cam_pos_ = np.atleast_2d(pose.translation()).T - P2 = np.hstack([np.dot(K, R_), -np.dot(R_, cam_pos_)]) - P1 = np.hstack([K, np.zeros((3, 1))]) - im_pts1_ = np.dot(P1, wrld_pts_) - im_pts2_ = np.dot(P2, wrld_pts_) - im_pts1_ = im_pts1_[:2]/im_pts1_[2] - im_pts2_ = im_pts2_[:2]/im_pts2_[2] - #plt.plot(im_pts2[0], im_pts2[1], 'go') - #plt.plot(im_pts2_[0], im_pts2_[1], 'bo') - - N = im_pts1.shape[1] - Sigma = np.diag([pixel_sigma**2, pixel_sigma**2]) - inv_sigma = np.linalg.inv(Sigma) - dx = im_pts1 - im_pts1_ - err1 = sum([np.dot(np.dot(dx_, inv_sigma), dx_)/2 for dx_ in dx.T]) - dx = im_pts2 - im_pts2_ - err2 = sum([np.dot(np.dot(dx_, inv_sigma), dx_)/2 for dx_ in dx.T]) - err = err1 + err2 - print('Total error: ', err) - - print(sqrt(np.mean(np.sum((im_pts1 - im_pts1_)**2, 0)))) - print(sqrt(np.mean(np.sum((im_pts2 - im_pts2_)**2, 0)))) - - return pose, pose_cov - - -class OfflineSLAM(object): - def __init__(self, cm, min_pos_std=None, min_orientation_std=None, - pixel_sigma=10, ins_drift_rate=[0.1, 0.5], - imu_drift_rate=[0.1, 0.5], robust_pixels_k=4, robust_ins_k=4, - max_viz_dist=None, balance_measurements=False, - estimate_camera=False): - """Initialize the offline SLAM instance. - - Parameters - ---------- - sfm_cm : camera_models.StandardCamera - Camera model for the camera the acquired the image feature points. - - :min_pos_std: float, 3-array of float | None - GPS and INS may at times report a covariance for estimate position - that is overconfident. Therefore, this parameter clamps the assumed - estimated position standard deviation to at least this minimum - value. If a scalar, it applied to the x, y, and z components of the - position covariance. If a 3-array, it specifies each axis - seperately. - - :min_orientation_std: float, 3-array of float | None - INS may at times report a covariance for estimated orientation that - is overconfident. Therefore, this parameter clamps the assumed - estimated orientation standard deviations to at least this minimum - value (radians). If a scalar, it applied to the yaw, pitch, and - roll components of the covariance uniformly. If a 3-array, it - specifies each (yaw, pitch, roll) seperately. - - :pixel_sigma: float - Estimated image feature location is assumed to subject to random - noise with standard deviation equal to this value. - - :ins_drift_rate: 2-array - Model for the drift due to noise of the accelerometer and - gyroscope. The first element is the drift in position in - m/sqrt(hr). The second element is orientation drift in - deg/sqrt(hr). - - :imu_drift_rate: 2-array - Model for the drift due to noise of the accelerometer and - gyroscope. The first element is the accelerometer velocity random - walk (VRW) in m/sec/sqrt(hr). The second element is the gyro angle - random walk (ARW) in deg/sec/sqrt(hr). - - :robust_pixels_k: float - - robust_ins_k: float - - :param max_viz_dist: float - Maximum visible distance for landmark points. This adds a prior to - make reconstruction more stable. - - :balance_measurements: bool - Create an instance of the prior for pose per image feature - measurement. This balances the weighting of image features versus - INS pose measurements. - - :estimate_camera: bool - Estimate a correction to the orientation of the camera relative to - the INS. This will update cam_quat of the camera model output by - 'convert_solution'. - """ - # The camera that images the world may have distortion, but modeling it - # explicitly within the GTSAM optimization wastes repeated computation. - # So, we instead define a pinhole camera model that covers the field of - # view and locally through the image, one original-image pixel is - # approximately equal to one pinhole-camera pixel. Therefore, assuming - # distortion isn't too large, the calculated reprojection error won't - # change much. - self.cm = cm - cm_pinhole = fit_pinhole_camera(cm) - self.pinhole_K = cm_pinhole.K - - # Define a GTSAM pinhole camera model (no distortion) that we will use - # to optimize pose during SLAM. - self.gtsam_camera = Cal3_S2(cm_pinhole.fx, cm_pinhole.fy, 0.0, - cm_pinhole.cx, cm_pinhole.cy) - - if min_pos_std is not None: - if not hasattr(min_pos_std, "__len__"): - min_pos_std = [min_pos_std, min_pos_std, min_pos_std] - - min_pos_std = np.array(min_pos_std) - - if min_orientation_std is not None: - if not hasattr(min_orientation_std, "__len__"): - min_orientation_std = [min_orientation_std, min_orientation_std, - min_orientation_std] - - min_orientation_std = np.array(min_orientation_std) - - if imu_drift_rate is not None: - if len(imu_drift_rate) == 2: - imu_drift_rate = [imu_drift_rate[0], imu_drift_rate[0], imu_drift_rate[0], - imu_drift_rate[1], imu_drift_rate[1], imu_drift_rate[1]] - - imu_drift_rate = np.array(imu_drift_rate) - - if ins_drift_rate is not None: - if len(ins_drift_rate) == 2: - ins_drift_rate = [ins_drift_rate[0], ins_drift_rate[0], ins_drift_rate[0], - ins_drift_rate[1], ins_drift_rate[1], ins_drift_rate[1]] - - ins_drift_rate = np.array(ins_drift_rate) - - self.ins_drift_rate = ins_drift_rate - self.imu_drift_rate = imu_drift_rate - self.min_pos_std = min_pos_std - self.min_orientation_std = min_orientation_std - self.pixel_sigma = pixel_sigma - self.robust_ins_k = robust_ins_k - self.robust_pixels_k = robust_pixels_k - self.max_viz_dist = max_viz_dist - self.balance_measurements = balance_measurements - self.estimate_camera = estimate_camera - - # Rotation matrix that moves vectors from ins coordinate system into - # camera coordinate system. - Rcam = cm.get_camera_pose(0)[:, :3] - Rins = quaternion_matrix(cm.platform_pose_provider.pose(0)[1])[:3, :3].T - self.Rins_to_cam = np.dot(Rcam, Rins.T) - - self.reset_graph() - - def __str__(self): - string = ['OfflineSLAM\n'] - string.append('min_pos_std: %s\n' % str(self.min_pos_std)) - string.append('min_orientation_std: %s\n' % str(self.min_orientation_std)) - string.append('ins_drift_rate: %s\n' % str(self.ins_drift_rate)) - string.append('imu_drift_rate: %s\n' % str(self.imu_drift_rate)) - string.append('max_viz_dist: %s\n' % str(self.max_viz_dist)) - string.append('pixel_sigma: %s\n' % str(self.pixel_sigma)) - string.append('robust_ins_k: %s\n' % str(self.robust_ins_k)) - string.append('robust_pixels_k: %s\n' % str(self.robust_pixels_k)) - string.append('balance_measurements: %s\n' % str(self.balance_measurements)) - return ''.join(string) - - def __repr__(self): - return self.__str__() - - def reset_graph(self): - self.graph = NonlinearFactorGraph() - self.num_landmarks = None - - def pose_at_time_ins(self, t, weight=1): - """Return INS-predicted pose and pose_noise for time t. - - :weight: float - Weighting of the log likelihood desired when using pose_noise. - """ - # Provide a prior derived from the INS. - min_pos_std = self.min_pos_std - min_orientation_std = self.min_orientation_std - pos_ins, quat_ins, std = self.cm.platform_pose_provider.pose(t, - return_std=True) - std_e, std_n, std_u, std_y, std_p, std_r = std - - if self.min_pos_std is not None: - std_e = max([std_e, min_pos_std[0]]) - std_n = max([std_n, min_pos_std[1]]) - std_u = max([std_u, min_pos_std[2]]) - - if self.min_orientation_std is not None: - std_y = max([std_y, min_orientation_std[0]]) - std_p = max([std_p, min_orientation_std[1]]) - std_r = max([std_r, min_orientation_std[2]]) - - Rins = quaternion_matrix(quat_ins)[:3, :3].T - - # This is the estimate for the pose of the camera as predicted by - # the INS and the previous calibration ('Rins_to_cam') for the - # orientation of the camera relative to the INS. This is defined - # such that when it operates on a vector defined in the world, it - # returns the specification of that vector within the camera - # coordinate system. - Rcam_from_ins = np.dot(self.Rins_to_cam, Rins) - -# if cm_sfm is not None: -# P = cm_sfm.get_camera_pose(t) -# Rcam = P[:, :3] -# pos = np.dot(-Rcam.T, P[:, 3]) -# else: -# P = self.cm.get_camera_pose(t) - - pose = Pose3(Rot3(Rcam_from_ins.T), pos_ins) - - # Vector defined within the INS coordinate system representing the - # uncertainty of its orientation - rot_ind_std = np.array([std_r, std_p, std_y]) - - # Rotate into camera coordinate system - std_r, std_p, std_y = np.abs(np.dot(self.Rins_to_cam, rot_ind_std)) - - # Pose standard deviation defined in roll (rad), pitch (rad), yaw (rad), x - # (m), y (m), z (m). - d = np.array([std_r, std_p, std_y, std_e, std_n, std_u]) - pose_noise = gtsam.noiseModel.Diagonal.Sigmas(d/sqrt(weight)) - - if self.robust_ins_k is not None: - huber = gtsam.noiseModel.mEstimator.Huber.Create(self.robust_ins_k) - pose_noise = gtsam.noiseModel.Robust.Create(huber, pose_noise) - - return pose, pose_noise - - def define_problem(self, im_pts_at_time, wrld_pts, imu_data=None, - time_uncertainty=None): - """ - Parameters - ---------- - imu_data : N x 13 array - High-rate IMU data with the following columns: - [0] time (s) - [1] accel-x (m/s^2) - [2] accel-y (m/s^2) - [3] accel-z (m/s^2) - [4] gyro-x (rad/s) - [5] gyro-y (rad/s) - [6] gyro-z (rad/s) - [7] mag-x (Ga) - [8] mag-y (Ga) - [9] mag-z (Ga) - [10] imu-temp (C) - [11] pressure (Pa) - [12] ambient-temp (C) - - :time_uncertainty: float - The synchronization between image times and INS data times is - assumed to be within plus or minus this value (s). - - """ - print('Calculating initial poses and adding pose prior estimates') - tic = time.time() - - self.initial_estimate = Values() - - if self.estimate_camera: - # 'self.Rins_to_cam', derived from the camera model's cam_quat - # quaternion, hopefully provides a good initial guess for the - # orientation of the camera relative to the navigation coordinate - # system encoded by 'cm.platform_provider'. We want to solve for an - # additional rotation beyond the navigation-estimated pose, - # applied in the initially-estimated camera coordinate system so - # that the overall solution better agrees with the image feature - # measurements. However, gtsam does not have a direct way to do - # this. Instead, we apply a hack to the gtsam capability used to - # estimate IMU/gyro bias. For each ith image exposure time, we - # define two poses, X(i) - params = gtsam.PreintegrationParams.MakeSharedU(0) - params.setAccelerometerCovariance(1e-5*np.identity(3)) - params.setGyroscopeCovariance(1e-5*np.identity(3)) - params.setIntegrationCovariance(1e-5*np.identity(3)) - params.setOmegaCoriolis(np.array([0, 0, 0])) - self.fake_imu_params = params - zeroBias = gtsam.imuBias.ConstantBias(np.array([0, 0, 0]), - np.array([0, 0, 0])) - fake_pim = gtsam.PreintegratedImuMeasurements(params, zeroBias) - self.fake_pim = fake_pim - fake_pim.integrateMeasurement(np.zeros(3), np.zeros(3), 1) - self.initial_estimate.insert(W(0), np.zeros(3)) - self.initial_estimate.insert(W(1), np.zeros(3)) - self.initial_estimate.insert(DUMMY_BIAS_KEY, zeroBias) - same_pos_noise = gtsam.noiseModel.Diagonal.Sigmas([1e10,1e10,1e10, - 1e-1,1e-1, - 1e-1]) - - if self.ins_drift_rate is not None: - ins_drift_rate = np.zeros(6) - - # Swap the order since gtsam.noiseModel.Diagonal.Sigmas expects - # orientation uncertainty first. - - # Convert from m/sqrt(hr) to m/sqrt(s) - ins_drift_rate[3:] = ins_drift_rate[:3]/60 - - # Convert from deg/sqrt(hr) to rad/sqrt(s) - ins_drift_rate[:3] = ins_drift_rate[3:]*np.pi/180/60 - else: - ins_drift_rate = None - - if imu_data is not None: - # Set up all components related to integrating inertial measurement - # unit outputs. - assert self.imu_drift_rate is not None - - imu_data = imu_data[:, :7] - ind = np.argsort(imu_data[:, 0]) - imu_data = imu_data[ind, :7] - imu_times = imu_data[:, 0] - - # The constant value to assume over the whole time bin where - # you are in bin bisect.bisect_right(imu_times, t) - 1 for time t. - accel_gyro_data = (imu_data[:-1, 1:7] + imu_data[1:, 1:7])/2 - - # Rotate from the INS coordinate system into the camera coordinate - # system. - accel_gyro_data[:, :3] = np.dot(self.Rins_to_cam, accel_gyro_data[:, :3].T).T - accel_gyro_data[:, 3:] = np.dot(self.Rins_to_cam, accel_gyro_data[:, 3:].T).T - - gravity = 0 - #gravity = 9.81 - imu_params = gtsam.PreintegrationParams.MakeSharedU(gravity) - - # Convert from m/s/sqrt(hr) to m/s/sqrt(s) - accel_sigma = self.imu_drift_rate[:3]/60 - - # Convert from deg/s/sqrt(hr) to rad/s/sqrt(s) - gyro_sigma = self.imu_drift_rate[3:]*np.pi/180/60 - - imu_params.setAccelerometerCovariance(np.diag(accel_sigma**2)) - imu_params.setGyroscopeCovariance(np.diag(gyro_sigma**2)) - print(imu_params) - - #params.setIntegrationCovariance(np.zeros((3, 3))) - imu_params.setIntegrationCovariance(1e-3*np.identity(3, np.float)) - - # We are only integrating over a small time, so we can ignore the - # coriolis rate for this application. - imu_params.setOmegaCoriolis(np.array([0, 0, 0])) - - # We use zero bias here since we are just looking to smooth out - # rapid changes that shouldn't exist. We are still relying on the - # INS state output for the general constraining. - zeroBias = gtsam.imuBias.ConstantBias(np.array([0, 0, 0]), - np.array([0, 0, 0])) - - pim = gtsam.PreintegratedImuMeasurements(imu_params, zeroBias) - - self.initial_estimate.insert(BIAS_KEY, zeroBias) - - image_times = sorted(list(im_pts_at_time.keys())) - poses = [] - - # Create the set of ground-truth landmarks - points3d = wrld_pts.T - wrld_pts_used = np.zeros(len(points3d), dtype=bool) - - - if self.max_viz_dist is not None: - max_viz_dist_noise = gtsam.noiseModel.Diagonal.Sigmas([self.max_viz_dist, - self.max_viz_dist, - self.max_viz_dist]) - origin = Point3(np.zeros(3)) - max_viz_ref = [origin for _ in range(len(points3d))] - - for i, t in enumerate(image_times): - # Loop over all images - - # This image has 'N' points. - im_pts0, point3D_ind = im_pts_at_time[t] - - # These image points are within the, potentially, distorted camera. - # We need to map them to where they would have been imaged into the - # proxy pinhole camera model. - im_pts = cv2.undistortPoints(im_pts0.astype(np.float32), self.cm.K, - self.cm.dist, None, self.pinhole_K) - im_pts = im_pts.squeeze(axis=1).T - - if self.balance_measurements: - weight = sqrt(im_pts.shape[1]) - else: - weight = 1 - - # ------------------- Add pose prior from INS ------------------- - pose, pose_noise = self.pose_at_time_ins(t) - poses.append(pose) - - if ins_drift_rate is not None and i > 0: - for j in range(i): - dt = image_times[i] - image_times[j] - if dt > 1: - continue - - dpose_noise = gtsam.noiseModel.Diagonal.Sigmas(ins_drift_rate*sqrt(dt)) - - if self.robust_ins_k is not None: - huber = gtsam.noiseModel.mEstimator.Huber.Create(self.robust_ins_k) - dpose_noise = gtsam.noiseModel.Robust.Create(huber, dpose_noise) - - dpose = poses[j].between(poses[i]) - - factor = gtsam.BetweenFactorPose3(X(j), X(i), dpose, - dpose_noise) - self.graph.push_back(factor) - - if imu_data is not None and i < len(image_times) - 1: - pim.resetIntegration() - - # We need to consider the imu outputs between times t1 and t2 - t1 = image_times[i] - t2 = image_times[i + 1] - - if t2 - t1 < 5: - # This is the index for either an exact match for - # imu_times[ind1]=t1 or largest imu_times where imu_times[ind1]<=t1. - # This is the first bin that partially intersects with the timespan - # t1->t2. If negative, that means that there all imu_times > t1. - ind1 = bisect.bisect_right(imu_times, t1) - 1 - - # ind2 is the smallest imu_times value such that t2 < imu_times[ind2]. - # If ind2 == len(imu_times), that means there is no time such that - # all imu_times < t2. - ind2 = bisect.bisect_right(imu_times, t2) - - if ind2 == ind1 + 1: - # t1 and t2 live entirely inside the imu_times[ind1]->imu_times[ind2] - dt = t2 - t1 - pim.integrateMeasurement(accel_gyro_data[ind1, :3], - accel_gyro_data[ind1, 3:], dt) - else: - if ind1 >= 0 and ind2 > 1: - # t1 to t1 + dt is inside imu_times[ind1] -> imu_times[ind1 + 1]. - dt = imu_times[ind1 + 1] - t1 - assert dt > 0 - pim.integrateMeasurement(accel_gyro_data[ind1, :3], - accel_gyro_data[ind1, 3:], dt) - - for ind in range(ind1+1, ind2 - 1): - # These are bins that are entire covered by the range t1 -> t2. - dt = imu_times[ind + 1] - imu_times[ind] - pim.integrateMeasurement(accel_gyro_data[ind, :3], - accel_gyro_data[ind, 3:], dt) - - if ind2 < len(imu_times): - dt = t2 - imu_times[ind2 - 1] - if dt > 0: - pim.integrateMeasurement(accel_gyro_data[ind2-1, :3], - accel_gyro_data[ind2-1, 3:], dt) - - print(np.diag(pim.preintMeasCov())) - - factor = gtsam.ImuFactor(X(i), V(i), X(i+1), - V(i+1), BIAS_KEY, pim) - self.graph.push_back(factor) - - if time_uncertainty is None: - factor = PriorFactorPose3(X(i), poses[i], pose_noise) - self.graph.push_back(factor) - else: - # We don't exactly know where in the +/- time_uncertainty we - # reside, so we put a sampling of all so that we smooth out the - # prior over the possible times. - num_times = 50 - ts = np.linspace(t - time_uncertainty, t + time_uncertainty, - num_times) - for t_ in ts: - pose, pose_noise = self.pose_at_time_ins(t_, - weight=1/num_times) - factor = PriorFactorPose3(X(i), pose, pose_noise) - self.graph.push_back(factor) - - # -------------- Add image measurements for this image ----------- - - measurement_noise = gtsam.noiseModel.Isotropic.Sigma(2, - self.pixel_sigma*weight) - - # The idea here is that we want to balance the contribution to the - # overall likelihood between an INS factor and the factors from - # image measurements on one image. For delta^2 loss, where delta is - # a normalized version of some paramenter normalized by its - # standard deviation, when delta is 1 (i.e., 1-sigma) dLoss/ddelta - # is 2. If we similarly express the pixel error in units of sigma, - # we derivative of the sum of the loss over all pixel measurements - # in an image to equal 2. So, we can use Huber loss to effect this - # by chosing Huber k such that when all pixels errors are at - # 1-sigma, the sum of the derivatives equals 2. - # huber loss = (delta)^2/2 up to k with loss gradient x. So at k, - # slope isk forever. So, k = 2/N, where N is the number of points. - #robust_pixels_k = pixel_weight*2/N - #robust_pixels_k = 1.5 - robust_pixels_k = self.robust_pixels_k - -# robust_pixels_k = self.robust_pixels_k -# pixel_weight = 1 -# robust_pixels_k = pixel_weight*2/N - - if robust_pixels_k is not None: - huber = gtsam.noiseModel.mEstimator.Huber.Create(robust_pixels_k) - measurement_noise = gtsam.noiseModel.Robust.Create(huber, - measurement_noise) - - if self.estimate_camera: - # We apply a hack that uses gtsam's IMU/gyro bias estimation to - # solve for a correction to 'self.Rins_to_cam'. This isn't a - # real IMU/gyro that we are modeling, hence the name fake_pim. - # See the earlier comment in this method for more details. - pose_sym = Y(i) - self.initial_estimate.insert(Y(i), poses[i]) - factor = gtsam.ImuFactor(X(i), W(0), Y(i), W(1), - DUMMY_BIAS_KEY, fake_pim) - self.graph.push_back(factor) - - factor = gtsam.BetweenFactorPose3(X(i), Y(i), Pose3(), - same_pos_noise) - self.graph.push_back(factor) - else: - pose_sym = X(i) - - for j in range(im_pts.shape[1]): - factor = GenericProjectionFactorCal3_S2(im_pts[:, j], - measurement_noise, - pose_sym, - L(point3D_ind[j]), - self.gtsam_camera) - wrld_pts_used[point3D_ind[j]] = True - self.graph.push_back(factor) - - if self.max_viz_dist is not None: - max_viz_ref[j] = poses[-1].translation() - - # Create the data structure to hold the initial estimate to the - # solution intentionally initialize the variables off from the ground - # truth. - print('Setting initial solution') - - self.pose_times = image_times - for i, pose in enumerate(poses): - self.initial_estimate.insert(X(i), pose) - - if imu_data is not None: - self.initial_estimate.insert(V(i), np.zeros(3)) - - self.wrld_pts_orig = wrld_pts - self.wrld_pts_orig_ind = np.where(wrld_pts_used)[0] - for j in self.wrld_pts_orig_ind: - point = points3d[j] - self.initial_estimate.insert(L(j), point) - - if self.max_viz_dist is not None: - # Constrain the points are nearby. - factor = PriorFactorPoint3(L(j), max_viz_ref[j], - max_viz_dist_noise) - self.graph.push_back(factor) - - self.result = self.initial_estimate - - print('Time elapsed:', time.time() - tic) - - def update_camera_parameters(self, pixel_sigma=10, robust_pixels_k=4): - measurement_noise = gtsam.noiseModel.Isotropic.Sigma(2, pixel_sigma) - huber = gtsam.noiseModel.mEstimator.Huber.Create(robust_pixels_k) - measurement_noise = gtsam.noiseModel.Robust.Create(huber, - measurement_noise) - for i in range(self.graph.size()): - f = self.graph.at(i) - if isinstance(f, GenericProjectionFactorCal3_S2): - raise Exception() - xi, lj = f.keys() - f2 = GenericProjectionFactorCal3_S2(f.measured(), - measurement_noise, - xi, lj, f.calibration()) - self.graph.replace(i, f2) - - def update_reproj_sigma(self, pixel_sigma=10, robust_pixels_k=4): - measurement_noise = gtsam.noiseModel.Isotropic.Sigma(2, pixel_sigma) - huber = gtsam.noiseModel.mEstimator.Huber.Create(robust_pixels_k) - measurement_noise = gtsam.noiseModel.Robust.Create(huber, - measurement_noise) - for i in range(self.graph.size()): - f = self.graph.at(i) - if isinstance(f, GenericProjectionFactorCal3_S2): - xi, lj = f.keys() - f2 = GenericProjectionFactorCal3_S2(f.measured(), - measurement_noise, - xi, lj, f.calibration()) - self.graph.replace(i, f2) - - def solve(self): - """Solve problem, set self.results and update self.initial_estimate. - """ - tic = time.time() - # Optimize the graph and print results - print('Running optimizer') - if False: - params = gtsam.DoglegParams() - params.setAbsoluteErrorTol(1e-6) - params.setRelativeErrorTol(1e-6) - params.setVerbosity('TERMINATION') - optimizer = DoglegOptimizer(self.graph, self.initial_estimate, params) - print('Optimizing:') - elif False: - params = gtsam.GaussNewtonParams() - params.setAbsoluteErrorTol(1e-6) - params.setRelativeErrorTol(1e-6) - params.setVerbosity('TERMINATION') - optimizer = gtsam.GaussNewtonOptimizer(self.graph, self.initial_estimate, params) - print('Optimizing:') - - err0 = optimizer.error() - for _ in range(20): - result = optimizer.optimize() - err = optimizer.error() - print('Relative reduction in error:', (err0-err)/err0) - #err0 = err - else: - params = gtsam.LevenbergMarquardtParams() - optimizer = gtsam.LevenbergMarquardtOptimizer(self.graph, - self.initial_estimate, - params) - - self.result = optimizer.optimize() - print('Time elapsed:', time.time() - tic) - self.initial_estimate = self.result - return optimizer.error() - - def convert_solution(self): - if False: - marginals = gtsam.Marginals(self.graph, self.result) - pose_cov = marginals.marginalCovariance(X(1)) - pose = result.atPose3(X(1)) - - cm2 = copy.deepcopy(self.cm) - - if self.estimate_camera: - fake_bias = self.result.atConstantBias(DUMMY_BIAS_KEY) - print('Bias', fake_bias) - fake_pim = gtsam.PreintegratedImuMeasurements(self.fake_imu_params, - fake_bias) - fake_pim.integrateMeasurement(np.zeros(3), np.zeros(3), 1) - print('V(0)', self.result.atVector(W(0))) - ns0 = gtsam.NavState(Pose3(), self.result.atVector(W(0))) - ns = fake_pim.predict(ns0, - self.result.atConstantBias(DUMMY_BIAS_KEY)) - print('dPos', ns.position()) - R = ns.attitude().matrix() - #self.Rins_to_cam = np.dot(R.T, self.Rins_to_cam) - cm2.cam_quat = quaternion_from_matrix(np.dot(R.T, self.Rins_to_cam).T) - print(R) - - wrld_pts = self.wrld_pts_orig.copy() - wrld_pts_ = np.array([self.result.atPoint3(L(i)) - for i in self.wrld_pts_orig_ind]).T - wrld_pts[:, self.wrld_pts_orig_ind] = wrld_pts_ - - ppp = PlatformPoseInterp(self.cm.platform_pose_provider.lat0, - self.cm.platform_pose_provider.lon0, - self.cm.platform_pose_provider.h0) - for i in range(len(self.pose_times)): - t = self.pose_times[i] - pose = self.result.atPose3(X(i)) - R = pose.rotation().matrix().T - R = np.dot(self.Rins_to_cam.T, R) - # The gtsam rotation matrix is a coordinate system rotation. - quat = np.array(quaternion_from_matrix(R.T)) - quat *= np.sign(quat[-1]) - ppp.add_to_pose_time_series(t, [pose.x(), pose.y(), pose.z()], quat) - - cm2.platform_pose_provider = ppp - - position_err = [] - position_err0 = [] - for i, t in enumerate(self.pose_times): - pos0, quat0 = self.cm.platform_pose_provider.pose(t) - pos1 = cm2.platform_pose_provider.pos(t) - position_err.append(np.linalg.norm(pos0 - pos1)) - position_err0.append(np.linalg.norm(pos0 - self.cm.platform_pose_provider.pos(t))) - - print('Mean position difference reduced from', np.mean(position_err0), 'to', np.mean(position_err)) - - return cm2, wrld_pts - - diff --git a/kamera/colmap_processing/static_camera_model.py b/kamera/colmap_processing/static_camera_model.py deleted file mode 100644 index 7de91bc..0000000 --- a/kamera/colmap_processing/static_camera_model.py +++ /dev/null @@ -1,217 +0,0 @@ -#! /usr/bin/python -""" -ckwg +31 -Copyright 2018 by Kitware, Inc. -All rights reserved. - -Redistribution and use in source and binary forms, with or without -modification, are permitted provided that the following conditions are met: - - * Redistributions of source code must retain the above copyright notice, - this list of conditions and the following disclaimer. - - * Redistributions in binary form must reproduce the above copyright notice, - this list of conditions and the following disclaimer in the documentation - and/or other materials provided with the distribution. - - * Neither name of Kitware, Inc. nor the names of any contributors may be used - to endorse or promote products derived from this software without specific - prior written permission. - -THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS ``AS IS'' -AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHORS OR CONTRIBUTORS BE LIABLE FOR -ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - -============================================================================== - -""" -from __future__ import division, print_function -import numpy as np -import os, cv2, yaml, PIL -from PIL import Image - - -def to_str(v): - """Convert numerical values (scalar or float) to string for saving to yaml - - """ - if hasattr(v, "__len__"): - if len(v) > 1: - return repr(list(v)) - else: - v = v[0] - - return repr(v) - - -def load_static_camera_from_file(filename): - with open(filename, 'r') as f: - calib = yaml.load(f) - - assert calib['model_type'] == 'static' - - # fill in CameraInfo fields - width = calib['image_width'] - height = calib['image_height'] - dist = np.array(calib['distortion_coefficients'], dtype=np.float64) - - if isinstance(dist, str) and dist == 'None': - dist = np.zeros(4, dtype=np.float64) - - fx = calib['fx'] - fy = calib['fy'] - cx = calib['cx'] - cy = calib['cy'] - K = np.array([[fx, 0, cx], [0, fy, cy], [0, 0, 1]]) - R = np.reshape(np.array(calib['R']), (3, 3)) - latitude = calib['latitude'] - longitude = calib['longitude'] - altitude = calib['altitude'] -# image_topic = calib['image_topic'] -# frame_id = calib['frame_id'] - - depth_map_fname = '%s_depth_map.tif' % os.path.splitext(filename)[0] - try: - depth_map = np.asarray(PIL.Image.open(depth_map_fname)) - except OSError: - depth_map = None - - return height, width, K, dist, R, depth_map, latitude, longitude, altitude - - -def save_static_camera(filename, height, width, K, dist, R, depth_map, - latitude, longitude, altitude): - dist = np.array(dist, dtype=np.float32).ravel() - - with open(filename, 'w') as f: - f.write(''.join(['# The type of camera model.\n', - 'model_type: static\n\n', - '# Image dimensions\n'])) - - f.write(''.join(['image_width: ', to_str(width), '\n'])) - f.write(''.join(['image_height: ', to_str(height), '\n\n'])) - - f.write('# Focal length along the image\'s x-axis.\n') - f.write(''.join(['fx: ', to_str(K[0, 0]), '\n\n'])) - - f.write('# Focal length along the image\'s y-axis.\n') - f.write(''.join(['fy: ', to_str(K[1, 1]), '\n\n'])) - - f.write('# Principal point is located at (cx,cy).\n') - f.write(''.join(['cx: ', to_str(K[0, 2]), '\n'])) - f.write(''.join(['cy: ', to_str(K[1, 2]), '\n\n'])) - - f.write(''.join(['# Distortion coefficients following OpenCv\'s ', - 'convention\n'])) - f.write(''.join(['distortion_coefficients: ', - to_str(dist), '\n\n'])) - - f.write(''.join(['# Rotation matrix mapping vectors defined in an ' - 'east/north/up coordinate system\n# centered at ' - 'the camera into vectors defined in the camera' - 'coordinate system.\n', - 'R: [%0.10f, %0.10f, %0.10f,\n' - ' %0.10f, %0.10f, %0.10f,\n' - ' %0.10f, %0.10f, %0.10f]' % - tuple(R.ravel()), '\n\n'])) - - f.write(''.join(['# Location of the camera\'s center of ' - 'projection. Latitude and longitude are in\n# ' - 'degrees, and altitude is meters above the WGS84 ' - 'ellipsoid.\n', - 'latitude: %0.10f\n' % latitude, - 'longitude: %0.10f\n' % longitude, - 'altitude: %0.10f' % altitude,'\n\n'])) - - f.write('# Topic on which this camera\'s image is published\n') - f.write(''.join(['image_topic: \n\n'])) - - f.write('# The frame_id embedded in the published image header.\n') - f.write(''.join(['frame_id: '])) - - if depth_map is not None: - im = PIL.Image.fromarray(depth_map, mode='F') # float32 - depth_map_fname = '%s_depth_map.tif' % os.path.splitext(filename)[0] - im.save(depth_map_fname) - - -def write_camera_krtd(camera, fout): - """Write a single camera in ASCII KRTD format to the file object. - """ - K, R, t, d = camera - t = np.reshape(np.array(t), 3) - fout.write('%.12g %.12g %.12g\n' % tuple(K.tolist()[0])) - fout.write('%.12g %.12g %.12g\n' % tuple(K.tolist()[1])) - fout.write('%.12g %.12g %.12g\n\n' % tuple(K.tolist()[2])) - fout.write('%.12g %.12g %.12g\n' % tuple(R.tolist()[0])) - fout.write('%.12g %.12g %.12g\n' % tuple(R.tolist()[1])) - fout.write('%.12g %.12g %.12g\n\n' % tuple(R.tolist()[2])) - fout.write('%.12g %.12g %.12g\n\n' % tuple(t.tolist())) - for v in d: - fout.write('%.12g ' % v) - - -def write_camera_krtd_file(camera, filename): - """Write a camera to a krtd file - """ - with open(filename,'w') as f: - write_camera_krtd(camera, f) - - -def unproject_from_camera(im_pts, K, dist, R, cam_pos, depth_map): - # Unproject rays into the camera coordinate system. - ray_dir = np.ones((3, len(im_pts)), dtype=np.float) - ray_dir0 = cv2.undistortPoints(np.expand_dims(im_pts, 0), K, dist, R=None) - ray_dir[:2] = np.squeeze(ray_dir0, 0).T - - # We want the z-coordinate of the ray direction to be 1. - - enu0 = np.array(cam_pos, copy=True) - - # Rotate rays into the local east/north/up coordinate system. - ray_dir = np.dot(R.T, ray_dir) - - height, width = depth_map.shape - enu = np.zeros((len(im_pts), 3)) - for i in range(im_pts.shape[0]): - x, y = im_pts[i] - if x == 0: - ix = 0 - elif x == width: - ix = int(width - 1) - else: - ix = int(round(x - 0.5)) - - if y == 0: - iy = 0 - elif y == height: - iy = int(height - 1) - else: - iy = int(round(y - 0.5)) - - if ix < 0 or iy < 0 or ix >= width or iy >= height: - print(x == width) - print(y == height) - raise ValueError('Coordinates (%0.1f,%0.f) are outside the ' - '%ix%i image' % (x, y, width, height)) - - enu[i] = enu0 + ray_dir[:, i]*depth_map[iy, ix] - - return enu.T - - -def project_to_camera(wrld_pts, K, dist, R, cam_pos): - # Unproject rays into the camera coordinate system. - tvec = -np.dot(R, cam_pos).ravel() - rvec = cv2.Rodrigues(R)[0] - im_pts = cv2.projectPoints(wrld_pts.T, rvec, tvec, K, dist)[0] - im_pts = np.squeeze(im_pts) - - return im_pts diff --git a/kamera/colmap_processing/test/test_camera_models.py b/kamera/colmap_processing/test/test_camera_models.py deleted file mode 100644 index 7079cb8..0000000 --- a/kamera/colmap_processing/test/test_camera_models.py +++ /dev/null @@ -1,131 +0,0 @@ -#! /usr/bin/python -""" -ckwg +31 -Copyright 2020 by Kitware, Inc. -All rights reserved. - -Redistribution and use in source and binary forms, with or without -modification, are permitted provided that the following conditions are met: - - * Redistributions of source code must retain the above copyright notice, - this list of conditions and the following disclaimer. - - * Redistributions in binary form must reproduce the above copyright notice, - this list of conditions and the following disclaimer in the documentation - and/or other materials provided with the distribution. - - * Neither name of Kitware, Inc. nor the names of any contributors may be used - to endorse or promote products derived from this software without specific - prior written permission. - -THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS ``AS IS'' -AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHORS OR CONTRIBUTORS BE LIABLE FOR -ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - -============================================================================== - -""" -from __future__ import division, print_function -import numpy as np -import os -import cv2 -import matplotlib.pyplot as plt -from osgeo import osr, gdal -from scipy.optimize import fmin, minimize, fminbound -import transformations - -# Colmap Processing imports. -from colmap_processing.geo_conversions import llh_to_enu -from colmap_processing.colmap_interface import read_images_binary, Image, \ - read_points3d_binary, read_cameras_binary, qvec2rotmat -from colmap_processing.camera_models import StandardCamera -from colmap_processing.platform_pose import PlatformPoseInterp - - -# ---------------------------------------------------------------------------- -# Define the directory where all of the relavant COLMAP files are saved. -# If you are placing your data within the 'data' folder of this repository, -# this will be mapped to '/home_user/adapt_postprocessing/data' inside the -# Docker container. -data_dir = '/media/data' - -image_subdirs = ['1', '2'] - -# COLMAP data directory. -images_bin_fname = '%s/images.bin' % data_dir -camera_bin_fname = '%s/cameras.bin' % data_dir -points_bin_fname = '%s/points3D.bin' % data_dir -# ---------------------------------------------------------------------------- - - -# Read in the details of all images. -images = read_images_binary(images_bin_fname) -cameras = read_cameras_binary(camera_bin_fname) -points = read_points3d_binary(points_bin_fname) - - -# Pretend image index is the time. -platform_pose_provider = PlatformPoseInterp() -for image_id in images: - image = images[image_id] - - R = qvec2rotmat(image.qvec) - pos = -np.dot(R.T, image.tvec) - - # The qvec used by Colmap is a (w, x, y, z) quaternion representing the - # rotation of a vector defined in the world coordinate system into the - # camera coordinate system. However, the 'camera_models' module assumes - # (x, y, z, w) quaternions representing a coordinate system rotation. - quat = transformations.quaternion_inverse(image.qvec) - quat = [quat[1], quat[2], quat[3], quat[0]] - - t = image_id - platform_pose_provider.add_to_pose_time_series(t, pos, quat) - - -std_cams = {} -for camera_id in set([images[image_id].camera_id for image_id in images]): - colmap_camera = cameras[image.camera_id] - - if colmap_camera.model == 'OPENCV': - fx, fy, cx, cy, d1, d2, d3, d4 = colmap_camera.params - - K = K = np.array([[fx, 0, cx], [0, fy, cy], [0, 0, 1]]) - dist = np.array([d1, d2, d3, d4]) - std_cams[image.camera_id] = StandardCamera(colmap_camera.width, - colmap_camera.height, K, dist, - [0, 0, 0], [0, 0, 0, 1], - platform_pose_provider) - - -# Calculate reprojection error. -for image_id in images: - image = images[image_id] - colmap_camera = cameras[image.camera_id] - fname = '%s/%s.txt' % (data_dir, os.path.splitext(image.name)[0]) - R = qvec2rotmat(image.qvec) - - ind = image.point3D_ids >= 0 - - im_pts = image.xys[ind].T - wrld_pts = np.array([points[i].xyz for i in image.point3D_ids[ind]]).T - - if colmap_camera.model == 'OPENCV': - fx, fy, cx, cy, d1, d2, d3, d4 = colmap_camera.params - - K = np.array([[fx, 0, cx], [0, fy, cy], [0, 0, 1]]) - dist = np.array([d1, d2, d3, d4]) - tvec = image.tvec - rvec = cv2.Rodrigues(R)[0] - im_pts2 = np.squeeze(cv2.projectPoints(wrld_pts.T, rvec, tvec, K, dist)[0]).T - err = np.sqrt(np.sum(im_pts2 - im_pts, axis=0)) - - std_cams[image.camera_id].project(wrld_pts, t=image_id) - diff --git a/kamera/colmap_processing/vtk_util.py b/kamera/colmap_processing/vtk_util.py index 084c88d..5b1f49e 100644 --- a/kamera/colmap_processing/vtk_util.py +++ b/kamera/colmap_processing/vtk_util.py @@ -1,141 +1,165 @@ -#! /usr/bin/python -""" -ckwg +31 -Copyright 2018 by Kitware, Inc. -All rights reserved. - -Redistribution and use in source and binary forms, with or without -modification, are permitted provided that the following conditions are met: - - * Redistributions of source code must retain the above copyright notice, - this list of conditions and the following disclaimer. - - * Redistributions in binary form must reproduce the above copyright notice, - this list of conditions and the following disclaimer in the documentation - and/or other materials provided with the distribution. - - * Neither name of Kitware, Inc. nor the names of any contributors may be used - to endorse or promote products derived from this software without specific - prior written permission. - -THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS ``AS IS'' -AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHORS OR CONTRIBUTORS BE LIABLE FOR -ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - -============================================================================== - -""" from __future__ import division, print_function +import copy +import cv2 +import os +import vtk +import matplotlib.pyplot as plt import numpy as np -import copy, os, cv2, vtk from vtk.util import numpy_support -import matplotlib.pyplot as plt -# from matplotlib.patches import Polygon -# from matplotlib.collections import PatchCollection -# from mpl_toolkits.mplot3d import Axes3D +from scipy.spatial.transform import Rotation -from colmap_processing.camera_models import StandardCamera, \ - quaternion_from_matrix -from colmap_processing.platform_pose import PlatformPoseFixed +from kamera.colmap_processing.camera_models import StandardCamera +from kamera.colmap_processing.platform_pose import PlatformPoseFixed class Camera(object): - def __init__(self, res_x, res_y, vfov, pos, rmat): - """Create camera. + def __init__( + self, + camera: vtk.vtkCamera, + res_x: int, + res_y: int, + vfov: float, + pos: list[float], + rmat: np.ndarray, + ) -> None: + self.res_x = res_x + self.res_y = res_y + self.vfov = vfov + self.pos = pos + self.rmat = Rotation.from_matrix(rmat) + self.dist = None + self._kmat = None + self.camera = camera + + self.sync() - When pan and tilt are zero, the camera points north. + @property + def focal_length(self): + """Return the unitless focal length.""" + return 1 / (2 * np.tan(self.vfov / 180 * np.pi / 2)) * self.res_y - :param res_x: Horizontal resolution of the image. - :type res_x: int + @property + def kmat(self): + """Camera calibration matrix.""" + if self._kmat is not None: + return self._kmat + else: + return np.array( + [ + [self.focal_length, 0, self.res_x / 2], + [0, self.focal_length, self.res_y / 2], + [0, 0, 1], + ] + ) + + @kmat.setter + def kmat(self, val): + self._kmat = val + + def vtk_rotation_2_cm(self, R: np.ndarray) -> Rotation: + # a rotation consisting of the np.column_stack((right, true_up, forward)) + # vectors of a VTK camera + # VTK uses left-handed convention, so we have to flip the rotation matrix + # for it to be proper in scipy + R[:, 0] *= -1 + + # z flip it back using scipy + rot = Rotation.from_matrix(R) + z_flip = Rotation.from_euler("z", 180, degrees=True) + original = rot * z_flip + return original + + def cm_rotation_to_vtk(self, R: Rotation) -> np.ndarray: + # a rotation consisting of the np.column_stack((right, true_up, forward)) + # vectors of a StandardCamera model (right-handed orientation) + z_flip = Rotation.from_euler("z", 180, degrees=True) + original = R * z_flip + vtkR = original.as_matrix() + vtkR[:, 0] *= -1 + return vtkR + + def compute_camera_rotation( + self, position: np.ndarray, focal_point: np.ndarray, view_up: np.ndarray + ) -> Rotation: + """ + Compute the camera rotation matrix from VTK parameters. + + Parameters: + position : array-like + The camera position in world coordinates. + focal_point : array-like + The point in space the camera is looking at. + view_up : array-like + The view up vector (e.g., (0.3230, -0.4076, 0.8541)). + + Returns: + R : 3x3 numpy.ndarray + The rotation matrix from the camera coordinate system to the world coordinate system. + """ + position = np.array(position, dtype=np.float64) + focal_point = np.array(focal_point, dtype=np.float64) + view_up = np.array(view_up, dtype=np.float64) - :param res_y: Vertical resolution of the image. - :type res_y: int + # Compute the forward vector (camera's viewing direction) + forward = focal_point - position + forward = forward / np.linalg.norm(forward) - :param vfov: Vertical field of view (degrees). - :type vfov: float + # Compute the right vector (perpendicular to both forward and view up) + right = np.cross(forward, view_up) + right = right / np.linalg.norm(right) - :param pos: Position of the camera within the world. - :type pos: 3-array of float + # Compute the true up vector + true_up = np.cross(right, forward) - :param pan: Pan of the camera (degrees). When pan and tilt are zero, - the camera points along world y axis (i.e., north). - :type pan: float + # Construct the rotation matrix. + # with VTK -> CV convention, forward is positive + R = np.column_stack((right, true_up, forward)) - :param tilt: Tilt of the camera (degrees). When pan and tilt are zero, - the camera points along world y axis (i.e., north). - :type tilt: float + # VTK uses left-handed convention, so we have to flip the rotation matrix + # for it to be proper in scipy + if np.linalg.det(R) < 0: + R[:, 0] *= -1 - """ - self._res_x = res_x - self._res_y = res_y - self._vfov = vfov - self._pos = pos - self._rmat = rmat - self._model_reader = None + # flip it back using scipy + rot = Rotation.from_matrix(R) + q_flip = Rotation.from_euler("z", 180, degrees=True) + original = rot * q_flip - self._update_vtk_camera() - - @property - def focal_length(self): - """Return the unitless focal length. + return original + def get_yaw_pitch(self): """ - return 1/(2*np.tan(self.vfov/180*np.pi/2))*self.res_y - - @property - def res_x(self): - return self._res_x - - @res_x.setter - def res_x(self, val): - self._res_x = val - self._update_vtk_camera() - - @property - def res_y(self): - return self._res_y - - @res_y.setter - def res_y(self, val): - self._res_y = val + Computes the current yaw and pitch of a vtkCamera based on its position and focal point. - @property - def vfov(self): - return self._vfov + Assumes a coordinate system with Z as up. - @vfov.setter - def vfov(self, val): - self._vfov = val + Parameters: + camera: vtk.vtkCamera instance. - @property - def pos(self): - return self._pos + Returns: + yaw, pitch: Tuple of angles in degrees. + - yaw: Angle between the projection of the view direction onto the XY plane and the X-axis. + - pitch: Angle between the view direction and the XY plane. + """ + # Get camera position and focal point + pos = np.array(self.camera.GetPosition()) + focal = np.array(self.camera.GetFocalPoint()) - @pos.setter - def pos(self, val): - self._pos = val + # Compute the normalized view direction vector + view_dir = focal - pos + view_dir = view_dir / np.linalg.norm(view_dir) - @property - def rmat(self): - """Orientation rotation matrix. + # Yaw: angle in the XY plane. Use arctan2 to get the full [-180,180] range. + yaw = np.degrees(np.arctan2(view_dir[1], view_dir[0])) - """ - return self._rmat + # Pitch: angle between the view direction and the XY plane. + # When the camera is looking horizontally, view_dir[2] is zero (pitch = 0). + pitch = np.degrees(np.arcsin(view_dir[2])) - @property - def kmat(self): - """Camera calibration matrix. + return yaw, pitch - """ - return np.array([[self.focal_length, 0, self.res_x/2], - [0, self.focal_length, self.res_y/2], [0, 0, 1]]) + def set_focal_point(self, pos: list[float]): + self.camera.SetFocalPoint(pos) def ifov_image(self, downsample=1): """Return angle subtended by each pixel. @@ -146,10 +170,10 @@ def ifov_image(self, downsample=1): """ inv_kmat = np.linalg.inv(self.kmat) - res_x = self.res_x//downsample - res_y = self.res_y//downsample - X,Y = np.meshgrid(np.arange(res_x), np.arange(res_y)) - xy1 = np.ones((3,res_x*res_y)) + res_x = self.res_x // downsample + res_y = self.res_y // downsample + X, Y = np.meshgrid(np.arange(res_x), np.arange(res_y)) + xy1 = np.ones((3, res_x * res_y)) xy1[0] = X.ravel() xy1[1] = Y.ravel() @@ -164,44 +188,46 @@ def ifov_image(self, downsample=1): xy2 = np.dot(inv_kmat, xy2) xy3 = np.dot(inv_kmat, xy3) - #Normalize + # Normalize xy1 /= np.sqrt(np.sum(xy1**2, 0)) xy2 /= np.sqrt(np.sum(xy2**2, 0)) xy3 /= np.sqrt(np.sum(xy3**2, 0)) - ifov_x = np.arccos(np.maximum(np.minimum(np.sum(xy1*xy2, 0), 1), -1)) - ifov_y = np.arccos(np.maximum(np.minimum(np.sum(xy1*xy3, 0), 1), -1)) + ifov_x = np.arccos(np.maximum(np.minimum(np.sum(xy1 * xy2, 0), 1), -1)) + ifov_y = np.arccos(np.maximum(np.minimum(np.sum(xy1 * xy3, 0), 1), -1)) ifov = np.sqrt(ifov_x**2 + ifov_y**2) - ifov.shape = (self.res_y,self.res_x) + ifov.shape = (self.res_y, self.res_x) return ifov - def _update_vtk_camera(self): - camera = vtk.vtkCamera() - - # Set vertical field of view in degrees. - camera.SetViewAngle(self.vfov) - - # Define a level camera looking along the world y-axis (i.e., north). - R = self.rmat - focal_point = copy.deepcopy(self.pos) - focal_point += R[2] - camera.SetPosition(self.pos) - camera.SetFocalPoint(focal_point) - camera.SetViewUp(-R[1]) - - #camera.ParallelProjectionOn() - #camera.SetParallelScale(10) - camera.SetClippingRange(10,200) - - #camera.Pitch(-5) - #camera.OrthogonalizeViewUp() - - self.vtk_camera = camera - - def render_image(self, model_reader, clipping_range=[2,100], diffuse=0.6, - ambient=0.6, specular=0.1, light_color=[1.0, 1.0, 1.0], - light_pos=[0,0,1000]): + def sync(self): + self.pos = self.camera.GetPosition() + self.vfov = self.camera.GetViewAngle() + + view_angle = self.vfov # vertical FOV (degrees) + view_angle_rad = np.deg2rad(view_angle) + + # get focal lengths + fy = (self.res_y / 2.0) / np.tan(view_angle_rad / 2.0) + fx = fy * (self.res_x / self.res_y) + + # retain original cx, cy + cx = self.kmat[0, 2] + cy = self.kmat[1, 2] + + K = np.array([[fx, 0, cx], [0, fy, cy], [0, 0, 1]]) + self.kmat = K + + def render_image( + self, + model_reader, + clipping_range=[2, 100], + diffuse=0.6, + ambient=0.6, + specular=0.1, + light_color=[1.0, 1.0, 1.0], + light_pos=[0, 0, 1000], + ): """Render a view of the loaded model. :param model_reader: World mesh model reader. @@ -226,7 +252,7 @@ def render_image(self, model_reader, clipping_range=[2,100], diffuse=0.6, mapper.SetInputConnection(model_reader.GetOutputPort()) # Actor - actor =vtk.vtkActor() + actor = vtk.vtkActor() actor.SetMapper(mapper) # Set lighting properties @@ -236,9 +262,8 @@ def render_image(self, model_reader, clipping_range=[2,100], diffuse=0.6, # Renderer renderer = vtk.vtkRenderer() - self._update_vtk_camera() - self.vtk_camera.SetClippingRange(clipping_range[0],clipping_range[1]) - renderer.SetActiveCamera(self.vtk_camera) + self.camera.SetClippingRange(clipping_range[0], clipping_range[1]) + renderer.SetActiveCamera(self.camera) renderer.AddActor(actor) light = vtk.vtkLight() @@ -248,6 +273,7 @@ def render_image(self, model_reader, clipping_range=[2,100], diffuse=0.6, # RenderWindow renderWindow = vtk.vtkRenderWindow() + renderWindow.SetOffScreenRendering(1) renderWindow.AddRenderer(renderer) renderWindow.SetSize(self.res_x, self.res_y) @@ -257,8 +283,10 @@ def render_image(self, model_reader, clipping_range=[2,100], diffuse=0.6, rgbfilter.SetInputBufferTypeToRGB() rgbfilter.Update() dims = rgbfilter.GetOutput().GetDimensions() - npdims = [dims[1],dims[0],3] - image = numpy_support.vtk_to_numpy(rgbfilter.GetOutput().GetPointData().GetScalars()).reshape(npdims) + npdims = [dims[1], dims[0], 3] + image = numpy_support.vtk_to_numpy( + rgbfilter.GetOutput().GetPointData().GetScalars() + ).reshape(npdims) image = np.flipud(image) del renderer, renderWindow, rgbfilter @@ -268,27 +296,24 @@ def render_image(self, model_reader, clipping_range=[2,100], diffuse=0.6, def project(self, wrld_pt): if wrld_pt.ndim == 2: if wrld_pt.shape[0] == 3: - wrld_pt = np.vstack([wrld_pt,np.ones(wrld_pt.shape[1])]) + wrld_pt = np.vstack([wrld_pt, np.ones(wrld_pt.shape[1])]) else: - raise Exception('Not implemented') + raise Exception("Not implemented") im_pt = np.dot(self.get_camera_matrix(), wrld_pt) - return im_pt[:2]/im_pt[2] + return im_pt[:2] / im_pt[2] def unproject(self, im_pt): pass def get_camera_matrix(self): - """Return camera projection matrix that maps world to image. - - """ + """Return camera projection matrix that maps world to image.""" T = -np.dot(self.rmat, self.pos) - P = np.dot(self.kmat, np.hstack([self.rmat,np.atleast_2d(T).T])) + P = np.dot(self.kmat, np.hstack([self.rmat, np.atleast_2d(T).T])) return P - def unproject_view(self, model_reader, clipping_range=[2,100], - return_image=False): + def unproject_view(self, model_reader, clipping_range=[2, 100], return_image=False): """Return the coordinates of intersection of each pixel with the world. :param model_reader: World mesh model output. @@ -300,19 +325,19 @@ def unproject_view(self, model_reader, clipping_range=[2,100], mapper.SetInputConnection(model_reader.GetOutputPort()) # Actor - actor =vtk.vtkActor() + actor = vtk.vtkActor() actor.SetMapper(mapper) # Renderer renderer = vtk.vtkRenderer() - self._update_vtk_camera() - self.vtk_camera.SetClippingRange(clipping_range[0],clipping_range[1]) - renderer.SetActiveCamera(self.vtk_camera) + self.camera.SetClippingRange(clipping_range[0], clipping_range[1]) + renderer.SetActiveCamera(self.camera) renderer.AddActor(actor) # RenderWindow renderWindow = vtk.vtkRenderWindow() renderWindow.AddRenderer(renderer) + renderWindow.SetOffScreenRendering(1) renderWindow.SetSize(self.res_x, self.res_y) # Read Z Buffer @@ -323,8 +348,10 @@ def unproject_view(self, model_reader, clipping_range=[2,100], # transform zbuffer to numpy array dims = zfilter.GetOutput().GetDimensions() - npdims = [dims[1],dims[0]] - array = numpy_support.vtk_to_numpy(zfilter.GetOutput().GetPointData().GetScalars()).reshape(npdims) + npdims = [dims[1], dims[0]] + array = numpy_support.vtk_to_numpy( + zfilter.GetOutput().GetPointData().GetScalars() + ).reshape(npdims) array = np.flipud(array) if return_image: @@ -337,22 +364,20 @@ def unproject_view(self, model_reader, clipping_range=[2,100], del renderer, renderWindow, zfilter # Convert ZBuffer to range. - near, far = self.vtk_camera.GetClippingRange() - b = near*far/(near - far) - a = -b/near - depth = b/(array - a) - - X, Y = np.meshgrid(np.arange(depth.shape[1]), - np.arange(depth.shape[0])) - xy = np.vstack([X.ravel(), Y.ravel(), - np.ones(depth.shape[0]*depth.shape[1])]) - inv = np.linalg.inv(np.dot(self.kmat, self.rmat)) + near, far = self.camera.GetClippingRange() + b = near * far / (near - far) + a = -b / near + depth = b / (array - a) + + X, Y = np.meshgrid(np.arange(depth.shape[1]), np.arange(depth.shape[0])) + xy = np.vstack([X.ravel(), Y.ravel(), np.ones(depth.shape[0] * depth.shape[1])]) + inv = np.linalg.inv(np.dot(self.kmat, self.rmat.as_matrix())) ray_dir = np.dot(inv, xy) # Z-buffer is the distance the ray travels projected onto the optical # axis. The rays already have unit length when projected along the # optical axis. - xyz = ray_dir*depth.ravel() + np.atleast_2d(self.pos).T + xyz = ray_dir * depth.ravel() + np.atleast_2d(self.pos).T X = np.reshape(xyz[0], (self.res_y, self.res_x)) Y = np.reshape(xyz[1], (self.res_y, self.res_x)) @@ -362,7 +387,9 @@ def unproject_view(self, model_reader, clipping_range=[2,100], # Render image dims = rgbfilter.GetOutput().GetDimensions() npdims = [dims[1], dims[0], 3] - image = numpy_support.vtk_to_numpy(rgbfilter.GetOutput().GetPointData().GetScalars()).reshape(npdims) + image = numpy_support.vtk_to_numpy( + rgbfilter.GetOutput().GetPointData().GetScalars() + ).reshape(npdims) del rgbfilter image = np.flipud(image) return X, Y, Z, depth, image @@ -370,15 +397,32 @@ def unproject_view(self, model_reader, clipping_range=[2,100], return X, Y, Z, depth def to_standard_camera(self): - quat = quaternion_from_matrix(self.rmat.T) - platform_pose_provider = PlatformPoseFixed(self.pos, quat) - cm = StandardCamera(self.res_x, self._res_y, self.kmat, np.zeros(5), - [0, 0, 0], [0, 0, 0, 1], platform_pose_provider) + self.sync() + # Get the current position, focal point, and view up vector + # of the camera to compute the corresponding CV rotation matrix + pos = np.array(self.camera.GetPosition()) + focal = np.array(self.camera.GetFocalPoint()) + view = np.array(self.camera.GetViewUp()) + rot = self.compute_camera_rotation(pos, focal, view) + quat = rot.as_quat() + # make the external pose the same as the camera's pose + platform_pose_provider = PlatformPoseFixed( + np.array([0, 0, 0]), np.array([0, 0, 0, 1]) + ) + cm = StandardCamera( + self.res_x, + self.res_y, + self.kmat, + self.dist, + pos, + quat, + platform_pose_provider, + ) return cm class CameraPanTilt(Camera): - def __init__(self, res_x, res_y, vfov, pos, pan, tilt): + def __init__(self, camera, res_x, res_y, vfov, pos, pan, tilt): """Create camera. When pan and tilt are zero, the camera points north. @@ -406,137 +450,144 @@ def __init__(self, res_x, res_y, vfov, pos, pan, tilt): """ self._pan = pan self._tilt = tilt - self.set_rmat_from_pan_tilt() - super(CameraPanTilt, self).__init__(res_x, res_y, vfov, pos, - self._rmat) + self.set_rmat_from_pan_tilt(pan, tilt) + super(CameraPanTilt, self).__init__(camera, res_x, res_y, vfov, pos, self._rmat) @property def pan(self): - """Pan in degrees. - - """ + """Pan in degrees.""" return self._pan @pan.setter def pan(self, val): self._pan = val - self.set_rmat_from_pan_tilt() + self.set_rmat_from_pan_tilt(self._pan, self.tilt) @property def tilt(self): - """Tilt in degrees. - - """ + """Tilt in degrees.""" return self._tilt @tilt.setter def tilt(self, val): self._tilt = val - self.set_rmat_from_pan_tilt() - - def set_rmat_from_pan_tilt(self): - """Orientation rotation matrix. - - """ - tilt = self.tilt - pan = self.pan + self.set_rmat_from_pan_tilt(self.pan, self.tilt) - theta = tilt/180*np.pi + def set_rmat_from_pan_tilt(self, pan: float, tilt: float) -> Rotation: + """Return the orientation matrix given pan and tilt in degrees.""" + theta = np.deg2rad(tilt) c = np.cos(theta) s = np.sin(theta) - rx = np.array([[1,0,0],[0,c,-s],[0,s,c]]) + rx = np.array([[1, 0, 0], [0, c, -s], [0, s, c]]) - theta = pan/180*np.pi + theta = np.rad2deg(pan) c = np.cos(-theta) s = np.sin(-theta) - rz = np.array([[c,-s,0],[s,c,0],[0,0,1]]) + rz = np.array([[c, -s, 0], [s, c, 0], [0, 0, 1]]) - r1 = np.dot(rx, np.array([[1,0,0],[0,0,-1],[0,1,0]]).T).T + r1 = np.dot(rx, np.array([[1, 0, 0], [0, 0, -1], [0, 1, 0]]).T).T r = np.dot(rz, r1.T).T - self._rmat = r - - -def render_distored_image(width, height, K, dist, cam_pos, R, model_reader, - return_depth=True, monitor_resolution=(1000, 1000), - clipping_range=[1, 2000], fill_holes=True): - """Render a view from a camera with distortion. - - """ + self.rmat = Rotation.from_matrix(r) + + +def render_distorted_image( + width, + height, + K, + dist, + cam_pos, + R, + model_reader, + return_depth=True, + monitor_resolution=(1000, 1000), + clipping_range=[1, 2000], + fill_holes=True, +): + """Render a view from a camera with distortion.""" render_resolution = list(monitor_resolution) if monitor_resolution[0] != monitor_resolution[1]: - raise Exception('There is a bug when the monitor resolution isn\'t ' - 'square. Your actual monitor doesn\'t need to be ' - 'square, just pick the largest square resolution that ' - 'fits inside your monitor.') + raise Exception( + "There is a bug when the monitor resolution isn't " + "square. Your actual monitor doesn't need to be " + "square, just pick the largest square resolution that " + "fits inside your monitor." + ) # Generate points along the border of the distorted camera. num_points = 1000 - perimeter = 2*(height + width) - ds = num_points/float(perimeter) - xn = np.max([2, int(ds*width)]) - yn = np.max([2, int(ds*height)]) + perimeter = 2 * (height + width) + ds = num_points / float(perimeter) + xn = np.max([2, int(ds * width)]) + yn = np.max([2, int(ds * height)]) x = np.linspace(0, width, xn) y = np.linspace(0, height, yn)[1:-1] - pts = np.vstack([np.hstack([x, np.full(len(y), width, dtype=np.float64), - x[::-1], np.zeros(len(y))]), - np.hstack([np.zeros(xn), y, - np.full(xn, height, dtype=np.float64), - y[::-1]])]).T + pts = np.vstack( + [ + np.hstack( + [x, np.full(len(y), width, dtype=np.float64), x[::-1], np.zeros(len(y))] + ), + np.hstack( + [np.zeros(xn), y, np.full(xn, height, dtype=np.float64), y[::-1]] + ), + ] + ).T # Unproject these rays. - ray_dir = np.ones((len(pts), 3), dtype=np.float) + ray_dir = np.ones((len(pts), 3), dtype=np.float64) ray_dir0 = cv2.undistortPoints(np.expand_dims(pts, 0), K, dist, R=None) ray_dir[:, :2] = np.squeeze(ray_dir0) K_ = np.identity(3) - points2 = cv2.projectPoints(ray_dir, np.zeros(3, dtype=np.float32), - np.zeros(3, dtype=np.float32), K_, None)[0] + points2 = cv2.projectPoints( + ray_dir, np.zeros(3, dtype=np.float32), np.zeros(3, dtype=np.float32), K_, None + )[0] points2 = np.squeeze(points2, 1).T # points2 are now in the distortion-free camera. if False: plt.plot(points2[0], points2[1]) - plt.plot([0, render_resolution[0], render_resolution[0], 0, 0], - [0, 0, render_resolution[1], render_resolution[1], 0]) + plt.plot( + [0, render_resolution[0], render_resolution[0], 0, 0], + [0, 0, render_resolution[1], render_resolution[1], 0], + ) r1 = np.abs(points2[0]).max() r2 = np.abs(points2[1]).max() - s1 = render_resolution[0]/2/r1 - s2 = render_resolution[1]/2/r2 - K_[0, 0] = K_[1, 1] = min([s1, s2])*0.98 + s1 = render_resolution[0] / 2 / r1 + s2 = render_resolution[1] / 2 / r2 + K_[0, 0] = K_[1, 1] = min([s1, s2]) * 0.98 if s1 > s2: - render_resolution[0] = int(np.ceil(render_resolution[1]*r1/r2)) + render_resolution[0] = int(np.ceil(render_resolution[1] * r1 / r2)) else: - render_resolution[1] = int(np.ceil(render_resolution[0]*r2/r1)) - - K_[0, 2] = render_resolution[0]/2 - K_[1, 2] = render_resolution[1]/2 - - vfov = np.arctan(render_resolution[1]/2/K_[1, 1])*2*180/np.pi - vtk_camera = Camera(render_resolution[0], render_resolution[1], vfov, - cam_pos, R) - - img = vtk_camera.render_image(model_reader, clipping_range=clipping_range, - diffuse=0.6, ambient=0.6, specular=0.1, - light_color=[1.0, 1.0, 1.0], - light_pos=[0, 0, 1000]) - - #img = cv2.resize(img, tuple(render_resolution)) + render_resolution[1] = int(np.ceil(render_resolution[0] * r2 / r1)) + + K_[0, 2] = render_resolution[0] / 2 + K_[1, 2] = render_resolution[1] / 2 + + vfov = np.arctan(render_resolution[1] / 2 / K_[1, 1]) * 2 * 180 / np.pi + camera = vtk.vtkCamera() + vtk_camera = Camera( + camera, render_resolution[0], render_resolution[1], vfov, cam_pos, R + ) + + img = vtk_camera.render_image( + model_reader, + clipping_range=clipping_range, + diffuse=0.6, + ambient=0.6, + specular=0.1, + light_color=[1.0, 1.0, 1.0], + light_pos=[0, 0, 1000], + ) if return_depth or fill_holes: - ret = vtk_camera.unproject_view(model_reader, - clipping_range=clipping_range) + ret = vtk_camera.unproject_view(model_reader, clipping_range=clipping_range) E, N, U, depth = ret - #depth = cv2.resize(depth, tuple(render_resolution)) - - #plt.figure(); plt.imshow(img) - - #plt.figure(); plt.imshow(real_image) # Warp the rendered view back to the original, possibly distorted, camera # view. @@ -544,16 +595,21 @@ def render_distored_image(width, height, K, dist, cam_pos, R, model_reader, # These are the pixel coordinates for the centers of all the pixels in the # image of size img.shape, which we will scale up to the pixel coordinates # for those same locations in the image of size (height, width). - x = (np.arange(img.shape[1]) + 0.5)*width/img.shape[1] - y = (np.arange(img.shape[0]) + 0.5)*height/img.shape[0] + x = (np.arange(img.shape[1]) + 0.5) * width / img.shape[1] + y = (np.arange(img.shape[0]) + 0.5) * height / img.shape[0] X, Y = np.meshgrid(x, y) points = np.vstack([X.ravel(), Y.ravel()]) ray_dir = cv2.undistortPoints(np.expand_dims(points.T, 0), K, dist, None) ray_dir = np.squeeze(ray_dir).astype(np.float32).T ray_dir = np.vstack([ray_dir, np.ones(ray_dir.shape[1])]) - points2 = cv2.projectPoints(ray_dir.T, np.zeros(3, dtype=np.float32), - np.zeros(3, dtype=np.float32), K_, None)[0] + points2 = cv2.projectPoints( + ray_dir.T, + np.zeros(3, dtype=np.float32), + np.zeros(3, dtype=np.float32), + K_, + None, + )[0] points2 = np.squeeze(points2, 1).T X2 = np.reshape(points2[0], X.shape).astype(np.float32) Y2 = np.reshape(points2[1], Y.shape).astype(np.float32) @@ -575,9 +631,9 @@ def render_distored_image(width, height, K, dist, cam_pos, R, model_reader, # Holes that extend to the edge of the image won't be filled via # inpainting. - - output = cv2.connectedComponentsWithStats(hole_mask.astype(np.uint8), - 8, cv2.CV_32S) + output = cv2.connectedComponentsWithStats( + hole_mask.astype(np.uint8), 8, cv2.CV_32S + ) labels = output[1] # Remove components that touch outer boundary. @@ -596,8 +652,7 @@ def render_distored_image(width, height, K, dist, cam_pos, R, model_reader, X = cv2.inpaint(X.astype(np.float32), mask, 3, cv2.INPAINT_NS) Y = cv2.inpaint(Y.astype(np.float32), mask, 3, cv2.INPAINT_NS) Z = cv2.inpaint(Z.astype(np.float32), mask, 3, cv2.INPAINT_NS) - depth = cv2.inpaint(depth.astype(np.float32), mask, 3, - cv2.INPAINT_NS) + depth = cv2.inpaint(depth.astype(np.float32), mask, 3, cv2.INPAINT_NS) if return_depth: return img, depth, X, Y, Z @@ -610,18 +665,17 @@ def load_world_model(fname): """ :param fname: Path to .stl or .ply file. :type fname: str - """ ext = os.path.splitext(fname)[-1] - if ext == '.stl': + if ext == ".stl": model_reader = vtk.vtkSTLReader() - elif ext == '.ply': + elif ext == ".ply": model_reader = vtk.vtkPLYReader() - elif ext == '.obj': + elif ext == ".obj": model_reader = vtk.vtkOBJReader() else: - raise Exception('Unhandled model extension: \'%s\'' % ext) + raise Exception("Unhandled model extension: '%s'" % ext) model_reader.SetFileName(fname) model_reader.Update() @@ -637,15 +691,15 @@ def get_azel_from_ray_dir(ray_dir, cam_pos): ray_dir /= np.atleast_2d(np.sqrt(np.sum(ray_dir**2, 1))).T azel = np.zeros((2, ray_dir.shape[1])) azel[0] = np.arctan2(ray_dir[0], ray_dir[1]) - azel[1] = np.arctan(ray_dir[2]/np.sqrt(ray_dir[0]**2 + ray_dir[1]**2)) + azel[1] = np.arctan(ray_dir[2] / np.sqrt(ray_dir[0] ** 2 + ray_dir[1] ** 2)) return azel def get_ray_dir_from_azel(azel): azel = np.atleast_2d(azel) - azel.shape = (2,-1) + azel.shape = (2, -1) ray_dir = np.zeros((3, azel.shape[1])) - ray_dir[0] = np.sin(azel[0])*np.cos(azel[1]) - ray_dir[1] = np.cos(azel[0])*np.cos(azel[1]) + ray_dir[0] = np.sin(azel[0]) * np.cos(azel[1]) + ray_dir[1] = np.cos(azel[0]) * np.cos(azel[1]) ray_dir[2] = np.sin(azel[1]) return ray_dir diff --git a/kamera/colmap_processing/world_models.py b/kamera/colmap_processing/world_models.py index ff85cfa..46353f4 100644 --- a/kamera/colmap_processing/world_models.py +++ b/kamera/colmap_processing/world_models.py @@ -1,37 +1,4 @@ #! /usr/bin/python -""" -ckwg +31 -Copyright 2020 by Kitware, Inc. -All rights reserved. - -Redistribution and use in source and binary forms, with or without -modification, are permitted provided that the following conditions are met: - - * Redistributions of source code must retain the above copyright notice, - this list of conditions and the following disclaimer. - - * Redistributions in binary form must reproduce the above copyright notice, - this list of conditions and the following disclaimer in the documentation - and/or other materials provided with the distribution. - - * Neither name of Kitware, Inc. nor the names of any contributors may be used - to endorse or promote products derived from this software without specific - prior written permission. - -THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS ``AS IS'' -AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHORS OR CONTRIBUTORS BE LIABLE FOR -ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - -============================================================================== - -""" from __future__ import division, print_function import numpy as np import csv diff --git a/kamera/postflight/alignment.py b/kamera/postflight/alignment.py new file mode 100644 index 0000000..16cc96c --- /dev/null +++ b/kamera/postflight/alignment.py @@ -0,0 +1,995 @@ +import random +import cv2 +import copy +import numpy as np +from dataclasses import dataclass +from typing import List, NamedTuple, Optional, Tuple +from scipy.optimize import minimize, fminbound +from scipy.spatial.transform import Rotation +import pycolmap +from kamera.colmap_processing.camera_models import StandardCamera +from kamera.sensor_models.nav_state import NavStateINSJson, NavStateFixed +from rich import print + + +@dataclass +class VisiblePoint: + """Point visible in a camera view.""" + + point_3d: np.ndarray # 3D point coordinates (3,) + point_3d_id: int # Unique ID given by Colmap of the 3D point + point_2d: np.ndarray # 2D observation in image (2,) + uncertainty: float # Point uncertainty/error + time: float + visible: bool = False # Whether point is visible in this camera + + +@dataclass +class RotationEstimate: + """Result of rotation estimation.""" + + quaternion: np.ndarray # Estimated rotation quaternion (x,y,z,w) + covariance: np.ndarray # 3x3 covariance matrix in angle-axis space + fisher_information: np.ndarray # Fisher information matrix + num_inliers: int # Number of inlier observations used + mean_error: float # Mean error of the estimate + + +def compute_jacobian(points: np.ndarray, R: np.ndarray) -> np.ndarray: + """ + Compute Jacobian of rotation with respect to angle-axis parameters. + + Args: + points: Nx3 array of 3D points + R: 3x3 rotation matrix + Returns: + 3Nx3 Jacobian matrix + """ + jac = np.zeros((3 * len(points), 3)) + for i, p in enumerate(points): + # Skew-symmetric matrix for cross product + skew = np.array([[0, -p[2], p[1]], [p[2], 0, -p[0]], [-p[1], p[0], 0]]) + jac[3 * i : 3 * (i + 1)] = (-R @ skew)[0] # First row + jac[3 * i + 1 : 3 * (i + 2)] = (-R @ skew)[1] # Second row + jac[3 * i + 2 : 3 * (i + 3)] = (-R @ skew)[2] # Third row + return jac + + +def compute_fisher_information( + points: np.ndarray, R: np.ndarray, weights: np.ndarray +) -> np.ndarray: + """Compute Fisher Information Matrix without forming full weight matrix.""" + J = compute_jacobian(points, R) + fisher = np.zeros((3, 3)) + for i in range(len(weights)): + block = J[3 * i : 3 * (i + 3)] + fisher += weights[i] * (block.T @ block) + return fisher + + +def compute_covariance( + R: np.ndarray, observations: List[List[VisiblePoint]] +) -> np.ndarray: + """ + Compute covariance matrix for rotation estimate. + + Args: + R: 3x3 rotation matrix + observations: List of visible points per camera + Returns: + 3x3 covariance matrix in angle-axis space + """ + points = [] + weights = [] + + for camera_obs in observations: + for obs in camera_obs: + points.append(obs.point_3d) + weights.append(1.0 / (obs.uncertainty**2 + 1e-10)) + + points = np.array(points) + weights = np.array(weights) + + fisher = compute_fisher_information(points, R, weights) + return np.linalg.inv(fisher) + + +def weighted_horn_alignment_partial( + observations: np.ndarray[List[VisiblePoint]], + sfm_quats: np.ndarray, + ins_quats: np.ndarray, + min_points_per_image: int = 3, + ransac_iters: int = 100, + error_threshold: float = 0.1, + max_3d_points: int = 5000, +) -> RotationEstimate: + """ + Find rotation between INS and camera frames with partial point visibility. + + Args: + observations: List of visible points for each camera + sfm_quats: Nx4 array of SfM-derived camera quaternions + ins_quats: Nx4 array of INS-measured quaternions + min_points_per_image: Minimum points needed per camera + ransac_iters: Number of RANSAC iterations + error_threshold: Error threshold for inlier classification + + Returns: + RotationEstimate containing quaternion and uncertainty information + """ + + print(f"Number of observations: {len(observations)}") + print(f"Number of sfm quats: {len(sfm_quats)}, {len(ins_quats)}") + + def compute_rotation_for_subset( + image_indices: List[int], max_3d_points: int = 1000 + ) -> Tuple[Optional[np.ndarray], Optional[float], Optional[List[int]]]: + """Compute rotation for a subset of images.""" + points_ins = [] + points_cam = [] + weights = [] + inlier_indices = [] + + for idx, image_idx in enumerate(image_indices): + # Get visible points for this camera + visible_obs = [obs for obs in observations[image_idx] if obs.visible] + + if len(visible_obs) < min_points_per_image: + print("Not enough visible observations, skipping.") + return None, None, None + + # errors = [obs.uncertainty for obs in visible_obs] + # keep the smallest error points + # keep_idx = np.argsort(errors)[:max_3d_points] + # visible_obs = np.array(visible_obs)[keep_idx] + + R_ins = Rotation.from_quat(ins_quats[image_idx]) + R_sfm = Rotation.from_quat(sfm_quats[image_idx]) + + for obs in visible_obs: + # Transform point through both rotations + p_ins = R_ins.apply(obs.point_3d) + p_cam = R_sfm.apply(obs.point_3d) + + points_ins.append(p_ins) + points_cam.append(p_cam) + weights.append(1.0 / (obs.uncertainty**2 + 1e-10)) + inlier_indices.append(idx) + + if len(points_ins) < 5: # Need minimum points for reliable estimate + print( + f"Number of points is {points_ins}, which is less than the min of 5 needed." + ) + return None, None, None + + points_ins = np.array(points_ins) + points_cam = np.array(points_cam) + weights = np.array(weights) + + # Weighted Horn method + centroid_ins = np.sum(weights[:, None] * points_ins, axis=0) / np.sum(weights) + centroid_cam = np.sum(weights[:, None] * points_cam, axis=0) / np.sum(weights) + + centered_ins = points_ins - centroid_ins + centered_cam = points_cam - centroid_cam + + H = (centered_ins.T * weights) @ centered_cam + U, _, Vt = np.linalg.svd(H) + R = Vt.T @ U.T + + if np.linalg.det(R) < 0: + Vt[-1, :] *= -1 + R = Vt.T @ U.T + + errors = np.linalg.norm(R @ centered_ins.T - centered_cam.T, axis=0) + + # Identify inliers + inliers = errors < error_threshold + if np.sum(inliers) < min_points_per_image: + print( + f"Num inliers is {np.sum(inliers)}, which is less than the min required of {min_points_per_image}." + ) + print(f"Mean error is {np.mean(errors)}, threshold is {error_threshold}.") + return None, None, None + + mean_error = np.mean(errors[inliers]) + return R, mean_error, [inlier_indices[i] for i in np.where(inliers)[0]] + + best_R = None + best_error = float("inf") + best_inliers = [] + n_images = len(observations) + + for _ in range(ransac_iters): + # Sample subset of images + n_sample = min(5, n_images) + image_indices = np.random.choice(n_images, n_sample, replace=False) + + R, error, inliers = compute_rotation_for_subset( + image_indices, max_3d_points=max_3d_points + ) + if R is not None and error < best_error: + # Verify with all images + R_full, error_full, inliers_full = compute_rotation_for_subset( + inliers, + ) + if R_full is not None and error_full < best_error: + best_R = R_full + best_error = error_full + best_inliers = inliers_full + + if best_R is None: + raise RuntimeError("Could not find valid rotation - check visibility") + + print(best_R) + + # Compute uncertainty + # covariance = compute_covariance(best_R, observations) + # fisher = compute_fisher_information( + # np.array( + # [ + # obs.point_3d + # for i in best_inliers + # for obs in observations[i] + # if obs.visible + # ] + # ), + # best_R, + # np.array( + # [ + # 1.0 / (obs.uncertainty**2 + 1e-10) + # for i in best_inliers + # for obs in observations[i] + # if obs.visible + # ] + # ), + # ) + covariance = np.eye(3) + fisher = np.eye(3) + + return RotationEstimate( + quaternion=Rotation.from_matrix(best_R).as_quat(), + covariance=covariance, + fisher_information=fisher, + num_inliers=len(best_inliers), + mean_error=best_error, + ) + + +def analyze_rotation_estimate(estimate: RotationEstimate) -> None: + """ + Analyze and print information about rotation estimate quality. + + Args: + estimate: RotationEstimate from alignment + """ + # Extract principal uncertainties + eigenvals, eigenvecs = np.linalg.eigh(estimate.covariance) + std_devs = np.sqrt(eigenvals) + + print("\nRotation Alignment Analysis") + print("==========================") + print(f"Quaternion (x,y,z,w): {estimate.quaternion}") + print(f"\nNumber of inliers: {estimate.num_inliers}") + print(f"Mean error: {estimate.mean_error:.6f}") + + print("\nUncertainty Analysis:") + print("Standard deviations (degrees):") + for i, std in enumerate(std_devs): + print(f" Axis {i+1}: {np.degrees(std):.4f}°") + + # Condition number of Fisher Information + cond = np.linalg.cond(estimate.fisher_information) + print(f"\nFisher Information condition number: {cond:.2e}") + + # 99% confidence intervals + conf_intervals = 2.576 * std_devs + print("\n99% Confidence intervals (degrees):") + for i, interval in enumerate(conf_intervals): + print(f" Axis {i+1}: ±{np.degrees(interval):.4f}°") + + +class RANSACParams: + def __init__( + self, + min_samples: int = 3, + max_iterations: int = 1000, + inlier_threshold: float = 0.1, # radians + min_inliers_ratio: float = 0.5, + ): + self.min_samples = min_samples + self.max_iterations = max_iterations + self.inlier_threshold = inlier_threshold + self.min_inliers_ratio = min_inliers_ratio + + +def compute_rotation_horn( + sfm_rotations: np.ndarray, ins_rotations: np.ndarray +) -> np.ndarray: + """ + Compute optimal rotation matrix using Horn's method for a subset of rotations. + + Args: + sfm_rotations: Array of SfM rotation matrices (N, 3, 3) + ins_rotations: Array of INS rotation matrices (N, 3, 3) + + Returns: + optimal_rotation: 3x3 rotation matrix + """ + # Build correlation matrix + M = sum(ins_R @ sfm_R.T for sfm_R, ins_R in zip(sfm_rotations, ins_rotations)) + + # Perform SVD + U, _, Vt = np.linalg.svd(M) + + # Ensure proper rotation (det = 1) + det = np.linalg.det(U @ Vt) + S = np.eye(3) + if det < 0: + S[2, 2] = -1 + + return U @ S @ Vt + + +def compute_alignment_errors( + sfm_rotations: np.ndarray, ins_rotations: np.ndarray, optimal_rotation: np.ndarray +) -> np.ndarray: + """ + Compute angular errors between aligned rotations. + + Args: + sfm_rotations: Array of SfM rotation matrices + ins_rotations: Array of INS rotation matrices + optimal_rotation: Computed optimal rotation matrix + + Returns: + errors: Array of angular errors in radians + """ + errors = [] + for sfm_R, ins_R in zip(sfm_rotations, ins_rotations): + aligned_R = optimal_rotation @ sfm_R + relative_R = aligned_R.T @ ins_R + angle_error = abs(Rotation.from_matrix(relative_R).magnitude()) + errors.append(angle_error) + return np.array(errors) + + +def register_camera_horn_ransac( + sfm_poses: List[np.ndarray], + ins_poses: List[np.ndarray], + points_per_image: Optional[List] = None, + colmap_camera: Optional[object] = None, + ransac_params: Optional[RANSACParams] = None, +) -> Tuple[np.ndarray, float, np.ndarray]: + """ + Register camera poses using Horn's method with RANSAC for robust estimation. + + Args: + sfm_poses: List of quaternions from Structure from Motion + ins_poses: List of quaternions from INS measurements + points_per_image: Optional list of 2D-3D point correspondences + colmap_camera: Optional COLMAP camera object + ransac_params: Optional RANSAC parameters + + Returns: + optimal_rotation: 3x3 rotation matrix + best_rmse: Root mean square error of the alignment + inlier_mask: Boolean array indicating inlier poses + """ + if ransac_params is None: + ransac_params = RANSACParams() + + # Convert quaternions to rotation matrices + sfm_rotations = np.array([Rotation.from_quat(q).as_matrix() for q in sfm_poses]) + ins_rotations = np.array([Rotation.from_quat(q).as_matrix() for q in ins_poses]) + + num_poses = len(sfm_poses) + best_rotation = None + best_rmse = float("inf") + best_inlier_mask = None + + # RANSAC iterations + for _ in range(ransac_params.max_iterations): + # Randomly sample pose pairs + sample_indices = random.sample(range(num_poses), ransac_params.min_samples) + sample_sfm = sfm_rotations[sample_indices] + sample_ins = ins_rotations[sample_indices] + + # Compute candidate rotation using sampled pairs + candidate_rotation = compute_rotation_horn(sample_sfm, sample_ins) + + # Compute errors for all poses using this rotation + errors = compute_alignment_errors( + sfm_rotations, ins_rotations, candidate_rotation + ) + + # Identify inliers + print(np.mean(errors)) + inlier_mask = errors < ransac_params.inlier_threshold + num_inliers = np.sum(inlier_mask) + + # Check if we have enough inliers + if num_inliers / num_poses >= ransac_params.min_inliers_ratio: + # Recompute rotation using all inliers + inlier_sfm = sfm_rotations[inlier_mask] + inlier_ins = ins_rotations[inlier_mask] + refined_rotation = compute_rotation_horn(inlier_sfm, inlier_ins) + + # Compute RMSE for refined rotation + refined_errors = compute_alignment_errors( + sfm_rotations, ins_rotations, refined_rotation + ) + rmse = np.sqrt(np.mean(refined_errors[inlier_mask] ** 2)) + + # Update best solution if this is better + if rmse < best_rmse: + best_rotation = refined_rotation + best_rmse = rmse + best_inlier_mask = inlier_mask + + if best_rotation is None: + raise RuntimeError( + "RANSAC failed to find a good alignment. Consider adjusting parameters." + ) + + # Verify alignment using 3D points if available + if points_per_image and colmap_camera: + verify_alignment(points_per_image, best_rotation, colmap_camera) + + # Print some statistics about the solution + inlier_percentage = np.sum(best_inlier_mask) / len(best_inlier_mask) * 100 + print("RANSAC statistics:") + print(f"- Inlier percentage: {inlier_percentage:.1f}%") + print(f"- Final RMSE: {best_rmse:.3f} radians") + + return best_rotation, best_rmse, best_inlier_mask + + +def verify_alignment( + points_per_image: List, optimal_rotation: np.ndarray, colmap_camera: object +) -> None: + """ + Verify alignment quality using 3D point reprojection. + """ + reprojection_errors = [] + + for points in points_per_image: + for pt in points: + point_2d = pt.image_point + point_3d = pt.point_3d + # Transform 3D point using optimal rotation + transformed_point = optimal_rotation @ point_3d + + # Project 3D point using camera intrinsics + fx, fy, cx, cy = colmap_camera.params[:4] # Assuming standard pinhole model + + projected_x = fx * transformed_point[0] / transformed_point[2] + cx + projected_y = fy * transformed_point[1] / transformed_point[2] + cy + + error = np.sqrt( + (projected_x - point_2d[0]) ** 2 + (projected_y - point_2d[1]) ** 2 + ) + reprojection_errors.append(error) + + mean_reprojection_error = np.mean(reprojection_errors) + print(f"Mean reprojection error: {mean_reprojection_error:.2f} pixels") + + +def iterative_alignment( + sfm_quats: List[np.ndarray], + ins_quats: List[np.ndarray], + points_per_image: List[List[VisiblePoint]], + colmap_camera: pycolmap.Camera, + nav_state_provider: NavStateINSJson, +) -> tuple[StandardCamera, float]: + # Both quaternions are of the form (x, y, z, w) and represent a coordinate + # system rotation. + cam_quats = [ + (Rotation.from_quat(sfm_quats[k]) * Rotation.from_quat(ins_quats[k])) + .inv() + .as_quat() + for k in range(len(ins_quats)) + ] + + K = colmap_camera.calibration_matrix() + if colmap_camera.model.name == "OPENCV": + fx, fy, cx, cy, d1, d2, d3, d4 = colmap_camera.params + elif colmap_camera.model.name == "SIMPLE_RADIAL": + d1 = d2 = d3 = d4 = 0 + elif colmap_camera.model.name == "PINHOLE": + d1 = d2 = d3 = d4 = 0 + else: + raise SystemError(f"Unexpected camera model found: {colmap_camera.model.name}") + dist = np.array([d1, d2, d3, d4]) + + organized_points_per_frame = [] + max_uncertainty = np.inf + errs = [] + for pts in points_per_image: + xy = [] + xyz = [] + for pt in pts: + errs.append(pt.uncertainty) + if pt.uncertainty < max_uncertainty: + xy.append(pt.point_2d) + xyz.append(pt.point_3d) + + # Eliminate points 10x the distance away from median + max_uncertainty = np.median(errs) * 10 + # time is the same for all points, since it's a single frame + xy = np.array(xy) + xyz = np.array(xyz) + organized_points_per_frame.append((xy, xyz, pts[0].time)) + + def cam_quat_error(cam_quat: np.ndarray) -> float: + cam_quat = cam_quat / np.linalg.norm(cam_quat) + camera_model = StandardCamera( + colmap_camera.width, + colmap_camera.height, + K, + dist, + [0, 0, 0], + cam_quat, + platform_pose_provider=nav_state_provider, + ) + err = [] + + # Update uncertainty based on errors + # check quaternion alignment over all frames + for xys, xyzs, t in organized_points_per_frame: + xys = np.array(xys) + xyzs = np.array(xyzs) + # Error in meters. + # Rays coming out of the camera in the direction of the imaged points. + ray_pos, ray_dir = camera_model.unproject(xys.T, t) + + # Direction coming out of the camera pointing at the actual 3-D points' + # locations. + ray_dir2 = xyzs.T - ray_pos + d = np.sqrt(np.sum((ray_dir2) ** 2, axis=0)) + ray_dir2 /= d + + dp = np.minimum(np.sum(ray_dir * ray_dir2, axis=0), 1) + dp = np.maximum(dp, -1) + theta = np.arccos(dp) + err_ = np.sin(theta) * d + # err.append(np.percentile(err_, 90)) + err.append(np.median(err_)) + + # print("Average uncertainty: ") + # print(np.mean(uncertainties)) + # err = err[err < np.percentile(err, 90)] + + err = np.median(err) + # print('RMS reproject error for quat', cam_quat, ': %0.8f' % err) + return err + + print("Iterating through %s quaternion guesses." % len(cam_quats)) + random.shuffle(cam_quats) + best_quat = None + best_err = np.inf + for cam_quat in cam_quats: + err = cam_quat_error(cam_quat) + if err < best_err: + best_err = err + best_quat = cam_quat + + if best_err < 5: + break + + print("Best error: ", best_err) + print("Best quat: ") + print(cam_quat) + + print("Minimizing error over camera quaternions") + + ret = minimize(cam_quat_error, best_quat) + best_quat = ret.x / np.linalg.norm(ret.x) + ret = minimize(cam_quat_error, best_quat, method="BFGS") + best_quat = ret.x / np.linalg.norm(ret.x) + ret = minimize(cam_quat_error, best_quat, method="Powell") + best_quat = ret.x / np.linalg.norm(ret.x) + + # Sequential 1-D optimizations. + for i in range(4): + + def set_x(x): + quat = best_quat.copy() + quat = quat / np.linalg.norm(quat) + while abs(quat[i] - x) > 1e-6: + quat[i] = x + quat = quat / np.linalg.norm(quat) + + return quat + + def func(x): + return cam_quat_error(set_x(x)) + + x = np.linspace(-1, 1, 100) + x = sorted(np.hstack([x, best_quat[i]])) + y = [func(x_) for x_ in x] + x = fminbound(func, x[np.argmin(y) - 1], x[np.argmin(y) + 1], xtol=1e-8) + best_quat = set_x(x) + + camera_model = StandardCamera( + colmap_camera.width, + colmap_camera.height, + K, + dist, + [0, 0, 0], + best_quat, + platform_pose_provider=nav_state_provider, + ) + + final_error = cam_quat_error(best_quat) + + return camera_model, final_error + + +def transfer_alignment( + colmap_camera: pycolmap.Camera, + calibrated_camera_model: StandardCamera, + points_per_image: List[List[VisiblePoint]], + colocated_points_per_image: List[List[VisiblePoint]], +) -> tuple[StandardCamera, float]: + """If you have a 3D colmap sparse model that was generated using multiple, + colocated modalities, you can generate the camera model for the higher-resolution + camera first, then bootstrap the calibration process for the lower-resolution + modalities by transferring initial the transformation to the INS that was + already found by unprojecting the matching 3D features. + + Args: + colmap_camera (pycolmap.Camera): The camera to calibrate. + calibrated_camera_model (StandardCamera): The colocated, already calibrated camera. + points_per_image (List[List[VisiblePoint]]): The 2D and 3D correspondences found + in each image in the camera to calibrate. + colocated_points_per_image (List[List[VisiblePoint]]): The 2D and 3D correspondences + found in each image in the colocated, calibrated camera. + + Raises: + SystemError: An unsupported camera model was found in the colmap model. + + Returns: + StandardCamera: The refined camera model. + """ + im_pts = [] + colocated_im_pts = [] + matched_pairs = 0 + total = 0 + + for i, pts in enumerate(points_per_image): + pts = np.asarray(pts) + pt_3d_ids = np.asarray([pt.point_3d_id for pt in pts]) + colocated_pts = np.asarray(colocated_points_per_image[i]) + colocated_pt_3d_ids = np.asarray([pt.point_3d_id for pt in colocated_pts]) + + # Need to make a unique here, since there are some non-unique 3d IDs for a + # multiple 2D points. This gets complicated, since we need to retain the original + # indices, so we return the index obtained via np.unique + uniq_pt_3d_ids, uniq_idx = np.unique(pt_3d_ids, return_index=True) + uniq_colocated_pt_3d_ids, colocated_uniq_idx = np.unique( + colocated_pt_3d_ids, return_index=True + ) + + # Get correspondences from uncalibrated -> colocated camera + match_idx = np.isin( + uniq_pt_3d_ids, uniq_colocated_pt_3d_ids, assume_unique=True + ) + if len(match_idx) < 1: + print("No matching 3D points found!") + continue + temp_pts = [pt.point_2d for pt in pts[uniq_idx][match_idx]] + + # Since they're both unique, commutative, so repeat the process but in opposite + # to find colocated -> uncalibrated correspondences + colocated_match_idx = np.isin( + uniq_colocated_pt_3d_ids, uniq_pt_3d_ids, assume_unique=True + ) + temp_colocated_pts = [ + pt.point_2d for pt in colocated_pts[colocated_uniq_idx][colocated_match_idx] + ] + + im_pts += temp_pts + colocated_im_pts += temp_colocated_pts + matched_pairs += 1 + + # Since both are equal, can take the total of either + total += np.sum(colocated_match_idx) + print( + f"Matched {matched_pairs}/{len(points_per_image)} image pairs, " + f"resulting in {total} matching 2D points." + ) + + im_pts = np.array(im_pts) + colocated_im_pts = np.array(colocated_im_pts) + + # Arbitrary cut off + minimum_pts_required = 10 + if ( + len(im_pts) < minimum_pts_required + or len(colocated_im_pts) < minimum_pts_required + ): + print("[ERROR] Not enough matching 2D image points were found for camera.") + return None, None + + # Treat as co-located cameras (they are) and unproject out of the + # calibrated camera and into the other camera. The unprojection must + # happen in the platform (INS) frame: with a live nav provider and no + # explicit time, unproject() evaluates the INS attitude at the current + # wall clock (clamped to the nearest nav sample), which bakes an + # arbitrary aircraft attitude into the solved mount quaternion and can + # push the optimization into mirrored (negative focal) minima. + nav_state_fixed = NavStateFixed(np.zeros(3), [0, 0, 0, 1]) + calibrated_fixed = copy.copy(calibrated_camera_model) + calibrated_fixed.platform_pose_provider = nav_state_fixed + ray_pos, ray_dir = calibrated_fixed.unproject(colocated_im_pts.T, 0) + wrld_pts = ray_dir.T * 1e4 + assert np.all(np.isfinite(wrld_pts)), "World points contain non-finite values." + + K = colmap_camera.calibration_matrix() + if colmap_camera.model.name == "OPENCV": + fx, fy, cx, cy, d1, d2, d3, d4 = colmap_camera.params + elif colmap_camera.model.name == "SIMPLE_RADIAL": + f, cx, cy, d1 = colmap_camera.params + fx = fy = f + d2 = d3 = d4 = 0 + elif colmap_camera.model.name == "PINHOLE": + fx, fy, cx, cy = colmap_camera.params + d1 = d2 = d3 = d4 = 0 + else: + raise SystemError(f"Unexpected camera model found: {colmap_camera.model.name}") + dist = np.array([d1, d2, d3, d4, 0]) + + flags = cv2.CALIB_ZERO_TANGENT_DIST + flags = flags | cv2.CALIB_USE_INTRINSIC_GUESS + flags = flags | cv2.CALIB_FIX_PRINCIPAL_POINT + # Optionally, fix the K intrinsics + flags = flags | cv2.CALIB_FIX_K1 + flags = flags | cv2.CALIB_FIX_K2 + flags = flags | cv2.CALIB_FIX_K3 + flags = flags | cv2.CALIB_FIX_K4 + flags = flags | cv2.CALIB_FIX_K5 + flags = flags | cv2.CALIB_FIX_K6 + + criteria = ( + cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, + 30000, + 0.0000001, + ) + + ret = cv2.calibrateCamera( + [wrld_pts.astype(np.float32)], + [im_pts.astype(np.float32)], + (colmap_camera.width, colmap_camera.height), + cameraMatrix=K.copy(), + distCoeffs=dist.copy(), + flags=flags, + criteria=criteria, + ) + + err, _, _, rvecs, tvecs = ret + + # R = np.identity(4) + R = cv2.Rodrigues(rvecs[0])[0] + cam_quat = Rotation.from_matrix(R.T).as_quat() + # cam_quat = quaternion_from_matrix(R.T) + + ## Only optimize 3/4 components of the quaternion. + # static_quat_ind = 3 # Fixing the 'w' component + dynamic_quat_ind = [0, 1, 2] # Optimizing 'x', 'y', 'z' components + dynamic_quat_ind = np.array(dynamic_quat_ind) + cam_quat = np.asarray(cam_quat) + cam_quat /= np.linalg.norm(cam_quat) + x0 = cam_quat[dynamic_quat_ind].copy() # [x, y, z] + + def get_cm(x): + """ + Create a camera model with updated quaternion and intrinsic parameters. + + Parameters: + - x: array-like, shape (N,) + Optimization variables where the first 3 elements correspond to + the dynamic quaternion components ('x', 'y', 'z'), optionally + followed by intrinsic parameters ('fx', 'fy', etc.). + + Returns: + - cm: StandardCamera instance + Updated camera model with new parameters. + """ + # Ensure 'x' has at least 3 elements for quaternion + assert ( + len(x) > 2 + ), "Optimization variable 'x' must have at least 3 elements for quaternion." + + # Validate 'x[:3]' are finite numbers + assert np.all( + np.isfinite(x[:3]) + ), "Quaternion components contain non-finite values." + + # Initialize quaternion with fixed 'w' component + cam_quat_new = np.ones(4) + + # Assign dynamic components from optimization variables + cam_quat_new[dynamic_quat_ind] = x[:3] + + # Normalize to ensure it's a unit quaternion + norm = np.linalg.norm(cam_quat_new) + assert norm > 1e-6, "Quaternion has zero or near-zero magnitude." + cam_quat_new /= norm + + # Extract intrinsic parameters + if len(x) > 3: + fx_ = x[3] + fy_ = x[4] + else: + fx_ = fx + fy_ = fy + + if len(x) > 5: + dist_ = x[5:] + else: + dist_ = dist + + # Construct the intrinsic matrix + K = np.array([[fx_, 0, cx], [0, fy_, cy], [0, 0, 1]]) + + # Create the camera model + cm = StandardCamera( + colmap_camera.width, + colmap_camera.height, + K, + dist_, + [0, 0, 0], + cam_quat_new, + platform_pose_provider=nav_state_fixed, + ) + return cm + + def error(x): + try: + # Reject non-physical or runaway intrinsics. COLMAP's bundle + # adjustment already solved them jointly from the same imagery; + # the transfer refines the rotation and, at most, small + # focal/distortion corrections. Without this wall a mirrored + # solution (negative fy) fits the trimmed objective just as well + # as the true one. + if len(x) > 4 and not ( + 0.7 * fx < x[3] < 1.3 * fx and 0.7 * fy < x[4] < 1.3 * fy + ): + return 1e8 + if len(x) > 5 and np.any(np.abs(np.asarray(x[5:]) - dist) > 0.5): + return 1e8 + cm = get_cm(x) + projected = cm.project(wrld_pts.T).T # Shape: (N, 2) + + # Compute Euclidean distances + err = np.sqrt(np.sum((im_pts - projected) ** 2, axis=1)) + + # Apply Huber loss + delta = 20 + ind = err < delta + err[ind] = err[ind] ** 2 + err[~ind] = 2 * (err[~ind] - delta / 2) * delta + + # Sort and trim the error + err = sorted(err)[: len(err) - len(err) // 5] + + # Compute mean error + mean_err = np.sqrt(np.mean(err)) + + # Add regularization term (e.g., L2 penalty) + reg_strength = 1e-3 # Adjust as needed + reg_term = reg_strength * np.linalg.norm(x[:3]) ** 2 + + total_error = mean_err + reg_term + return total_error + except Exception as e: + print(f"Error in error function: {e}") + return np.inf # Assign a high error if computation fails + + print("Optimizing error for transfer models.") + x = x0.copy() + # Example bounds for [x, y, z] components + bounds = [ + (-1.0, 1.0), # x + (-1.0, 1.0), # y + (-1.0, 1.0), # z + ] + print("First pass") + # Perform optimization on [x, y, z] + ret = minimize( + error, + x, + method="L-BFGS-B", + bounds=bounds, + callback=None, # Optional: Monitor progress + options={"disp": False, "maxiter": 30000, "ftol": 1e-7}, + ) + assert ret.success, "Minimization of transfer calibration error failed." + x = np.hstack([ret.x, fx, fy]) + print("Second pass") + assert np.all(np.isfinite(x)), "Input quaternion with locked fx, fy, is not finite." + ret = minimize(error, x, method="Powell") + x = ret.x + print("Third pass") + assert np.all(np.isfinite(x)), "Input quaternion for BFGS is not finite." + ret = minimize(error, x, method="BFGS") + + print("Fourth pass") + x = np.hstack([ret.x, dist]) + ret = minimize(error, x, method="Powell") + x = ret.x + + print("Final pass") + ret = minimize(error, x, method="BFGS") + x = ret.x + + assert np.all(np.isfinite(x)), "Input quaternion for final model is not finite." + cm = get_cm(x) + final_error = error(x) + return cm, final_error + + +def manual_alignment( + camera_model: StandardCamera, + reference_camera_model: StandardCamera, + image_point_pairs: dict, +) -> tuple[StandardCamera, float]: + pts = np.array(image_point_pairs["rightPoints"]).astype(np.float32) + reference_pts = np.array(image_point_pairs["leftPoints"]).astype(np.float32) + + def get_new_cm(x): + tmp_cm = copy.deepcopy(camera_model) + cam_quat = x[:4] + cam_quat /= np.linalg.norm(cam_quat) + tmp_cm.update_intrinsics(cam_quat=cam_quat) + + if len(x) > 4: + tmp_cm.fx = x[4] + + if len(x) > 5: + tmp_cm.fy = x[5] + + return tmp_cm + + def proj_err(x): + tmp_cm = get_new_cm(x) + + wrld_pts = reference_camera_model.unproject(reference_pts.T)[1] + im_pts = tmp_cm.project(wrld_pts) + + err = np.sqrt(np.sum(np.sum((pts.T - im_pts) ** 2, 1))) + err /= len(wrld_pts) + return err + + min_err = np.inf + best_x = None + iterations = 10000 + for _ in range(iterations): + x = np.random.rand(4) * 2 - 1 + x /= np.linalg.norm(x) + err = proj_err(x) + if err < min_err: + min_err = err + best_x = x + + x = np.hstack([best_x, camera_model.fx, camera_model.fy]) + + x = minimize(proj_err, x).x + x = minimize(proj_err, x, method="Powell").x + x = minimize(proj_err, x, method="BFGS").x + x = minimize(proj_err, x).x + + tmp_cm = get_new_cm(x) + + final_err = proj_err(x) + print( + "[bold green] Mean error after manual alignment[/bold green]:", + final_err, + "pixels", + ) + return tmp_cm, final_err + + +if __name__ == "__main__": + pass diff --git a/kamera/postflight/boresight.py b/kamera/postflight/boresight.py new file mode 100644 index 0000000..2c895cd --- /dev/null +++ b/kamera/postflight/boresight.py @@ -0,0 +1,264 @@ +"""Solve the rig-to-INS boresight from a prior-mapped ENU reconstruction. + +The reference sensor defines the rig frame, so its per-frame pose is the +rig pose. The only unknown between the reconstruction and the aircraft +is then a single rigid transform: the rotation of the rig relative to +the INS (the boresight) and, optionally, the lever arm. One robust +estimate over every synchronized frame replaces the per-camera +quaternion searches of the legacy pipeline, and -- combined with the +`sensor_from_rig` extrinsics from `rig.derive_sensor_from_rig` -- makes +all camera mounts mutually consistent by construction: + + mount(cam) = ins_from_rig o rig_from_sensor(cam) + +This works directly on the single rigless ENU model that prior-position +mapping produces (grouping images into frames by trigger time); it does +NOT need a rig-constrained reconstruction, which fragments badly to +build. The reconstruction must share the nav provider's ENU frame. +""" + +import os +from dataclasses import dataclass, field +from typing import Dict, List, Optional + +import numpy as np +import pycolmap +from rich import print +from scipy.spatial.transform import Rotation + +from kamera.colmap_processing.camera_models import StandardCamera +from kamera.postflight.naming import KameraImageName +from kamera.postflight.rig import _order_by_ref, derive_sensor_from_rig +from kamera.sensor_models.nav_state import NavStateINSJson + +__all__ = [ + "BoresightEstimate", + "average_quaternions", + "solve_rig_boresight", + "export_rig_camera_models", +] + + +@dataclass +class BoresightEstimate: + # Quaternion (x, y, z, w) mapping rig coordinates into the INS body + # frame. This is the reference sensor's StandardCamera mount directly: + # StandardCamera's camera_quaternion maps camera->INS (see unproject in + # camera_models.py), and the reference sensor is the rig frame. + ins_from_rig: np.ndarray + lever_arm_ins: np.ndarray # rig origin relative to INS, in the INS frame (m) + num_frames: int # frames used (inliers) + num_rejected: int # frames rejected as outliers + residuals_deg: np.ndarray # per-inlier-frame angular residual + # Every per-frame sample, pre-outlier-rejection, so error_report.py + # can re-derive the residual under different assumptions (e.g. a + # camera-to-INS time offset). `inlier_mask` marks the frames the + # averaged boresight used. + sample_times: np.ndarray = field(default_factory=lambda: np.zeros(0)) + sample_quats: np.ndarray = field(default_factory=lambda: np.zeros((0, 4))) + inlier_mask: np.ndarray = field(default_factory=lambda: np.zeros(0, dtype=bool)) + + +def average_quaternions( + quats: np.ndarray, weights: Optional[np.ndarray] = None +) -> np.ndarray: + """Chordal L2 mean of unit quaternions (largest eigenvector of the + weighted outer-product matrix). Handles the q/-q sign ambiguity.""" + Q = np.asarray(quats, dtype=float).copy() + Q /= np.linalg.norm(Q, axis=1, keepdims=True) + signs = np.sign(Q @ Q[0]) + signs[signs == 0] = 1 + Q *= signs[:, None] + w = np.ones(len(Q)) if weights is None else np.asarray(weights, dtype=float) + M = (Q.T * w) @ Q + _, eigvecs = np.linalg.eigh(M) + mean = eigvecs[:, -1] + return mean / np.linalg.norm(mean) + + +def solve_rig_boresight( + reconstruction: "pycolmap.Reconstruction", + nav_state_provider: NavStateINSJson, + times: Dict[str, float], + ref_modality: str = "rgb", + outlier_threshold_deg: float = 2.0, + max_iterations: int = 5, +) -> BoresightEstimate: + """Estimate ins_from_rig by robust rotation averaging over the + reference sensor's images of an ENU-registered reconstruction. + + The reference sensor (first camera folder of `ref_modality`) defines + the rig frame, so its per-image pose is the rig pose. Per frame the + boresight is + + ins_from_rig = (enu_from_ins)^-1 . (rig_from_enu)^-1 + + which is constant across frames because the rig is rigidly mounted to + the INS. The reconstruction must share the nav provider's ENU frame + (which prior-position mapping yields); an arbitrary-gauge model, e.g. + from the global mapper which ignores priors, gives a non-constant + per-frame estimate (large residual spread) and must not be used here. + + `times` maps image base names to exposure times (see + `kamera.postflight.rig.basename_to_time`). + """ + folders = { + im.name.rsplit("/", 1)[0] + for im in reconstruction.images.values() + if im.has_pose and "/" in im.name + } + if not folders: + raise SystemError("Reconstruction has no posed, folder-qualified images.") + ref_folder = _order_by_ref(folders, ref_modality)[0] + + rel_quats: List[np.ndarray] = [] + lever_arms: List[np.ndarray] = [] + frame_times: List[float] = [] + for im in reconstruction.images.values(): + if not im.has_pose or im.name.rsplit("/", 1)[0] != ref_folder: + continue + try: + t = times[KameraImageName.parse(im.name).base_name] + except (ValueError, KeyError): + continue + ins_pos, ins_quat = nav_state_provider.pose(t) + # World here is ENU; the INS quaternion is the body attitude in ENU. + R_enu_from_ins = Rotation.from_quat(ins_quat) + rig_from_world = im.cam_from_world() + R_rig_from_enu = Rotation.from_quat(rig_from_world.rotation.quat) + # ins_from_rig = (enu_from_ins)^-1 . (rig_from_enu)^-1 + rel_quats.append((R_enu_from_ins.inv() * R_rig_from_enu.inv()).as_quat()) + frame_times.append(t) + # Rig origin in ENU, then expressed in the INS body frame. + R = rig_from_world.rotation.matrix() + rig_pos_enu = -R.T @ rig_from_world.translation + lever_arms.append(R_enu_from_ins.inv().apply(rig_pos_enu - np.asarray(ins_pos))) + + if len(rel_quats) < 3: + raise SystemError( + f"Only {len(rel_quats)} reference-sensor frames with nav times; " + "cannot solve the boresight." + ) + + quats = np.asarray(rel_quats) + levers = np.asarray(lever_arms) + inliers = np.ones(len(quats), dtype=bool) + mean = average_quaternions(quats) + for _ in range(max_iterations): + mean_rot = Rotation.from_quat(mean) + residuals = np.degrees( + (Rotation.from_quat(quats) * mean_rot.inv()).magnitude() + ) + new_inliers = residuals < outlier_threshold_deg + if new_inliers.sum() < 3 or np.array_equal(new_inliers, inliers): + inliers = new_inliers if new_inliers.sum() >= 3 else inliers + break + inliers = new_inliers + mean = average_quaternions(quats[inliers]) + + mean_rot = Rotation.from_quat(mean) + residuals = np.degrees( + (Rotation.from_quat(quats[inliers]) * mean_rot.inv()).magnitude() + ) + estimate = BoresightEstimate( + ins_from_rig=mean, + lever_arm_ins=np.median(levers[inliers], axis=0), + num_frames=int(inliers.sum()), + num_rejected=int((~inliers).sum()), + residuals_deg=residuals, + sample_times=np.asarray(frame_times), + sample_quats=quats, + inlier_mask=inliers, + ) + print( + f"Boresight from {estimate.num_frames} frames " + f"({estimate.num_rejected} rejected): " + f"median residual {np.median(residuals):.3f} deg, " + f"p90 {np.percentile(residuals, 90):.3f} deg." + ) + print(f"Lever arm (INS frame, m): {np.round(estimate.lever_arm_ins, 3)}") + return estimate + + +def _folder_camera( + reconstruction: "pycolmap.Reconstruction", +) -> Dict[str, "pycolmap.Camera"]: + """Map each camera folder to its Camera, via a posed image.""" + out: Dict[str, "pycolmap.Camera"] = {} + for image in reconstruction.images.values(): + if not image.has_pose or "/" not in image.name: + continue + folder = image.name.rsplit("/", 1)[0] + if folder not in out: + out[folder] = reconstruction.cameras[image.camera_id] + return out + + +def export_rig_camera_models( + reconstruction: "pycolmap.Reconstruction", + estimate: BoresightEstimate, + nav_state_provider: NavStateINSJson, + save_dir: str | os.PathLike, + ref_modality: str = "rgb", + sensor_from_rig: Optional[Dict[str, "pycolmap.Rigid3d"]] = None, + use_lever_arm: bool = False, + suffix: str = "", +) -> Dict[str, StandardCamera]: + """Compose and write a StandardCamera yaml per camera folder. + + The mount (StandardCamera.camera_quaternion, camera->INS) is + + cam_quat(sensor) = ins_from_rig . rig_from_sensor + + where rig_from_sensor comes from `sensor_from_rig` (derived by + `rig.derive_sensor_from_rig`; computed here if not supplied). For the + reference sensor rig_from_sensor is identity, so its mount is the + boresight itself. + """ + os.makedirs(save_dir, exist_ok=True) + if sensor_from_rig is None: + sensor_from_rig = derive_sensor_from_rig(reconstruction, ref_modality) + folder_camera = _folder_camera(reconstruction) + ref_folder = _order_by_ref(folder_camera, ref_modality)[0] + ins_from_rig = Rotation.from_quat(estimate.ins_from_rig) + position = estimate.lever_arm_ins if use_lever_arm else np.zeros(3) + + models: Dict[str, StandardCamera] = {} + for folder, camera in folder_camera.items(): + if folder == ref_folder: + rig_from_sensor = Rotation.identity() + cam_offset_rig = np.zeros(3) + elif folder in sensor_from_rig: + sfr = sensor_from_rig[folder] + rig_from_sensor = Rotation.from_quat(sfr.rotation.quat).inv() + cam_offset_rig = -sfr.rotation.matrix().T @ sfr.translation + else: + print(f"[yellow]No extrinsics for {folder}; skipping.") + continue + mount = ins_from_rig * rig_from_sensor + if camera.model.name == "OPENCV": + fx, fy, cx, cy, d1, d2, d3, d4 = camera.params + dist = np.array([d1, d2, d3, d4]) + elif camera.model.name == "PINHOLE": + fx, fy, cx, cy = camera.params + dist = np.zeros(4) + else: + raise SystemError(f"Unexpected camera model {camera.model.name}") + K = np.array([[fx, 0, cx], [0, fy, cy], [0, 0, 1]]) + # cam_offset_rig is in rig coords; the mount position is in the INS + # frame, so rotate rig->INS via ins_from_rig. + cam_position = position + ins_from_rig.apply(cam_offset_rig) + model = StandardCamera( + camera.width, + camera.height, + K, + dist, + cam_position if use_lever_arm else np.zeros(3), + mount.as_quat(), + platform_pose_provider=nav_state_provider, + ) + out_path = os.path.join(save_dir, f"{folder}{suffix}.yaml") + model.save_to_file(out_path) + print(f"Wrote {out_path}") + models[folder] = model + return models diff --git a/kamera/postflight/colmap.py b/kamera/postflight/colmap.py new file mode 100644 index 0000000..f07f3a4 --- /dev/null +++ b/kamera/postflight/colmap.py @@ -0,0 +1,535 @@ +import os +import copy +import json +import pathlib +import cv2 +import PIL.Image +import numpy as np +import os.path as osp +import ubelt as ub +from rich import print + +import pycolmap as pc +from dataclasses import dataclass +from typing import Optional, Tuple, List, Dict +from kamera.sensor_models.nav_state import NavStateFixed, NavStateINSJson +from kamera.colmap_processing.camera_models import StandardCamera +from kamera.colmap_processing.image_renderer import render_view +from kamera.postflight.alignment import ( + VisiblePoint, + iterative_alignment, + manual_alignment, + transfer_alignment, +) +from kamera.postflight.naming import ( + KameraCameraName, + KameraImageName, + swap_image_name_modality, +) + + +@dataclass(frozen=True) +class ColmapCalibrationData: + # each sfm pose is a [position (3,), quaternion (4,)] pair + sfm_poses: List[list] + ins_poses: List[np.ndarray] + img_fnames: List[str] + img_times: List[float] + llhs: List[np.ndarray] + points_per_image: List[List[VisiblePoint]] + basename_to_time: Dict[str, float] + fname_to_time_channel_modality: Dict[float, Dict[str, Dict[str, str]]] + best_cameras: Dict[str, pc.Camera] + + +class ColmapCalibration(object): + def __init__( + self, flight_dir: str | os.PathLike, + recon_dir: str | os.PathLike, + colmap_dir: str | os.PathLike + ) -> None: + self.flight_dir = flight_dir + self.recon_dir = recon_dir + self.colmap_dir = colmap_dir + # contains the images, 3D points, and cameras of the colmap database + self.R = pc.Reconstruction(self.recon_dir) + # populated by prepare_calibration_data() + self.ccd: Optional[ColmapCalibrationData] = None + self.nav_state_provider = self.load_nav_state_provider() + print(self.R.summary()) + + def load_nav_state_provider(self): + # Create navigation stream + json_glob = pathlib.Path(self.flight_dir).rglob("*_meta.json") + try: + next(json_glob) + except StopIteration: + raise SystemExit("No meta jsons were found, please check your filepaths.") + return NavStateINSJson((json_glob)) + + def get_base_name(self, fname: str | os.PathLike) -> str: + """Given an arbitrary filename (could be UV, IR, RGB, json), + extract the portion of the filename that is just the time, flight, + machine (C, L, R), and effort name. + """ + return KameraImageName.parse(fname).base_name + + def get_modality(self, fname: str | os.PathLike) -> str: + """Extract image modality (e.g. ir/meta) from a given kamera filename.""" + return KameraImageName.parse(fname).modality + + def get_channel(self, fname: str | os.PathLike) -> str: + """Extract channel (e.g. L/R/C) from a given kamera filename.""" + return KameraImageName.parse(fname).channel + + def get_view(self, fname: str | os.PathLike) -> str: + """Extract view (e.g. left_view, right_view, center_view) from + a given kamera filename""" + return KameraImageName.parse(fname).view + + def get_basename_to_time(self, flight_dir: str | os.PathLike) -> dict: + # Establish correspondence between real-world exposure times base of file + # names. + basename_to_time = {} + for json_fname in pathlib.Path(flight_dir).rglob("*_meta.json"): + try: + with open(json_fname) as json_file: + d = json.load(json_file) + # Time that the image was taken. + basename = self.get_base_name(json_fname) + basename_to_time[basename] = float(d["evt"]["time"]) + except (OSError, ValueError): + pass + return basename_to_time + + def prepare_calibration_data(self): + img_fnames = [] + img_times = [] + ins_poses = [] + sfm_poses = [] + llhs = [] + points_per_image = [] + basename_to_time = self.get_basename_to_time(self.flight_dir) + best_cameras = {} + most_points = {} + for image_num, image in self.R.images.items(): + try: + base_name = self.get_base_name(image.name) + t = basename_to_time[base_name] + except (KeyError, ValueError): + print( + "Couldn't find a _meta.json file associated with '%s'" % image.name + ) + continue + + # Query the navigation state recorded by the INS for this time. + ins_pose = self.nav_state_provider.pose(t) + llh = self.nav_state_provider.llh(t) + + # Query Colmaps pose for the camera. + pose = image.cam_from_world() + R = pose.rotation.matrix() + pos = -np.dot(R.T, pose.translation) + + pose.rotation.normalize() + quat = pose.rotation.quat + # invert the w (rotation) component, + # so we get the camera to world rotation + quat[3] *= -1 + + sfm_pose = [pos, quat] + + img_times.append(t) + ins_poses.append(ins_pose) + img_fnames.append(image.name) + sfm_poses.append(sfm_pose) + llhs.append(llh) + points = [] + # associate all the 2D and 3D points seen by this image + for pt in image.points2D: + if pt.has_point3D(): + point_2d = pt.xy + uncertainty = self.R.points3D[pt.point3D_id].error + point_3d = self.R.points3D[pt.point3D_id].xyz + visible = True + point_3d_id = pt.point3D_id + vpt = VisiblePoint( + point_3d, point_3d_id, point_2d, uncertainty, t, visible + ) + points.append(vpt) + points_per_image.append(points) + camera_name = os.path.basename(os.path.dirname(image.name)) + # as a way to choose the first quaternion we check, choose the one + # that has the most number of 3D points registered + if camera_name in best_cameras: + if image.num_points3D > most_points[camera_name]: + best_cameras[camera_name] = image.camera + most_points[camera_name] = image.num_points3D + else: + best_cameras[camera_name] = image.camera + most_points[camera_name] = image.num_points3D + + # sort all entries + ind = np.argsort(img_fnames) + img_fnames = [img_fnames[i] for i in ind] + img_times = [img_times[i] for i in ind] + ins_poses = [ins_poses[i] for i in ind] + sfm_poses = [sfm_poses[i] for i in ind] + points_per_image = [points_per_image[i] for i in ind] + llhs = [llhs[i] for i in ind] + + fname_to_time_channel_modality = self.create_fname_to_time_channel_modality( + img_fnames, basename_to_time + ) + + ccd = ColmapCalibrationData( + sfm_poses=sfm_poses, + ins_poses=ins_poses, + img_fnames=img_fnames, + img_times=img_times, + llhs=llhs, + points_per_image=points_per_image, + basename_to_time=basename_to_time, + fname_to_time_channel_modality=fname_to_time_channel_modality, + best_cameras=best_cameras, + ) + self.ccd = ccd + return ccd + + def calibrate_camera( + self, camera_name: str + ) -> Tuple[Optional[StandardCamera], Optional[float]]: + assert self.ccd is not None, "call prepare_calibration_data() first" + sfm_quats = np.array([pose[1] for pose in self.ccd.sfm_poses]) + ins_quats = np.array([pose[1] for pose in self.ccd.ins_poses]) + # Find all valid indices of current camera + cam_idxs = [ + 1 if camera_name == os.path.basename(os.path.dirname(im)) else 0 + for im in self.ccd.img_fnames + ] + print(f"Number of images in camera {camera_name}.") + print(np.sum(cam_idxs)) + observations = [ + pts for i, pts in enumerate(self.ccd.points_per_image) if cam_idxs[i] == 1 + ] + cam_sfm_quats = [q for i, q in enumerate(sfm_quats) if cam_idxs[i] == 1] + cam_ins_quats = [q for i, q in enumerate(ins_quats) if cam_idxs[i] == 1] + if len(observations) < 10: + print(f"Only {len(observations)} seen for camera {camera_name}, skipping.") + return None, None + cam = self.ccd.best_cameras[camera_name] + camera_model, error = iterative_alignment( + cam_sfm_quats, cam_ins_quats, observations, cam, self.nav_state_provider + ) + return camera_model, error + + def transfer_calibration( + self, + camera_name: str, + calibrated_camera_model: StandardCamera, + calibrated_modality: str, + ) -> Tuple[Optional[StandardCamera], Optional[float]]: + """ + Use the quaternion already solved of a colocated, calibrated camera + to bootstrap the calibration process. + """ + assert self.ccd is not None, "call prepare_calibration_data() first" + # Find all valid indices of current cameras + cam_idxs = [ + 1 if camera_name == os.path.basename(os.path.dirname(im)) else 0 + for im in self.ccd.img_fnames + ] + print(f"Number of images in camera {camera_name}.") + print(np.sum(cam_idxs)) + + # Now we have to find the overlapping observations on a per-frame basis between + # the camera to calibrate, and the colocated, calibrated, camera. + colocated_observations = [] + observations = [] + for i, fname in enumerate(self.ccd.img_fnames): + if cam_idxs[i] == 1: + # Swap in the modality of the already calibrated camera + calibrated_fname = swap_image_name_modality(fname, calibrated_modality) + try: + ii = self.ccd.img_fnames.index(calibrated_fname) + except ValueError: + print(f"Missing {calibrated_fname} from calibrated index.") + continue + colocated_observations.append(self.ccd.points_per_image[ii]) + observations.append(self.ccd.points_per_image[i]) + if len(observations) < 10 or len(colocated_observations) < 10: + print( + f"Only {len(observations)} seen for camera {camera_name}, and " + f" only {len(colocated_observations)} found for the calibrated camera, " + " skipping." + ) + return None, None + + cam = self.ccd.best_cameras[camera_name] + camera_model, error = transfer_alignment( + cam, calibrated_camera_model, observations, colocated_observations + ) + return camera_model, error + + def manual_calibration( + self, + camera_model: StandardCamera, + reference_camera_model: StandardCamera, + point_pairs: dict, + ) -> tuple[StandardCamera, float]: + print("Refining camera model using manually defined point pairs.") + refined_model, error = manual_alignment( + camera_model, reference_camera_model, point_pairs + ) + return refined_model, error + + def create_fname_to_time_channel_modality( + self, img_fnames, basename_to_time + ) -> Dict[float, Dict[str, Dict[str, str]]]: + print("Creating mapping between RGB and UV images...") + time_to_modality = ub.AutoDict() + for fname in img_fnames: + base_name = self.get_base_name(fname) + try: + t = basename_to_time[base_name] + except Exception as e: + print(e) + print(f"No ins time found for image {base_name}.") + continue + modality = self.get_modality(fname) + channel = self.get_channel(fname) + time_to_modality[t][channel][modality] = fname + return time_to_modality + + def _find_image_file( + self, + images_roots: List[str], + cam_dir: str, + bname: str, + ) -> Optional[str]: + """Find an image under any of the given images roots, trying common + extensions if the exact basename is not present.""" + for root in images_roots: + path = pathlib.Path(root) / cam_dir / bname + candidates = [path] + [ + path.with_suffix(ext) for ext in (".jpg", ".jpeg", ".png", ".tiff") + ] + for candidate in candidates: + if candidate.is_file(): + return str(candidate) + return None + + def write_gifs( + self, + gif_dir: str | os.PathLike, + camera_name: str, + colocated_modality: str, + camera_model: StandardCamera, + colocated_camera_model: StandardCamera, + num_gifs: int = 5, + ) -> None: + """Write registration gifs flipping between images of `camera_name` + (its image-folder name, e.g. "85mm_25_5deg_center_uv") and the + colocated camera's image warped into its view.""" + assert self.ccd is not None, "call prepare_calibration_data() first" + cam_name = KameraCameraName.parse(camera_name) + colocated_camera_name = cam_name.with_modality(colocated_modality).name + print( + f"Writing registration gifs for cameras {camera_name} " + f"and {colocated_camera_name}." + ) + ub.ensuredir(gif_dir) + + # Warp in the platform frame for both cameras. The models may arrive + # with different pose providers (a freshly solved camera carries the + # live INS provider, a loaded/transferred one an identity provider); + # mixing them injects an arbitrary INS attitude into the warp even + # when both mount quaternions are correct. + nav_state_fixed = NavStateFixed(np.zeros(3), np.array([0, 0, 0, 1])) + camera_model = copy.copy(camera_model) + camera_model.platform_pose_provider = nav_state_fixed + colocated_camera_model = copy.copy(colocated_camera_model) + colocated_camera_model.platform_pose_provider = nav_state_fixed + + images_root = osp.join(self.colmap_dir, "images0") + # The colocated modality may live in a different colmap workspace + # (e.g. the rgb images paired with an ir model live under colmap_rgb). + colocated_roots = [ + images_root, + osp.join(self.flight_dir, f"colmap_{colocated_modality}", "images0"), + ] + + img_fnames = self.ccd.img_fnames + for k in range(num_gifs): + inds = list(range(len(img_fnames))) + np.random.shuffle(inds) + img = colocated_img = None + bname = colocated_bname = None + for i in inds: + fname = img_fnames[i] + cam_dir = osp.basename(osp.dirname(fname)) + if cam_dir != camera_name: + continue + bname = osp.basename(fname) + abs_fname = self._find_image_file([images_root], cam_dir, bname) + if abs_fname is None: + print(f"No {cam_name.modality} image found for {bname}") + continue + img = cv2.imread(abs_fname, cv2.IMREAD_COLOR) + if img is None: + print(f"Could not read image at {abs_fname}") + continue + img = img[:, :, ::-1] + + # get the colocated image from the same time + colocated_dir = ( + KameraCameraName.parse(cam_dir) + .with_modality(colocated_modality) + .name + ) + colocated_bname = ( + KameraImageName.parse(bname) + .with_modality(colocated_modality) + .name + ) + abs_colocated_fname = self._find_image_file( + colocated_roots, colocated_dir, colocated_bname + ) + if abs_colocated_fname is None: + print(f"No {colocated_modality} image found for {bname}") + continue + colocated_img = cv2.imread(abs_colocated_fname, cv2.IMREAD_COLOR) + if colocated_img is None: + print(f"Could not read image at {abs_colocated_fname}") + continue + colocated_img = colocated_img[:, :, ::-1] + colocated_bname = osp.basename(abs_colocated_fname) + # Once we find a matching pair, break + break + + if img is None or colocated_img is None: + print("Failed to find matching image pair, skipping.") + continue + print(f"Writing {bname} and {colocated_bname} to gif.") + + # warps the given colocated image into the view of the original camera model + warped_colocated_img, mask = render_view( + colocated_camera_model, colocated_img, 0, camera_model, 0, block_size=10 + ) + fname_out = osp.join( + gif_dir, + f"{camera_name}_to_{colocated_camera_name}_registration_{k}.gif", + ) + print(f"Writing gif to {fname_out}.") + # Make sure gifs are reasonable size + h, w, _ = img.shape + new_w = 1280 + new_h = int(new_w * h / w) + pil_img = PIL.Image.fromarray(img).resize((new_w, new_h)) + pil_colocated_img = PIL.Image.fromarray(warped_colocated_img).resize( + (new_w, new_h) + ) + pil_img.save( + fname_out, + save_all=True, + append_images=[pil_colocated_img], + duration=350, + loop=0, + ) + + def align_model(self, output_dir: str | os.PathLike) -> None: + assert self.ccd is not None, "call prepare_calibration_data() first" + img_fnames = [im.name for im in self.R.images.values()] + points = [] + ins_poses = [] + for image_name in img_fnames: + base_name = self.get_base_name(image_name) + t = self.ccd.basename_to_time[base_name] + # Query the navigation state recorded by the INS for this time. + pose = self.nav_state_provider.pose(t) + ins_poses.append(pose) + x, y, z = pose[0] + points.append([x, y, z]) + points = np.array(points) + locations_txt = os.path.join(output_dir, "image_locations.txt") + self.write_image_locations(locations_txt, img_fnames, ins_poses) + print( + f"Aligning model given {len(img_fnames)} images and their ENU coordinates." + ) + ransac_options = pc.RANSACOptions( + max_error=4.0, # for example, the reprojection error in pixels + min_inlier_ratio=0.01, + confidence=0.9999, + min_num_trials=1000, + max_num_trials=100000, + ) + min_common_observations = 5 + tform = pc.align_reconstruction_to_locations( + self.R, img_fnames, points, min_common_observations, ransac_options + ) + print("Transformation after alignment: ") + print(tform.scale, tform.rotation, tform.translation) + # Apply transform to self + self.R.transform(tform) + print(f"Saving aligned model to {output_dir}...") + self.R.write(output_dir) + + def read_image_locations( + self, fname: str | os.PathLike + ) -> Tuple[List[str], np.ndarray]: + with open(fname, "r") as f: + lines = f.readlines() + points = [] + image_fnames = [] + for line in lines: + # Pull x,y,z out in ENU + try: + img_fname, x, y, z = line.split(" ") + except Exception as e: + print(e) + print("Skipping location line.") + continue + image_fnames.append(img_fname.strip()) + points.append((float(x), float(y), float(z))) + points = np.array(points) + return image_fnames, points + + def write_image_locations( + self, + locations_fname: str | os.PathLike, + img_fnames: List[str], + ins_poses: List[np.ndarray], + ) -> None: + print(f"Writing image locations to {locations_fname}") + img_fnames = sorted(img_fnames) + with open(locations_fname, "w") as fo: + for i in range(len(img_fnames)): + name = img_fnames[i] + pos = ins_poses[i][0] + fo.write("%s %0.8f %0.8f %0.8f\n" % (name, pos[0], pos[1], pos[2])) + + +def find_best_sparse_model(sparse_dir: str | os.PathLike) -> str: + all_models = sorted(os.listdir(sparse_dir)) if os.path.isdir(sparse_dir) else [] + print(f"All Models: {all_models}") + best_model = "" + most_images_aligned = 0 + for subdir in all_models: + dir = os.path.join(sparse_dir, subdir) + try: + R = pc.Reconstruction(dir) + except Exception as e: + print(e) + continue + num_images = len(R.images.keys()) + print(f"Number of images in {dir}: {num_images}") + if num_images > most_images_aligned: + most_images_aligned = num_images + best_model = dir + if not best_model: + raise SystemError( + f"No valid sparse models found in {sparse_dir}, please verify " + "your model built correctly." + ) + print(f"Selecting {best_model} as the best model.") + return best_model diff --git a/kamera/postflight/dat_to_csv.py b/kamera/postflight/dat_to_csv.py index 09ea121..b3c9e96 100644 --- a/kamera/postflight/dat_to_csv.py +++ b/kamera/postflight/dat_to_csv.py @@ -417,8 +417,11 @@ def parse_gsof_stream(buf): def maybe_gsof(buf): # type: (bytes) -> bool - if struct.unpack("B", buf[0:1])[0] == START_TX: - return True + try: + if struct.unpack('B', buf[0:1])[0] == START_TX: + return True + except: + pass return False diff --git a/kamera/postflight/deep_match.py b/kamera/postflight/deep_match.py new file mode 100644 index 0000000..a75d358 --- /dev/null +++ b/kamera/postflight/deep_match.py @@ -0,0 +1,97 @@ +"""Deep cross-modal image matching for calibration fusion. + +SIFT cannot match long-wave IR to visible imagery, which is why the +calibration pipeline reconstructs each modality group separately. This +module wraps MINIMA (modality-invariant matchers, CVPR 2025) via the +``vismatch`` package so IR images can be matched directly against EO +images and registered into the EO reconstruction (see ``fusion.py``). + +``vismatch`` pulls in torch and is deliberately an optional dependency +(``uv sync --group fusion``); it is imported lazily inside +``DeepMatcher`` so the rest of postflight works without it. Model +weights are downloaded automatically on first use (network required). +""" + +from typing import Tuple + +import cv2 +import numpy as np + +__all__ = ["DeepMatcher", "to_uint8"] + + +def to_uint8(img: np.ndarray, clahe: bool = False) -> np.ndarray: + """An image as 8-bit grayscale, contrast-stretched if necessary. + + uint8 input passes through (modulo an RGB->gray collapse); higher + bit depths (16-bit IR TIFFs) are stretched over their 1-99 + percentile range. `clahe` additionally applies local equalization, + which can help very low-contrast thermal scenes. + """ + if img.ndim == 3: + img = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) + if img.dtype != np.uint8: + img = img.astype(np.float64) + lo, hi = np.percentile(img, [1, 99]) + if hi <= lo: + lo, hi = float(img.min()), float(max(img.max(), img.min() + 1)) + img = np.clip((img - lo) / (hi - lo) * 255, 0, 255).astype(np.uint8) + if clahe: + img = cv2.createCLAHE(clipLimit=3.0, tileGridSize=(8, 8)).apply(img) + return img + + +def _matcher_input(img: np.ndarray) -> np.ndarray: + """A grayscale/BGR image as the (3, H, W) float32 [0, 1] array + vismatch matchers consume.""" + img = to_uint8(img) + chan = img.astype(np.float32) / 255.0 + return np.stack([chan] * 3) + + +class DeepMatcher: + """Semi-dense cross-modal matcher (default MINIMA-LoFTR). + + Matched keypoints are returned in the pixel coordinates of the + arrays passed to `match` -- any resizing the underlying model does + internally is rescaled back by vismatch (covered by the identity and + known-shift tests in test_deep_match.py). + """ + + def __init__( + self, + matcher_name: str = "minima-loftr", + device: str = "auto", + max_num_keypoints: int = 2048, + ): + try: + from vismatch import get_default_device, get_matcher + except ImportError as e: + raise ImportError( + "Cross-modal fusion needs the optional 'fusion' dependency " + "group: uv sync --group fusion" + ) from e + if device == "auto": + device = get_default_device() + self.name = matcher_name + self.device = device + try: + self._matcher = get_matcher( + matcher_name, device=device, max_num_keypoints=max_num_keypoints + ) + except Exception as e: + raise RuntimeError( + f"Could not initialize matcher '{matcher_name}' (weights are " + "downloaded on first use; check network access)." + ) from e + + def match( + self, img0: np.ndarray, img1: np.ndarray + ) -> Tuple[np.ndarray, np.ndarray]: + """Match two images; returns (kpts0 [N,2], kpts1 [N,2]) float64 + pixel coordinates of the putative matches (no per-match scores -- + downstream RANSAC handles outliers).""" + result = self._matcher(_matcher_input(img0), _matcher_input(img1)) + kpts0 = np.asarray(result["matched_kpts0"], dtype=np.float64).reshape(-1, 2) + kpts1 = np.asarray(result["matched_kpts1"], dtype=np.float64).reshape(-1, 2) + return kpts0, kpts1 diff --git a/kamera/postflight/error_report.py b/kamera/postflight/error_report.py new file mode 100644 index 0000000..1405142 --- /dev/null +++ b/kamera/postflight/error_report.py @@ -0,0 +1,331 @@ +"""Post-run calibration error report. + +Quantifies each modality group's boresight residual, attributes it to +the two error sources the data can distinguish -- a constant +camera-to-INS time offset and INS heading noise -- and writes a PDF +(``calibration_error_report.pdf``) with per-camera error tables next to +the exported yamls. + +The input is the per-frame boresight samples that `solve_rig_boresight` +retains (``BoresightEstimate.sample_*``). Each sample is an independent +single-frame measurement of the same physical rotation, so structure in +their residuals is diagnostic: + +- Time sync: the INS attitude is interpolated at the recorded exposure + time, so a constant clock offset converts attitude *rate* into + rotation error. Sweeping a trial offset and re-evaluating the + residual spread locates the offset the data supports; a flat sweep + curve means timing does not explain the residual. +- Heading: single-antenna INS heading is typically several times worse + than roll/pitch. Expressing each residual as a rotation vector in the + INS body frame (aerospace NED: x forward, y right, z down -- see the + ``rzyx`` euler order in ``nav_state``) splits it per axis; a dominant + z component points at heading noise. + +At runtime imagery is projected through the INS pose plus the static +mount, so the per-frame residual here is the per-frame pointing error +production will see -- ``altitude * tan(residual)`` of ground error. +""" + +import os +from typing import Dict, List, Optional, Tuple + +import matplotlib.pyplot as plt +import numpy as np +from matplotlib.backends.backend_pdf import PdfPages +from scipy.spatial.transform import Rotation + +from kamera.postflight.boresight import BoresightEstimate, average_quaternions + +__all__ = [ + "residual_components_deg", + "sweep_time_offset", + "write_error_report", +] + +_AXIS_LABELS = ["roll (body x)", "pitch (body y)", "heading (body z)"] + + +def _nav_attitudes(nav_state_provider, times: np.ndarray) -> Rotation: + """Batched enu_from_ins attitude at each time.""" + return Rotation.from_quat( + np.asarray([nav_state_provider.pose(t)[1] for t in times]) + ) + + +def _spread_deg(quats: np.ndarray) -> np.ndarray: + """Per-sample angular distance (deg) from the chordal mean.""" + mean = Rotation.from_quat(average_quaternions(quats)) + return np.degrees((Rotation.from_quat(quats) * mean.inv()).magnitude()) + + +def sweep_time_offset( + estimate: BoresightEstimate, + nav_state_provider, + max_offset_s: float = 0.25, + step_s: float = 0.005, +) -> Tuple[np.ndarray, np.ndarray]: + """Median boresight residual as a function of a constant offset added + to the exposure times before interpolating the INS attitude. + + Returns (offsets_s, median_residual_deg). The minimum is the + camera-to-INS clock offset best supported by the data. + """ + ts = np.asarray(estimate.sample_times)[estimate.inlier_mask] + quats = np.asarray(estimate.sample_quats)[estimate.inlier_mask] + # Undo the offset-zero nav attitude baked into each sample to recover + # the frame's SfM-side rotation, which does not depend on the offset: + # sample = enu_from_ins(t)^-1 . enu_from_rig + enu_from_rig = _nav_attitudes(nav_state_provider, ts) * Rotation.from_quat(quats) + offsets = np.arange(-max_offset_s, max_offset_s + step_s / 2, step_s) + medians = np.empty(len(offsets)) + for i, dt in enumerate(offsets): + shifted = _nav_attitudes(nav_state_provider, ts + dt) + medians[i] = float(np.median(_spread_deg((shifted.inv() * enu_from_rig).as_quat()))) + return offsets, medians + + +def residual_components_deg(estimate: BoresightEstimate) -> np.ndarray: + """Inlier residual rotations as rotation vectors (deg) in the INS + body frame, one row per frame: [roll(x), pitch(y), heading(z)].""" + mean = Rotation.from_quat(estimate.ins_from_rig) + quats = np.asarray(estimate.sample_quats)[estimate.inlier_mask] + return np.degrees((Rotation.from_quat(quats) * mean.inv()).as_rotvec()) + + +def _median_altitude_m(reconstruction) -> Optional[float]: + """Median camera height above the ENU origin, from posed images.""" + zs = [ + float(im.projection_center()[2]) + for im in reconstruction.images.values() + if im.has_pose + ] + return float(np.median(zs)) if zs else None + + +def _ground_m(residual_deg: float, altitude_m: Optional[float]) -> Optional[float]: + if altitude_m is None: + return None + return altitude_m * np.tan(np.radians(residual_deg)) + + +def _analyze_group(group: Dict, nav_state_provider) -> Dict: + """All derived error numbers for one _calibrate_group result.""" + est: BoresightEstimate = group["estimate"] + out = { + "estimate": est, + "altitude_m": _median_altitude_m(group["rec"]), + "median_deg": float(np.median(est.residuals_deg)), + "p90_deg": float(np.percentile(est.residuals_deg, 90)), + } + if len(est.sample_times) == 0: + return out + offsets, medians = sweep_time_offset(est, nav_state_provider) + best = int(np.argmin(medians)) + comps = residual_components_deg(est) + out.update( + offsets_s=offsets, + sweep_medians_deg=medians, + best_offset_s=float(offsets[best]), + best_offset_median_deg=float(medians[best]), + components_deg=comps, + axis_rms_deg=np.sqrt(np.mean(comps**2, axis=0)), + inlier_times=np.asarray(est.sample_times)[est.inlier_mask], + ) + return out + + +def _summary_lines(name: str, a: Dict) -> List[str]: + est = a["estimate"] + alt = a["altitude_m"] + lines = [ + f"Group '{name}': {est.num_frames} frames used, " + f"{est.num_rejected} rejected", + f" Boresight residual: median {a['median_deg']:.3f} deg, " + f"p90 {a['p90_deg']:.3f} deg", + ] + if alt is not None: + lines.append( + f" At {alt:.0f} m above the ENU origin that is " + f"{_ground_m(a['median_deg'], alt):.2f} m (median) / " + f"{_ground_m(a['p90_deg'], alt):.2f} m (p90) on the ground" + ) + if "best_offset_s" in a: + gain = a["median_deg"] - a["best_offset_median_deg"] + lines.append( + f" Time offset: residual minimized at {a['best_offset_s']*1e3:+.0f} ms " + f"(median {a['best_offset_median_deg']:.3f} deg, " + f"{gain:.3f} deg better than at 0 ms)" + ) + rms = a["axis_rms_deg"] + lines.append( + " Residual RMS by axis: " + + ", ".join(f"{lbl} {v:.3f} deg" for lbl, v in zip(_AXIS_LABELS, rms)) + ) + if rms[2] > 2 * max(rms[0], rms[1]): + lines.append( + " -> heading dominates: consistent with INS heading noise" + ) + lines.append( + f" Lever arm (INS frame, m): {np.round(est.lever_arm_ins, 3).tolist()}" + ) + return lines + + +_EXPLANATION = """\ +How to read this report + +The boresight solve treats every synchronized frame as an independent +measurement of the same rig-to-INS rotation. The residual is each +frame's disagreement with the averaged boresight. Because production +projects imagery through the INS pose plus this static mount, the +per-frame residual IS the pointing error to expect at runtime: +ground error = altitude x tan(residual). + +Time-offset sweep: a constant camera-to-INS clock offset turns attitude +rate into rotation error. The sweep re-solves the residual with the nav +attitude sampled at t + offset; a clear minimum away from 0 ms measures +the clock offset, while a flat curve rules timing out. + +Heading decomposition: each residual is split into rotations about the +INS body axes. Roll/pitch from the INS are usually good to a few +hundredths of a degree; heading is often several times worse. A heading +(body z) component that dominates the other two points at INS heading +noise rather than the camera calibration. + +Per-camera errors: the reprojection error measures how well each +camera's intrinsics + mount reproject the reconstruction's own 3D +points through the INS pose chain (median pixels over sampled images). +Its ground equivalent uses that camera's focal length at the flight +altitude; the boresight residual adds on top of it. +""" + + +def _summary_page(pdf: PdfPages, flight_name: str, analyses: Dict[str, Dict]): + fig = plt.figure(figsize=(8.5, 11)) + fig.text(0.08, 0.95, f"Calibration error report -- {flight_name}", + fontsize=15, weight="bold") + text: List[str] = [] + for name, a in analyses.items(): + text.extend(_summary_lines(name, a)) + text.append("") + fig.text(0.08, 0.90, "\n".join(text), fontsize=9, family="monospace", + va="top") + fig.text(0.08, 0.02, _EXPLANATION, fontsize=8, va="bottom") + pdf.savefig(fig) + plt.close(fig) + + +def _group_page(pdf: PdfPages, name: str, a: Dict): + if "components_deg" not in a: + return + fig = plt.figure(figsize=(8.5, 11)) + fig.suptitle(f"Boresight residual analysis -- group '{name}'", fontsize=13) + grid = fig.add_gridspec(2, 2, hspace=0.35, wspace=0.3) + + ax = fig.add_subplot(grid[0, :]) + t = a["inlier_times"] + minutes = (t - t.min()) / 60.0 + for i, lbl in enumerate(_AXIS_LABELS): + ax.plot(minutes, a["components_deg"][:, i], ".", ms=3, label=lbl) + ax.axhline(0, color="k", lw=0.5) + ax.set_xlabel("flight time (min)") + ax.set_ylabel("residual (deg)") + ax.set_title("Per-frame residual by INS body axis") + ax.legend(fontsize=8) + + ax = fig.add_subplot(grid[1, 0]) + ax.plot(a["offsets_s"] * 1e3, a["sweep_medians_deg"]) + ax.axvline(a["best_offset_s"] * 1e3, color="r", lw=0.8, + label=f"best {a['best_offset_s']*1e3:+.0f} ms") + ax.axvline(0, color="k", lw=0.5) + ax.set_xlabel("camera-to-INS time offset (ms)") + ax.set_ylabel("median residual (deg)") + ax.set_title("Time-offset sweep") + ax.legend(fontsize=8) + + ax = fig.add_subplot(grid[1, 1]) + ax.bar(range(3), a["axis_rms_deg"], color=["C0", "C1", "C2"]) + ax.set_xticks(range(3)) + ax.set_xticklabels(["roll", "pitch", "heading"], fontsize=9) + ax.set_ylabel("residual RMS (deg)") + ax.set_title("Heading vs tilt attribution") + + pdf.savefig(fig) + plt.close(fig) + + +def _camera_table_page(pdf: PdfPages, groups: Dict[str, Dict], + analyses: Dict[str, Dict]): + rows = [] + for name, group in groups.items(): + alt = analyses[name]["altitude_m"] + for folder, record in group.get("camera_records", {}).items(): + model = group.get("models", {}).get(folder) + reproj = record.get("reprojection_error_px") + ground = None + if reproj is not None and alt is not None and model is not None: + # one pixel subtends ~altitude/fx meters at nadir + ground = reproj * alt / float(model.fx) + ypr = Rotation.from_quat( + record["camera_quaternion_xyzw"] + ).as_euler("ZYX", degrees=True) + rows.append([ + folder, + name, + "yes" if record.get("is_reference") else "", + str(record.get("num_images", "")), + f"{reproj:.2f}" if reproj is not None else "n/a", + f"{ground:.2f}" if ground is not None else "n/a", + " ".join(f"{v:.2f}" for v in ypr), + ]) + if not rows: + return + fig = plt.figure(figsize=(11, 8.5)) + fig.suptitle("Per-camera errors", fontsize=13) + ax = fig.add_subplot(111) + ax.axis("off") + table = ax.table( + cellText=rows, + colLabels=["camera", "group", "ref", "images", + "reproj (px)", "ground (m)", "mount YPR (deg)"], + loc="upper center", + cellLoc="center", + ) + table.auto_set_font_size(False) + table.set_fontsize(8) + table.scale(1, 1.4) + fig.text( + 0.08, 0.06, + "reproj: median reprojection of the model's own 3D points through " + "the INS pose + mount, per camera.\nground: that error at the " + "flight altitude (reproj x altitude / fx); the group boresight " + "residual adds on top.", + fontsize=8, + ) + pdf.savefig(fig) + plt.close(fig) + + +def write_error_report( + save_dir: str | os.PathLike, + flight_name: str, + groups: Dict[str, Dict], + nav_state_provider, +) -> str: + """Write calibration_error_report.pdf for a finished run. + + `groups` maps group name to a `_calibrate_group` result dict + (estimate, rec, models, camera_records, ref_folder). + """ + analyses = { + name: _analyze_group(group, nav_state_provider) + for name, group in groups.items() + } + path = os.path.join(str(save_dir), "calibration_error_report.pdf") + with PdfPages(path) as pdf: + _summary_page(pdf, flight_name, analyses) + for name, a in analyses.items(): + _group_page(pdf, name, a) + _camera_table_page(pdf, groups, analyses) + return path diff --git a/kamera/postflight/flight_prep.py b/kamera/postflight/flight_prep.py new file mode 100644 index 0000000..97f6d42 --- /dev/null +++ b/kamera/postflight/flight_prep.py @@ -0,0 +1,253 @@ +"""Stage a raw KAMERA flight into the images0 layout the calibration +pipeline consumes. + +Raw flights store one folder per camera view (``center_view`` / +``left_view`` / ``right_view``), each holding every modality's images +plus ``*_meta.json``. The calibration pipeline instead wants one folder +per physical camera named ``__`` under +``/images0``, split by modality group (rgb+uv -> colmap_rgb, +ir -> colmap_ir) so SIFT never tries to match across the EO/IR gap. + +Every trigger event with a nav time is staged, as symlinks by default +so nothing is duplicated on disk; frame density is a flight-planning +concern, not something this pipeline second-guesses. + +IR is the exception to symlinking: its 16-bit imagery spans a sliver of +the sensor range, so SIFT finds nothing in the file as captured. IR +images are staged as contrast-stretched 8-bit copies instead (see +``stretch_to_uint8``). +""" + +import os +import pathlib +import shutil +from collections import defaultdict +from dataclasses import dataclass, field +from typing import Dict, List, Sequence, Tuple + +import cv2 +import numpy as np +from rich import print + +from kamera.postflight.naming import KameraImageName + +__all__ = [ + "TriggerFrame", + "discover_frames", + "find_raw_dir", + "modality_group", + "stage_flight", + "stretch_to_uint8", +] + +# EO modalities share a reconstruction; IR is on its own. +_MODALITY_GROUP = {"rgb": "rgb", "uv": "rgb", "ir": "ir"} + + +def modality_group(modality: str) -> str: + return _MODALITY_GROUP.get(modality, modality) + + +@dataclass +class TriggerFrame: + """All images captured at one synchronized trigger event.""" + + key: str # date_time + time: float # exposure time (s since epoch) + # camera folder name -> source image path + images: Dict[str, str] = field(default_factory=dict) + + +def _channel_word(view_dir: str) -> str: + """center_view -> center; also tolerates bare center/left/right.""" + base = os.path.basename(view_dir.rstrip("/")) + return base[: -len("_view")] if base.endswith("_view") else base + + +def _has_view_dirs(path: pathlib.Path) -> bool: + # both raw layouts occur in the wild: center_view/... and bare center/... + return any(path.glob("*_view")) or any( + (path / v).is_dir() for v in ("center", "left", "right") + ) + + +def find_raw_dir(flight_dir: str | os.PathLike) -> str: + """The raw imagery directory of a flight: `flight_dir` itself if it + holds the ``*_view`` folders, else the single immediate subdirectory + that does. Raises SystemError when none or several qualify (pass the + raw dir explicitly in that case).""" + flight = pathlib.Path(flight_dir) + if _has_view_dirs(flight): + return str(flight) + candidates = [d for d in flight.iterdir() if d.is_dir() and _has_view_dirs(d)] + if len(candidates) == 1: + return str(candidates[0]) + detail = ( + "none found" + if not candidates + else "several found: " + ", ".join(d.name for d in candidates) + ) + raise SystemError( + f"Could not identify the raw imagery dir under {flight_dir} " + f"(a folder containing *_view subdirectories): {detail}." + ) + + +def discover_frames( + raw_dir: str | os.PathLike, + prefix: str, + image_exts: Tuple[str, ...] = (".jpg", ".jpeg", ".png", ".tif", ".tiff"), +) -> List[TriggerFrame]: + """Group every image under `raw_dir`'s view folders into synchronized + trigger frames; frames without a meta-json exposure time are dropped + (no INS state means no prior or boresight downstream). + + The camera folder for each image is ``__``, + where channel comes from the containing view folder. + """ + frames: Dict[str, TriggerFrame] = {} + for path in pathlib.Path(raw_dir).rglob("*"): + if path.suffix.lower() not in image_exts: + continue + try: + name = KameraImageName.parse(path.name) + except ValueError: + continue + channel = _channel_word(str(path.parent)) + camera_folder = f"{prefix}_{channel}_{name.modality}" + key = f"{name.date}_{name.time}" + if key not in frames: + frames[key] = TriggerFrame(key=key, time=0.0) + frames[key].images[camera_folder] = str(path) + + times = _basename_times(raw_dir) + out: List[TriggerFrame] = [] + for key, frame in frames.items(): + # any image's base name resolves the trigger's exposure time + sample = next(iter(frame.images.values())) + base = KameraImageName.parse(os.path.basename(sample)).base_name + t = times.get(base) + if t is None: + continue + frame.time = t + out.append(frame) + out.sort(key=lambda f: f.time) + return out + + +def _basename_times(flight_dir: str | os.PathLike) -> Dict[str, float]: + import json + + out: Dict[str, float] = {} + for meta in pathlib.Path(flight_dir).rglob("*_meta.json"): + try: + with open(meta) as f: + d = json.load(f) + out[KameraImageName.parse(meta).base_name] = float(d["evt"]["time"]) + except (OSError, ValueError, KeyError): + continue + return out + + +def stretch_to_uint8( + img: np.ndarray, low_pct: float = 1.0, top_px: int = 30 +) -> np.ndarray: + """An 8-bit contrast stretch mapping [the ``low_pct`` percentile, + the value of the ``top_px``-th brightest pixel] onto [0, 255]. + + The high bound is count-based, not a percentile: the warm targets + these surveys care about (a seal on cold ice) can be a few dozen + pixels, far below any workable percentile, so a percentile top + either lumps them in with thousands of background pixels or lets a + single hot pixel set the range. + """ + if img.ndim == 3: + img = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) + flat = img.ravel() + k = int(np.clip(top_px, 1, flat.size)) + lo = float(np.percentile(flat, low_pct)) + hi = float(np.partition(flat, flat.size - k)[flat.size - k]) + if hi <= lo: # (near-)constant image + lo = float(flat.min()) + hi = max(float(flat.max()), lo + 1) + out = (img.astype(np.float32) - lo) * (255.0 / (hi - lo)) + return np.clip(np.rint(out), 0, 255).astype(np.uint8) + + +def _stage_stretched(src: str, dst: str, low_pct: float, top_px: int) -> None: + """Stage src at dst as a stretched 8-bit copy, atomically. An + existing regular file is trusted -- a crashed write never leaves one + -- so restretching with new parameters requires removing the staged + camera folders first.""" + if os.path.isfile(dst) and not os.path.islink(dst): + return + if os.path.lexists(dst): + os.remove(dst) # e.g. a symlink staged before stretching existed + img = cv2.imread(src, cv2.IMREAD_UNCHANGED) + if img is None: + raise SystemError(f"Could not read image {src}.") + stretched = stretch_to_uint8(img, low_pct, top_px) + tmp = f"{dst}.tmp{os.path.splitext(dst)[1]}" + if not cv2.imwrite(tmp, stretched): + raise SystemError(f"Could not write {tmp}.") + os.replace(tmp, dst) + + +def stage_flight( + raw_dir: str | os.PathLike, + flight_dir: str | os.PathLike, + prefix: str, + copy: bool = False, + stretch_modalities: Sequence[str] = ("ir",), + stretch_low_pct: float = 1.0, + stretch_top_px: int = 30, +) -> Dict[str, int]: + """Discover and stage every trigger frame into per-modality-group + ``/colmap_/images0//`` trees. + + Modalities in `stretch_modalities` are written as contrast-stretched + 8-bit copies (never symlinks; see ``stretch_to_uint8``) so feature + extraction sees usable contrast. + + Returns a map of camera folder -> number of images staged. + """ + frames = discover_frames(raw_dir, prefix) + if not frames: + raise SystemError(f"No parseable images found under {raw_dir}.") + print(f"Staging {len(frames)} triggers.") + + counts: Dict[str, int] = defaultdict(int) + for frame in frames: + for camera_folder, src in frame.images.items(): + modality = camera_folder.rsplit("_", 1)[-1] + group = modality_group(modality) + dst_dir = os.path.join( + str(flight_dir), f"colmap_{group}", "images0", camera_folder + ) + os.makedirs(dst_dir, exist_ok=True) + dst = os.path.join(dst_dir, os.path.basename(src)) + src_abs = os.path.abspath(src) + if modality in stretch_modalities: + _stage_stretched(src_abs, dst, stretch_low_pct, stretch_top_px) + elif not _staged_ok(src_abs, dst, copy): + if os.path.lexists(dst): + os.remove(dst) + if copy: + shutil.copyfile(src, dst) + else: + os.symlink(src_abs, dst) + counts[camera_folder] += 1 + return dict(counts) + + +def _staged_ok(src: str, dst: str, copy: bool) -> bool: + """Whether dst already stages src -- a symlink pointing at it, or for + copies a regular file of the same size (a mid-copy crash leaves a + shorter one). Lets restaging skip completed entries.""" + if copy: + return ( + os.path.isfile(dst) + and not os.path.islink(dst) + and os.path.getsize(dst) == os.path.getsize(src) + ) + return os.path.islink(dst) and os.readlink(dst) == src diff --git a/kamera/postflight/fusion.py b/kamera/postflight/fusion.py new file mode 100644 index 0000000..bf1b9bb --- /dev/null +++ b/kamera/postflight/fusion.py @@ -0,0 +1,755 @@ +"""Fuse IR images into the EO reconstruction with deep cross-modal matches. + +The per-group pipeline leaves EO and IR as two independent ENU models +whose mutual consistency comes only from both boresights referencing the +same INS -- an INS-attitude-noise-limited link. This module registers +every IR image directly into the EO model instead (MINIMA-LoFTR against +each image's co-located EO partner, pre-warped into the IR view so the +matcher faces only the modality gap), then re-derives the mounts from +that single multimodal reconstruction. `fuse_ir_into_eo` orchestrates; +the per-stage functions carry their own contracts. + +Two constraints shape the design: + +- Matched EO pixels are lifted to 3D at the interpolated depth of the + EO image's own triangulated points, not snapped to them -- EO images + carry only a few hundred SIFT points, far too sparse for thousands of + dense matches. For co-located cameras a depth error moves the point + along the EO ray, which barely changes its bearing from the IR + camera, so the recovered rotation is insensitive to the interpolation. +- Per-image PnP is only an outlier filter, never the pose: over + near-planar terrain a single image has a strong tilt/translation + ambiguity (degrees of rotation wobble). All inlier correspondences + jointly solve one Sim3 aligning the IR reconstruction to the EO + reconstruction (`align_ir_to_eo`), so every fused pose inherits the + IR model's own multi-view relative geometry. + +The pre-warp uses the two-boresight mounts only as an initialization (a +0.27 deg mount error is ~4 IR px of warp offset, well within matcher +tolerance); the fused geometry comes entirely from the matches. +""" + +import bisect +import functools +import os +from collections import defaultdict +from dataclasses import dataclass +from typing import Dict, List, Optional, Tuple + +import cv2 +import numpy as np +import pycolmap +from rich import print +from scipy.optimize import least_squares +from scipy.spatial import cKDTree +from scipy.spatial.transform import Rotation + +from kamera.colmap_processing.camera_models import StandardCamera +from kamera.postflight.boresight import _folder_camera +from kamera.postflight.deep_match import to_uint8 +from kamera.postflight.naming import KameraCameraName, KameraImageName +from kamera.postflight.registration_homography import pixel_homography +from kamera.postflight.rig import _frame_key + +__all__ = [ + "EoPartner", + "IrMatchResult", + "align_ir_to_eo", + "eo_partners", + "fuse_ir_into_eo", + "insert_ir_image", + "lift_eo_pixels", + "match_ir_image", + "mount_delta_deg", + "pnp_ir_image", + "prewarp_eo", + "refine_fused", + "snap_index", +] + + +def prewarp_eo( + eo_img: np.ndarray, + h_eo2ir: np.ndarray, + ir_size: Tuple[int, int], + scale: float = 1.0, +) -> Tuple[np.ndarray, np.ndarray]: + """Warp a full-res EO image into the IR view. + + Returns (warped, H) where warped is `scale` x the IR size and H maps + a full-res EO pixel to a warped pixel; a matched warped pixel maps + back to full-res EO coordinates exactly through inv(H). + + The ~20x EO->IR downscale would alias badly through warpPerspective + alone, so the image is first area-resized close to the target scale + (keeping ~2x oversampling) and the homography adjusted for OpenCV's + half-pixel-center resize convention. + """ + w_ir, h_ir = ir_size + out_w, out_h = round(w_ir * scale), round(h_ir * scale) + H = np.diag([scale, scale, 1.0]) @ h_eo2ir + + corners = np.array( + [[0, 0], [out_w - 1, 0], [out_w - 1, out_h - 1], [0, out_h - 1]], + dtype=np.float64, + ) + eo_corners = cv2.perspectiveTransform(corners[None], np.linalg.inv(H))[0] + span = max(np.ptp(eo_corners[:, 0]), np.ptp(eo_corners[:, 1])) + r = min(1.0, 2.0 * max(out_w, out_h) / max(span, 1.0)) + if r < 1.0: + small = cv2.resize(eo_img, None, fx=r, fy=r, interpolation=cv2.INTER_AREA) + # OpenCV resize maps src pixel x to r*x + (r-1)/2 (pixel centers). + A = np.array([[r, 0, (r - 1) / 2], [0, r, (r - 1) / 2], [0, 0, 1.0]]) + warped = cv2.warpPerspective(small, H @ np.linalg.inv(A), (out_w, out_h)) + else: + warped = cv2.warpPerspective(eo_img, H, (out_w, out_h)) + return warped, H + + +INVALID_POINT3D = np.uint64(np.iinfo(np.uint64).max) # pycolmap's invalid id + + +@dataclass +class _SnapIndex: + """KD-tree over an EO image's triangulated observations.""" + + tree: cKDTree + point3D_ids: np.ndarray + xyz: np.ndarray # [K, 3] world coordinates of the anchors + + +def snap_index( + image: "pycolmap.Image", + reconstruction: "pycolmap.Reconstruction", + min_points: int = 25, +) -> Optional[_SnapIndex]: + """Index of the image's points2D that carry a 3D point: the depth + anchors for `lift_eo_pixels` and the track targets for snapping. + None if too few to be useful.""" + xy, pids, xyz = [], [], [] + for p2 in image.points2D: + if p2.has_point3D(): + xy.append(p2.xy) + pids.append(p2.point3D_id) + xyz.append(reconstruction.points3D[p2.point3D_id].xyz) + if len(xy) < min_points: + return None + return _SnapIndex( + cKDTree(np.asarray(xy)), + np.asarray(pids, dtype=np.uint64), + np.asarray(xyz), + ) + + +def lift_eo_pixels( + camera: "pycolmap.Camera", + image: "pycolmap.Image", + snap: _SnapIndex, + eo_px: np.ndarray, + k: int = 4, + max_anchor_px: float = 600.0, +) -> Tuple[np.ndarray, np.ndarray]: + """World points for EO pixels, by casting each pixel's ray from the + image's reconstructed pose to the median camera-frame depth of its + `k` nearest triangulated observations. + + Returns (xyz [N,3], valid [N]); valid is False where the nearest + depth anchor is further than `max_anchor_px` (depth would be a + guess).""" + cfw = image.cam_from_world() + R, t = cfw.rotation.matrix(), cfw.translation + anchor_depth = (snap.xyz @ R.T + t)[:, 2] + + kk = min(k, len(snap.point3D_ids)) + dist, idx = snap.tree.query(eo_px, k=kk) + if kk == 1: + dist, idx = dist[:, None], idx[:, None] + depth = np.median(anchor_depth[idx], axis=1) + valid = dist[:, 0] <= max_anchor_px + + rays = camera.cam_from_img(np.asarray(eo_px, dtype=np.float64)) + xyz_cam = np.column_stack([rays * depth[:, None], depth]) + return (xyz_cam - t) @ R, valid + + +def eo_partners( + ir_images: Dict[int, str], + eo_images: Dict[int, str], + pairs_per_ir: int = 1, + ref_modality: str = "rgb", + max_dt_s: float = 15.0, +) -> Dict[int, List[int]]: + """EO partner image ids for each IR image, both given as + {image_id: colmap name (folder/basename)}. + + Partners come from the co-located station (the IR camera folder with + its modality swapped to `ref_modality`), keyed by trigger frame_key + -- never by rebuilding filenames, whose extensions differ across + modalities. The first partner is the temporally closest trigger + (the exact same-trigger image when it is registered); `pairs_per_ir` + walks outward to neighboring triggers. Candidates further than + `max_dt_s` seconds are dropped: past a few triggers the platform has + moved so far that the views no longer overlap (an IR trigger whose + EO images all fell out of the reconstruction gets no partner). + """ + eo_by_folder: Dict[str, Dict[str, int]] = defaultdict(dict) + for iid, name in eo_images.items(): + folder, _, base = name.rpartition("/") + try: + eo_by_folder[folder][_frame_key(base)] = iid + except ValueError: + continue + keys_by_folder = {f: sorted(d) for f, d in eo_by_folder.items()} + + out: Dict[int, List[int]] = {} + for iid, name in ir_images.items(): + folder, _, base = name.rpartition("/") + try: + eo_folder = KameraCameraName.parse(folder).with_modality(ref_modality).name + key = _frame_key(base) + except ValueError: + continue + keys = keys_by_folder.get(eo_folder) + if not keys: + continue + # walk outward from the insertion point, temporally nearest first + pos = bisect.bisect_left(keys, key) + picked: List[int] = [] + lo, hi = pos - 1, pos + while len(picked) < pairs_per_ir and (lo >= 0 or hi < len(keys)): + if hi < len(keys) and (lo < 0 or _key_dist(keys[hi], key) <= _key_dist(keys[lo], key)): + candidate, hi = keys[hi], hi + 1 + else: + candidate, lo = keys[lo], lo - 1 + if _key_dist(candidate, key) > max_dt_s: + break # walking outward only gets further away + picked.append(eo_by_folder[eo_folder][candidate]) + if picked: + out[iid] = picked + return out + + +def _key_dist(a: str, b: str) -> float: + """Temporal distance between two date_time frame keys, seconds.""" + + def parse(k: str) -> float: + date, _, t = k.partition("_") + return float(date) * 86400 + float(t) + + try: + return abs(parse(a) - parse(b)) + except ValueError: + return float("inf") + + +@dataclass +class EoPartner: + """One EO image prepared for matching against an IR image.""" + + img: np.ndarray # grayscale uint8, full resolution + time: float + model: StandardCamera # exported mount model, for the warp + image: "pycolmap.Image" # reconstructed pose, for the depth lift + camera: "pycolmap.Camera" + snap: _SnapIndex + + +@dataclass +class IrMatchResult: + """2D(IR) <-> 3D correspondences for one IR image. + + Every row has a depth-lifted 3D point (for PnP); rows that also + landed within `snap_px` of a triangulated observation carry that + point's id in `point3D_ids` (else INVALID_POINT3D) with the snap + distance in `snap_dist` -- those become track observations. + """ + + ir_xy: np.ndarray # [N, 2] + xyz: np.ndarray # [N, 3] + point3D_ids: np.ndarray # [N] uint64, INVALID_POINT3D where unsnapped + snap_dist: np.ndarray # [N] + num_raw: int = 0 + + +def _concat(rows: List[np.ndarray], empty_shape: Tuple, dtype="float64") -> np.ndarray: + return np.concatenate(rows) if rows else np.empty(empty_shape, dtype=dtype) + + +def match_ir_image( + ir_img: np.ndarray, + ir_model: StandardCamera, + t_ir: float, + partners: List[EoPartner], + matcher, + ground_z: float, + snap_px: float = 16.0, + warp_scale: float = 1.0, +) -> IrMatchResult: + """Match one IR image against its EO partners. + + Per partner: homography pre-warp -> deep match -> map EO keypoints + back to full resolution -> lift to 3D at interpolated depth (see + `lift_eo_pixels`), tagging the matches within `snap_px` (full-res EO + px) of a triangulated observation with that observation's point id. + """ + rows_xy, rows_xyz, rows_pid, rows_dist = [], [], [], [] + num_raw = 0 + ir_u8 = to_uint8(ir_img) + h, w = ir_u8.shape[:2] + for p in partners: + try: + H = pixel_homography(p.model, ir_model, p.time, t_ir, ground_z) + except ValueError: + continue # views no longer overlap (e.g. turn between triggers) + warped, H_used = prewarp_eo(p.img, H, (w, h), warp_scale) + eo_kpts, ir_kpts = matcher.match(warped, ir_u8) + num_raw += len(eo_kpts) + if not len(eo_kpts): + continue + eo_px = cv2.perspectiveTransform( + eo_kpts[None], np.linalg.inv(H_used) + )[0] + xyz, valid = lift_eo_pixels(p.camera, p.image, p.snap, eo_px) + dist, idx = p.snap.tree.query(eo_px) + pid = np.where( + dist <= snap_px, p.snap.point3D_ids[idx], INVALID_POINT3D + ) + rows_xy.append(ir_kpts[valid]) + rows_xyz.append(xyz[valid]) + rows_pid.append(pid[valid]) + rows_dist.append(dist[valid]) + + return IrMatchResult( + _concat(rows_xy, (0, 2)), + _concat(rows_xyz, (0, 3)), + _concat(rows_pid, (0,), "uint64"), + _concat(rows_dist, (0,)), + num_raw, + ) + + +def pnp_ir_image( + result: IrMatchResult, + ir_camera: "pycolmap.Camera", + ransac_px: float = 3.0, + min_inliers: int = 12, +) -> Optional[Tuple["pycolmap.Rigid3d", np.ndarray]]: + """PnP+RANSAC an IR image into the EO/ENU frame from its depth-lifted + 3D points. Distortion is handled by the passed pycolmap Camera; raw + IR pixels go in directly. Returns (cam_from_world, inlier_mask) or + None when registration is unreliable.""" + if len(result.ir_xy) < min_inliers: + return None + est = pycolmap.AbsolutePoseEstimationOptions() + est.ransac.max_error = ransac_px + ans = pycolmap.estimate_and_refine_absolute_pose( + result.ir_xy, result.xyz, ir_camera, est, + pycolmap.AbsolutePoseRefinementOptions(), + ) + if ans is None or ans["num_inliers"] < min_inliers: + return None + return ans["cam_from_world"], np.asarray(ans["inlier_mask"], dtype=bool) + + +@dataclass +class AlignmentEntry: + """One IR image's contribution to the model alignment: its pose in + the IR reconstruction and its PnP-inlier cross-modal correspondences.""" + + cam_from_irworld: "pycolmap.Rigid3d" + camera: "pycolmap.Camera" + ir_xy: np.ndarray # [M, 2] inlier IR pixels + xyz_eo: np.ndarray # [M, 3] their EO-model 3D points + + +def align_ir_to_eo( + entries: List[AlignmentEntry], + cap_per_image: int = 500, + f_scale_px: float = 2.0, +) -> Tuple[Rotation, np.ndarray, float, Dict]: + """Solve the Sim3 (irworld <- eoworld) that best reprojects every + EO 3D point into every IR image through the IR reconstruction's own + poses: X_ir = s * R @ X_eo + t. + + Both reconstructions are prior-mapped into the same ENU frame, so + the transform starts at identity and stays small; solving it jointly + over all images defeats the per-image planar-PnP tilt ambiguity. + Returns (R, t, s, stats).""" + poses, cams, pxs, xyzs = [], [], [], [] + rng = np.random.default_rng(0) + for e in entries: + n = len(e.ir_xy) + sel = ( + rng.choice(n, cap_per_image, replace=False) + if n > cap_per_image + else slice(None) + ) + poses.append( + (e.cam_from_irworld.rotation.matrix(), e.cam_from_irworld.translation) + ) + cams.append(e.camera) + pxs.append(np.asarray(e.ir_xy[sel], dtype=np.float64)) + xyzs.append(np.asarray(e.xyz_eo[sel], dtype=np.float64)) + + def residuals(p): + R = Rotation.from_rotvec(p[:3]).as_matrix() + t, s = p[3:6], np.exp(p[6]) + out = [] + for (Ri, ti), cam, px, xyz in zip(poses, cams, pxs, xyzs): + xc = (s * xyz @ R.T + t) @ Ri.T + ti + norm = np.column_stack( + [xc[:, 0] / xc[:, 2], xc[:, 1] / xc[:, 2], np.ones(len(xc))] + ) + out.append((cam.img_from_cam(norm) - px).ravel()) + return np.concatenate(out) + + fit = least_squares( + residuals, np.zeros(7), loss="soft_l1", f_scale=f_scale_px, x_scale="jac" + ) + R = Rotation.from_rotvec(fit.x[:3]) + t, s = fit.x[3:6], float(np.exp(fit.x[6])) + res = residuals(fit.x).reshape(-1, 2) + err = np.linalg.norm(res, axis=1) + stats = { + "num_images": len(entries), + "num_correspondences": int(len(err)), + "rotation_deg": float(np.degrees(R.magnitude())), + "translation_m": [float(x) for x in t], + "scale": s, + "reproj_px_median": float(np.median(err)), + "reproj_px_p90": float(np.percentile(err, 90)), + } + print( + f"IR->EO model alignment over {stats['num_images']} images / " + f"{stats['num_correspondences']} matches: rot {stats['rotation_deg']:.3f} deg, " + f"trans {np.round(t, 2)} m, scale {s:.5f}, " + f"reproj median {stats['reproj_px_median']:.2f} px." + ) + return R, t, s, stats + + +def aligned_ir_pose( + cam_from_irworld: "pycolmap.Rigid3d", + R: Rotation, + t: np.ndarray, + s: float, +) -> "pycolmap.Rigid3d": + """The IR image's pose in the EO world, given the Sim3 from + `align_ir_to_eo`. The scale is absorbed into the translation so the + projection is unchanged (X_cam and s*X_cam project identically).""" + Ri = cam_from_irworld.rotation.matrix() + ti = cam_from_irworld.translation + rot = Rotation.from_matrix(Ri @ R.as_matrix()) + return pycolmap.Rigid3d( + pycolmap.Rotation3d(rot.as_quat()), (Ri @ t + ti) / s + ) + + +def _fresh_image_id(rec: "pycolmap.Reconstruction") -> int: + # Trivial frames share their image's id, so stay clear of both. + return max(max(rec.images, default=0), max(rec.frames, default=0)) + 1 + + +def add_ir_camera( + rec: "pycolmap.Reconstruction", camera: "pycolmap.Camera" +) -> int: + """Copy an IR camera (from the IR group model) into the EO + reconstruction under a fresh id, with its trivial rig.""" + cid = max(rec.cameras, default=0) + 1 + rec.add_camera_with_trivial_rig( + pycolmap.Camera( + model=camera.model.name, + width=camera.width, + height=camera.height, + params=camera.params, + camera_id=cid, + ) + ) + return cid + + +def insert_ir_image( + rec: "pycolmap.Reconstruction", + camera_id: int, + name: str, + result: IrMatchResult, + inlier_mask: np.ndarray, + cam_from_world: "pycolmap.Rigid3d", +) -> Tuple[int, int]: + """Add a posed IR image; its PnP-inlier keypoints that snapped onto + an existing EO 3D point become observations of that point, extending + the track across the modality gap (at most one observation per point + -- COLMAP's track invariant -- keeping the smallest snap distance). + Returns (image id, number of observations added).""" + image_id = _fresh_image_id(rec) + image = pycolmap.Image( + name=name, + keypoints=np.asarray(result.ir_xy, dtype=np.float64), + camera_id=camera_id, + image_id=image_id, + ) + rec.add_image_with_trivial_frame(image, cam_from_world) + best: Dict[int, Tuple[float, int]] = {} + for idx, (pid, d, ok) in enumerate( + zip(result.point3D_ids, result.snap_dist, inlier_mask) + ): + if ok and pid != INVALID_POINT3D: + key = int(pid) + if key not in best or d < best[key][0]: + best[key] = (float(d), idx) + for pid, (_, idx) in best.items(): + rec.add_observation(pid, pycolmap.TrackElement(image_id, idx)) + return image_id, len(best) + + +def refine_fused( + rec: "pycolmap.Reconstruction", + ir_image_ids: List[int], + refine_ir_intrinsics: bool = False, + ir_camera_ids: Tuple[int, ...] = (), +) -> "pycolmap.BundleAdjuster": + """Bundle-adjust the fused model with the EO side frozen. + + EO frame poses and all intrinsics are held constant (IR intrinsics + optionally free); 3D points and IR poses refine, so the multimodal + tracks pull the IR registrations against the full EO structure + without touching the EO gauge or geometry. + """ + options = pycolmap.BundleAdjustmentOptions() + options.refine_principal_point = False + options.refine_focal_length = refine_ir_intrinsics + options.refine_extra_params = refine_ir_intrinsics + options.print_summary = False + + config = pycolmap.BundleAdjustmentConfig() + ir_ids = set(ir_image_ids) + ir_cams = set(ir_camera_ids) + for iid, im in rec.images.items(): + if not im.has_pose: + continue + if iid in ir_ids and im.num_points3D < 6: + continue # too few observations to constrain; PnP pose stands + config.add_image(iid) + if iid not in ir_ids: + config.set_constant_rig_from_world_pose(im.frame_id) + for cid in rec.cameras: + if not (refine_ir_intrinsics and cid in ir_cams): + config.set_constant_cam_intrinsics(cid) + + adjuster = pycolmap.create_default_bundle_adjuster(options, config, rec) + summary = adjuster.solve() + print( + f"Fused bundle adjustment: {summary.termination_type}, " + f"{summary.num_residuals} residuals." + ) + return adjuster + + +def mount_delta_deg(a: StandardCamera, b: StandardCamera) -> float: + """Angle between two exported camera mounts, degrees.""" + ra = Rotation.from_quat(np.asarray(a.cam_quat)) + rb = Rotation.from_quat(np.asarray(b.cam_quat)) + return float(np.degrees((ra * rb.inv()).magnitude())) + + +def _load_gray(path: str) -> Optional[np.ndarray]: + img = cv2.imread(path, cv2.IMREAD_UNCHANGED) + return None if img is None else to_uint8(img) + + +def fuse_ir_into_eo( + eo_rec: "pycolmap.Reconstruction", + ir_rec: "pycolmap.Reconstruction", + models: Dict[str, StandardCamera], + image_dirs: Dict[str, str], + times: Dict[str, float], + matcher, + pairs_per_ir: int = 1, + max_dt_s: float = 15.0, + snap_px: float = 16.0, + ransac_px: float = 3.0, + min_inliers: int = 12, + warp_scale: float = 1.0, + run_ba: bool = False, + refine_ir_intrinsics: bool = False, + max_images: Optional[int] = None, +) -> Dict: + """Register the IR group's images into the EO reconstruction + (mutating `eo_rec` into the fused model) and return a report. + + `models` are the per-group exported StandardCameras by camera folder + (both the warp initialization and the IR intrinsics come from that + stage); `image_dirs` maps folders to their images0 directories; + `times` maps base names to exposure times. `max_images` uniformly + subsamples the IR images for smoke runs. + + `run_ba` is off by default: the fused poses come from the joint + model alignment, and per-image bundle adjustment against the sparse + multimodal track observations would re-introduce the very per-image + tilt wobble the alignment defeats. `refine_ir_intrinsics` implies it. + """ + run_ba = run_ba or refine_ir_intrinsics + ground_z = float( + np.median([p.xyz[2] for p in eo_rec.points3D.values()]) + ) + print(f"Fusion ground plane z = {ground_z:.1f} m (median EO point).") + + ir_images = { + iid: im.name + for iid, im in ir_rec.images.items() + if im.has_pose and "/" in im.name + } + eo_images = { + iid: im.name + for iid, im in eo_rec.images.items() + if im.has_pose and "/" in im.name + } + partners_by_ir = eo_partners( + ir_images, eo_images, pairs_per_ir, max_dt_s=max_dt_s + ) + + ordered = sorted(ir_images, key=lambda i: ir_images[i]) + if max_images is not None and len(ordered) > max_images: + step = len(ordered) / max_images + ordered = [ordered[int(i * step)] for i in range(max_images)] + + ir_folder_camera = _folder_camera(ir_rec) + camera_ids: Dict[str, int] = {} + + @functools.lru_cache(maxsize=None) + def _snap(eo_id: int) -> Optional[_SnapIndex]: + return snap_index(eo_rec.images[eo_id], eo_rec) + + @functools.lru_cache(maxsize=16) # full-res EO images are large + def _gray(eo_id: int) -> Optional[np.ndarray]: + folder, _, base = eo_images[eo_id].rpartition("/") + return _load_gray(os.path.join(image_dirs[folder], base)) + + def eo_partner(eo_id: int) -> Optional[EoPartner]: + name = eo_images[eo_id] + folder, _, base = name.rpartition("/") + if folder not in models or folder not in image_dirs: + return None + try: + t = times[KameraImageName.parse(base).base_name] + except (ValueError, KeyError): + return None + snap, img = _snap(eo_id), _gray(eo_id) + if snap is None or img is None: + return None + image = eo_rec.images[eo_id] + return EoPartner( + img, t, models[folder], image, eo_rec.cameras[image.camera_id], snap + ) + + skipped = defaultdict(int) + matched = [] # (name, folder, result, inlier_mask, alignment_entry) + per_image: List[dict] = [] + inlier_counts: Dict[str, List[int]] = defaultdict(list) + for n, ir_id in enumerate(ordered, 1): + name = ir_images[ir_id] + folder, _, base = name.rpartition("/") + partners = [ + p + for p in map(eo_partner, partners_by_ir.get(ir_id, [])) + if p is not None + ] + if not partners: + skipped["no_partner"] += 1 + continue + if folder not in models or folder not in image_dirs: + skipped["no_ir_model"] += 1 + continue + try: + t_ir = times[KameraImageName.parse(base).base_name] + except (ValueError, KeyError): + skipped["no_time"] += 1 + continue + ir_img = cv2.imread( + os.path.join(image_dirs[folder], base), cv2.IMREAD_UNCHANGED + ) + if ir_img is None: + skipped["unreadable"] += 1 + continue + + result = match_ir_image( + ir_img, models[folder], t_ir, partners, matcher, ground_z, + snap_px=snap_px, warp_scale=warp_scale, + ) + # PnP filters outliers and gates reliability; the pose itself + # comes from the joint model alignment below. + pnp = pnp_ir_image( + result, ir_folder_camera[folder], + ransac_px=ransac_px, min_inliers=min_inliers, + ) + if pnp is None: + skipped["pnp_failed" if len(result.ir_xy) >= min_inliers else "few_matches"] += 1 + continue + _, inlier_mask = pnp + entry = AlignmentEntry( + ir_rec.images[ir_id].cam_from_world(), + ir_folder_camera[folder], + result.ir_xy[inlier_mask], + result.xyz[inlier_mask], + ) + result.xyz = np.empty((0, 3)) # the entry holds the inlier copy + matched.append((name, folder, result, inlier_mask, entry)) + inlier_counts[folder].append(int(inlier_mask.sum())) + if n % 25 == 0 or n == len(ordered): + print( + f" [{n}/{len(ordered)}] matched {len(matched)}, " + f"skipped {sum(skipped.values())}" + ) + + fused_ids: List[int] = [] + align_stats = None + if matched: + R, t, s, align_stats = align_ir_to_eo([m[4] for m in matched]) + for name, folder, result, inlier_mask, entry in matched: + pose = aligned_ir_pose(entry.cam_from_irworld, R, t, s) + if folder not in camera_ids: + camera_ids[folder] = add_ir_camera( + eo_rec, ir_folder_camera[folder] + ) + image_id, num_obs = insert_ir_image( + eo_rec, camera_ids[folder], name, result, inlier_mask, pose + ) + fused_ids.append(image_id) + per_image.append( + { + "name": name, + "num_raw": result.num_raw, + "num_lifted": len(result.ir_xy), + "num_inliers": int(inlier_mask.sum()), + "num_track_observations": num_obs, + } + ) + + if fused_ids and run_ba: + refine_fused( + eo_rec, fused_ids, + refine_ir_intrinsics=refine_ir_intrinsics, + ir_camera_ids=tuple(camera_ids.values()), + ) + + report = { + "matcher": getattr(matcher, "name", str(matcher)), + "num_ir_candidates": len(ordered), + "num_fused": len(fused_ids), + "skipped": dict(skipped), + "model_alignment": align_stats, + "bundle_adjusted": bool(fused_ids and run_ba), + "per_folder": { + f: { + "fused": len(c), + "median_inliers": float(np.median(c)), + } + for f, c in sorted(inlier_counts.items()) + }, + "per_image": per_image, + } + print( + f"Fused {report['num_fused']}/{report['num_ir_candidates']} IR images " + f"into the EO model (skipped: {report['skipped'] or 'none'})." + ) + return report diff --git a/kamera/postflight/naming.py b/kamera/postflight/naming.py new file mode 100644 index 0000000..81c3c52 --- /dev/null +++ b/kamera/postflight/naming.py @@ -0,0 +1,151 @@ +"""Parsing helpers for KAMERA file and directory naming conventions. + +Image basenames look like:: + + ____