commit fc66f02c9c76b6cf5c9664cc4af717af70a0b421 Author: sorlinv Date: Mon Jun 15 14:06:42 2026 +0200 Initial commit: pure-Rust COLMAP port diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..ea8c4bf --- /dev/null +++ b/.gitignore @@ -0,0 +1 @@ +/target diff --git a/Cargo.lock b/Cargo.lock new file mode 100644 index 0000000..6e920a7 --- /dev/null +++ b/Cargo.lock @@ -0,0 +1,772 @@ +# This file is automatically @generated by Cargo. +# It is not intended for manual editing. +version = 3 + +[[package]] +name = "ahash" +version = "0.8.12" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "5a15f179cd60c4584b8a8c596927aadc462e27f2ca70c04e0071964a73ba7a75" +dependencies = [ + "cfg-if", + "once_cell", + "version_check", + "zerocopy", +] + +[[package]] +name = "anyhow" +version = "1.0.102" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7f202df86484c868dbad7eaa557ef785d5c66295e41b460ef922eca0723b842c" + +[[package]] +name = "approx" +version = "0.5.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "cab112f0a86d568ea0e627cc1d6be74a1e9cd55214684db5561995f6dad897c6" +dependencies = [ + "num-traits", +] + +[[package]] +name = "autocfg" +version = "1.5.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f2032f911046de80f0a198e0901378627c33f59ea0ac00e363d481118bd70a53" + +[[package]] +name = "bitflags" +version = "2.13.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b4388bee8683e3d04af747c73422af53102d2bd24d9eadb6cbc100baef4b43f8" + +[[package]] +name = "bytemuck" +version = "1.25.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "c8efb64bd706a16a1bdde310ae86b351e4d21550d98d056f22f8a7f7a2183fec" + +[[package]] +name = "byteorder" +version = "1.5.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1fd0f2584146f6f2ef48085050886acf353beff7305ebd1ae69500e27c67f64b" + +[[package]] +name = "cc" +version = "1.2.64" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "dad887fd958be91b5098c0248def011f4523ab786cd411be668777e55063501f" +dependencies = [ + "find-msvc-tools", + "shlex", +] + +[[package]] +name = "cfg-if" +version = "1.0.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9330f8b2ff13f34540b44e946ef35111825727b38d33286ef986142615121801" + +[[package]] +name = "colmap" +version = "0.1.0" +dependencies = [ + "approx", + "byteorder", + "nalgebra", + "rusqlite", + "serde", + "tempfile", + "thiserror", +] + +[[package]] +name = "equivalent" +version = "1.0.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "877a4ace8713b0bcf2a4e7eec82529c029f1d0619886d18145fea96c3ffe5c0f" + +[[package]] +name = "errno" +version = "0.3.14" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "39cab71617ae0d63f51a36d69f866391735b51691dbda63cf6f96d042b63efeb" +dependencies = [ + "libc", + "windows-sys", +] + +[[package]] +name = "fallible-iterator" +version = "0.3.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "2acce4a10f12dc2fb14a218589d4f1f62ef011b2d0cc4b3cb1bba8e94da14649" + +[[package]] +name = "fallible-streaming-iterator" +version = "0.1.9" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7360491ce676a36bf9bb3c56c1aa791658183a54d2744120f27285738d90465a" + +[[package]] +name = "fastrand" +version = "2.4.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9f1f227452a390804cdb637b74a86990f2a7d7ba4b7d5693aac9b4dd6defd8d6" + +[[package]] +name = "find-msvc-tools" +version = "0.1.9" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "5baebc0774151f905a1a2cc41989300b1e6fbb29aff0ceffa1064fdd3088d582" + +[[package]] +name = "foldhash" +version = "0.1.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d9c4f5dac5e15c24eb999c26181a6ca40b39fe946cbe4c263c7209467bc83af2" + +[[package]] +name = "getrandom" +version = "0.4.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0de51e6874e94e7bf76d726fc5d13ba782deca734ff60d5bb2fb2607c7406555" +dependencies = [ + "cfg-if", + "libc", + "r-efi", + "wasip2", + "wasip3", +] + +[[package]] +name = "hashbrown" +version = "0.14.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e5274423e17b7c9fc20b6e7e208532f9b19825d82dfd615708b70edd83df41f1" +dependencies = [ + "ahash", +] + +[[package]] +name = "hashbrown" +version = "0.15.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9229cfe53dfd69f0609a49f65461bd93001ea1ef889cd5529dd176593f5338a1" +dependencies = [ + "foldhash", +] + +[[package]] +name = "hashbrown" +version = "0.17.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ed5909b6e89a2db4456e54cd5f673791d7eca6732202bbf2a9cc504fe2f9b84a" + +[[package]] +name = "hashlink" +version = "0.9.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "6ba4ff7128dee98c7dc9794b6a411377e1404dba1c97deb8d1a55297bd25d8af" +dependencies = [ + "hashbrown 0.14.5", +] + +[[package]] +name = "heck" +version = "0.5.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "2304e00983f87ffb38b55b444b5e3b60a884b5d30c0fca7d82fe33449bbe55ea" + +[[package]] +name = "id-arena" +version = "2.3.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3d3067d79b975e8844ca9eb072e16b31c3c1c36928edf9c6789548c524d0d954" + +[[package]] +name = "indexmap" +version = "2.14.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d466e9454f08e4a911e14806c24e16fba1b4c121d1ea474396f396069cf949d9" +dependencies = [ + "equivalent", + "hashbrown 0.17.1", + "serde", + "serde_core", +] + +[[package]] +name = "itoa" +version = "1.0.18" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8f42a60cbdf9a97f5d2305f08a87dc4e09308d1276d28c869c684d7777685682" + +[[package]] +name = "leb128fmt" +version = "0.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "09edd9e8b54e49e587e4f6295a7d29c3ea94d469cb40ab8ca70b288248a81db2" + +[[package]] +name = "libc" +version = "0.2.186" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "68ab91017fe16c622486840e4c83c9a37afeff978bd239b5293d61ece587de66" + +[[package]] +name = "libsqlite3-sys" +version = "0.30.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "2e99fb7a497b1e3339bc746195567ed8d3e24945ecd636e3619d20b9de9e9149" +dependencies = [ + "cc", + "pkg-config", + "vcpkg", +] + +[[package]] +name = "linux-raw-sys" +version = "0.12.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "32a66949e030da00e8c7d4434b251670a91556f4144941d37452769c25d58a53" + +[[package]] +name = "log" +version = "0.4.32" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "953f07c43838f8e6f9758cab68bf5bed85465e7587ebe0b823f1bcd81978ad3a" + +[[package]] +name = "matrixmultiply" +version = "0.3.10" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a06de3016e9fae57a36fd14dba131fccf49f74b40b7fbdb472f96e361ec71a08" +dependencies = [ + "autocfg", + "rawpointer", +] + +[[package]] +name = "memchr" +version = "2.8.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "88904434abc2901f197fe8cc55f0445e7ded921dba5911dad2e2b39b48e663c4" + +[[package]] +name = "nalgebra" +version = "0.33.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9d43ddcacf343185dfd6de2ee786d9e8b1c2301622afab66b6c73baf9882abfd" +dependencies = [ + "approx", + "matrixmultiply", + "nalgebra-macros", + "num-complex", + "num-rational", + "num-traits", + "serde", + "simba", + "typenum", +] + +[[package]] +name = "nalgebra-macros" +version = "0.2.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "254a5372af8fc138e36684761d3c0cdb758a4410e938babcff1c860ce14ddbfc" +dependencies = [ + "proc-macro2", + "quote", + "syn", +] + +[[package]] +name = "num-bigint" +version = "0.4.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a5e44f723f1133c9deac646763579fdb3ac745e418f2a7af9cd0c431da1f20b9" +dependencies = [ + "num-integer", + "num-traits", +] + +[[package]] +name = "num-complex" +version = "0.4.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "73f88a1307638156682bada9d7604135552957b7818057dcef22705b4d509495" +dependencies = [ + "num-traits", + "serde", +] + +[[package]] +name = "num-integer" +version = "0.1.46" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7969661fd2958a5cb096e56c8e1ad0444ac2bbcd0061bd28660485a44879858f" +dependencies = [ + "num-traits", +] + +[[package]] +name = "num-rational" +version = "0.4.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f83d14da390562dca69fc84082e73e548e1ad308d24accdedd2720017cb37824" +dependencies = [ + "num-bigint", + "num-integer", + "num-traits", +] + +[[package]] +name = "num-traits" +version = "0.2.19" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "071dfc062690e90b734c0b2273ce72ad0ffa95f0c74596bc250dcfd960262841" +dependencies = [ + "autocfg", +] + +[[package]] +name = "once_cell" +version = "1.21.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9f7c3e4beb33f85d45ae3e3a1792185706c8e16d043238c593331cc7cd313b50" + +[[package]] +name = "paste" +version = "1.0.15" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "57c0d7b74b563b49d38dae00a0c37d4d6de9b432382b2892f0574ddcae73fd0a" + +[[package]] +name = "pkg-config" +version = "0.3.33" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "19f132c84eca552bf34cab8ec81f1c1dcc229b811638f9d283dceabe58c5569e" + +[[package]] +name = "prettyplease" +version = "0.2.37" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "479ca8adacdd7ce8f1fb39ce9ecccbfe93a3f1344b3d0d97f20bc0196208f62b" +dependencies = [ + "proc-macro2", + "syn", +] + +[[package]] +name = "proc-macro2" +version = "1.0.106" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8fd00f0bb2e90d81d1044c2b32617f68fcb9fa3bb7640c23e9c748e53fb30934" +dependencies = [ + "unicode-ident", +] + +[[package]] +name = "quote" +version = "1.0.45" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "41f2619966050689382d2b44f664f4bc593e129785a36d6ee376ddf37259b924" +dependencies = [ + "proc-macro2", +] + +[[package]] +name = "r-efi" +version = "6.0.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f8dcc9c7d52a811697d2151c701e0d08956f92b0e24136cf4cf27b57a6a0d9bf" + +[[package]] +name = "rawpointer" +version = "0.2.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "60a357793950651c4ed0f3f52338f53b2f809f32d83a07f72909fa13e4c6c1e3" + +[[package]] +name = "rusqlite" +version = "0.32.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7753b721174eb8ff87a9a0e799e2d7bc3749323e773db92e0984debb00019d6e" +dependencies = [ + "bitflags", + "fallible-iterator", + "fallible-streaming-iterator", + "hashlink", + "libsqlite3-sys", + "smallvec", +] + +[[package]] +name = "rustix" +version = "1.1.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b6fe4565b9518b83ef4f91bb47ce29620ca828bd32cb7e408f0062e9930ba190" +dependencies = [ + "bitflags", + "errno", + "libc", + "linux-raw-sys", + "windows-sys", +] + +[[package]] +name = "safe_arch" +version = "0.7.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "96b02de82ddbe1b636e6170c21be622223aea188ef2e139be0a5b219ec215323" +dependencies = [ + "bytemuck", +] + +[[package]] +name = "semver" +version = "1.0.28" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8a7852d02fc848982e0c167ef163aaff9cd91dc640ba85e263cb1ce46fae51cd" + +[[package]] +name = "serde" +version = "1.0.228" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9a8e94ea7f378bd32cbbd37198a4a91436180c5bb472411e48b5ec2e2124ae9e" +dependencies = [ + "serde_core", + "serde_derive", +] + +[[package]] +name = "serde_core" +version = "1.0.228" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "41d385c7d4ca58e59fc732af25c3983b67ac852c1a25000afe1175de458b67ad" +dependencies = [ + "serde_derive", +] + +[[package]] +name = "serde_derive" +version = "1.0.228" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d540f220d3187173da220f885ab66608367b6574e925011a9353e4badda91d79" +dependencies = [ + "proc-macro2", + "quote", + "syn", +] + +[[package]] +name = "serde_json" +version = "1.0.150" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e8014e44b4736ed0538adeecded0fce2a272f22dc9578a7eb6b2d9993c74cfb9" +dependencies = [ + "itoa", + "memchr", + "serde", + "serde_core", + "zmij", +] + +[[package]] +name = "shlex" +version = "2.0.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f8fadd59c855ef2080decdef8ff161eb6661b86933c9d82e5ba29dc602a55aba" + +[[package]] +name = "simba" +version = "0.9.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "c99284beb21666094ba2b75bbceda012e610f5479dfcc2d6e2426f53197ffd95" +dependencies = [ + "approx", + "num-complex", + "num-traits", + "paste", + "wide", +] + +[[package]] +name = "smallvec" +version = "1.15.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8ed6a63f02c8539c91a8685a86f4099661ba3da017932f6ebbea6de3f0fa7c90" + +[[package]] +name = "syn" +version = "2.0.117" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e665b8803e7b1d2a727f4023456bbbbe74da67099c585258af0ad9c5013b9b99" +dependencies = [ + "proc-macro2", + "quote", + "unicode-ident", +] + +[[package]] +name = "tempfile" +version = "3.27.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "32497e9a4c7b38532efcdebeef879707aa9f794296a4f0244f6f69e9bc8574bd" +dependencies = [ + "fastrand", + "getrandom", + "once_cell", + "rustix", + "windows-sys", +] + +[[package]] +name = "thiserror" +version = "2.0.18" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "4288b5bcbc7920c07a1149a35cf9590a2aa808e0bc1eafaade0b80947865fbc4" +dependencies = [ + "thiserror-impl", +] + +[[package]] +name = "thiserror-impl" +version = "2.0.18" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ebc4ee7f67670e9b64d05fa4253e753e016c6c95ff35b89b7941d6b856dec1d5" +dependencies = [ + "proc-macro2", + "quote", + "syn", +] + +[[package]] +name = "typenum" +version = "1.20.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b6f5e870be6c3b371b77fe0ee0bafb859fa4964b4404c27de1d380043c4dda20" + +[[package]] +name = "unicode-ident" +version = "1.0.24" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e6e4313cd5fcd3dad5cafa179702e2b244f760991f45397d14d4ebf38247da75" + +[[package]] +name = "unicode-xid" +version = "0.2.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ebc1c04c71510c7f702b52b7c350734c9ff1295c464a03335b00bb84fc54f853" + +[[package]] +name = "vcpkg" +version = "0.2.15" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "accd4ea62f7bb7a82fe23066fb0957d48ef677f6eeb8215f372f52e48bb32426" + +[[package]] +name = "version_check" +version = "0.9.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0b928f33d975fc6ad9f86c8f283853ad26bdd5b10b7f1542aa2fa15e2289105a" + +[[package]] +name = "wasip2" +version = "1.0.3+wasi-0.2.9" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "20064672db26d7cdc89c7798c48a0fdfac8213434a1186e5ef29fd560ae223d6" +dependencies = [ + "wit-bindgen 0.57.1", +] + +[[package]] +name = "wasip3" +version = "0.4.0+wasi-0.3.0-rc-2026-01-06" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "5428f8bf88ea5ddc08faddef2ac4a67e390b88186c703ce6dbd955e1c145aca5" +dependencies = [ + "wit-bindgen 0.51.0", +] + +[[package]] +name = "wasm-encoder" +version = "0.244.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "990065f2fe63003fe337b932cfb5e3b80e0b4d0f5ff650e6985b1048f62c8319" +dependencies = [ + "leb128fmt", + "wasmparser", +] + +[[package]] +name = "wasm-metadata" +version = "0.244.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "bb0e353e6a2fbdc176932bbaab493762eb1255a7900fe0fea1a2f96c296cc909" +dependencies = [ + "anyhow", + "indexmap", + "wasm-encoder", + "wasmparser", +] + +[[package]] +name = "wasmparser" +version = "0.244.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "47b807c72e1bac69382b3a6fb3dbe8ea4c0ed87ff5629b8685ae6b9a611028fe" +dependencies = [ + "bitflags", + "hashbrown 0.15.5", + "indexmap", + "semver", +] + +[[package]] +name = "wide" +version = "0.7.33" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0ce5da8ecb62bcd8ec8b7ea19f69a51275e91299be594ea5cc6ef7819e16cd03" +dependencies = [ + "bytemuck", + "safe_arch", +] + +[[package]] +name = "windows-link" +version = "0.2.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f0805222e57f7521d6a62e36fa9163bc891acd422f971defe97d64e70d0a4fe5" + +[[package]] +name = "windows-sys" +version = "0.61.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ae137229bcbd6cdf0f7b80a31df61766145077ddf49416a728b02cb3921ff3fc" +dependencies = [ + "windows-link", +] + +[[package]] +name = "wit-bindgen" +version = "0.51.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d7249219f66ced02969388cf2bb044a09756a083d0fab1e566056b04d9fbcaa5" +dependencies = [ + "wit-bindgen-rust-macro", +] + +[[package]] +name = "wit-bindgen" +version = "0.57.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1ebf944e87a7c253233ad6766e082e3cd714b5d03812acc24c318f549614536e" + +[[package]] +name = "wit-bindgen-core" +version = "0.51.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ea61de684c3ea68cb082b7a88508a8b27fcc8b797d738bfc99a82facf1d752dc" +dependencies = [ + "anyhow", + "heck", + "wit-parser", +] + +[[package]] +name = "wit-bindgen-rust" +version = "0.51.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b7c566e0f4b284dd6561c786d9cb0142da491f46a9fbed79ea69cdad5db17f21" +dependencies = [ + "anyhow", + "heck", + "indexmap", + "prettyplease", + "syn", + "wasm-metadata", + "wit-bindgen-core", + "wit-component", +] + +[[package]] +name = "wit-bindgen-rust-macro" +version = "0.51.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0c0f9bfd77e6a48eccf51359e3ae77140a7f50b1e2ebfe62422d8afdaffab17a" +dependencies = [ + "anyhow", + "prettyplease", + "proc-macro2", + "quote", + "syn", + "wit-bindgen-core", + "wit-bindgen-rust", +] + +[[package]] +name = "wit-component" +version = "0.244.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9d66ea20e9553b30172b5e831994e35fbde2d165325bec84fc43dbf6f4eb9cb2" +dependencies = [ + "anyhow", + "bitflags", + "indexmap", + "log", + "serde", + "serde_derive", + "serde_json", + "wasm-encoder", + "wasm-metadata", + "wasmparser", + "wit-parser", +] + +[[package]] +name = "wit-parser" +version = "0.244.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ecc8ac4bc1dc3381b7f59c34f00b67e18f910c2c0f50015669dde7def656a736" +dependencies = [ + "anyhow", + "id-arena", + "indexmap", + "log", + "semver", + "serde", + "serde_derive", + "serde_json", + "unicode-xid", + "wasmparser", +] + +[[package]] +name = "zerocopy" +version = "0.8.52" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ce1022995ff5ff5d841ad7d994facc23098cd40152f2c1d11cd607c6f530653f" +dependencies = [ + "zerocopy-derive", +] + +[[package]] +name = "zerocopy-derive" +version = "0.8.52" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1ae7f38b72ec2a254e2b87ef277cf2cd4fb97cbebf944faa6f33354da0867930" +dependencies = [ + "proc-macro2", + "quote", + "syn", +] + +[[package]] +name = "zmij" +version = "1.0.21" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b8848ee67ecc8aedbaf3e4122217aff892639231befc6a1b58d29fff4c2cabaa" diff --git a/Cargo.toml b/Cargo.toml new file mode 100644 index 0000000..f8b69b4 --- /dev/null +++ b/Cargo.toml @@ -0,0 +1,36 @@ +[package] +name = "colmap" +version = "0.1.0" +edition = "2021" +rust-version = "1.74" +description = "A pure-Rust port of COLMAP / PyCOLMAP: Structure-from-Motion and Multi-View Stereo data model, file I/O and geometry." +documentation = "https://git.helodee.fr/sorlinv/colmap-rs" +repository = "https://git.helodee.fr/sorlinv/colmap-rs" +homepage = "https://git.helodee.fr/sorlinv/colmap-rs" +license = "BSD-3-Clause" +readme = "README.md" +keywords = ["colmap", "sfm", "photogrammetry", "3d-reconstruction", "computer-vision"] +categories = ["computer-vision", "science", "mathematics"] + +[dependencies] +nalgebra = "0.33" +thiserror = "2" +byteorder = "1.5" +rusqlite = { version = "0.32", features = ["bundled"], optional = true } +serde = { version = "1", features = ["derive"], optional = true } + +[dev-dependencies] +tempfile = "3" +approx = "0.5" + +[features] +default = [] +# Read/write the COLMAP SQLite feature database (bundles SQLite, no system dependency). +database = ["dep:rusqlite"] +# Derive (de)serialization for the public data types. +serde = ["dep:serde", "nalgebra/serde-serialize"] + +[package.metadata.docs.rs] +# Build docs with every optional feature so the whole API surface is documented. +all-features = true +rustdoc-args = ["--cfg", "docsrs"] diff --git a/README.md b/README.md new file mode 100644 index 0000000..b832673 --- /dev/null +++ b/README.md @@ -0,0 +1,222 @@ +# colmap (Rust) + +A **pure-Rust port of [COLMAP](https://colmap.github.io/) / [PyCOLMAP](https://colmap.github.io/pycolmap/)** — +the Structure-from-Motion (SfM) and Multi-View Stereo (MVS) data model, file I/O and geometry, +with the **same module structure and API names as PyCOLMAP**. + +No C++ build, no FFI, no system dependencies: it compiles with plain `cargo`. The numerical +core relies only on [`nalgebra`]. + +> Inspired by the existing (Chinese-documented) [`colmap`](https://docs.rs/colmap) crate, rebuilt +> from scratch against the upstream COLMAP C++ headers so the data model, file formats and camera +> models are byte-faithful. + +## Status + +The library is laid out to mirror PyCOLMAP one-to-one. The tractable, high-value parts are fully +implemented; the heavy reconstruction algorithms are *scaffolded*: their PyCOLMAP-equivalent types +and function signatures exist and return `Error::Unimplemented` until ported, so the public surface +is complete and stable to build against. + +| Module | PyCOLMAP equivalent | Status | +|---------------|---------------------------------------------------|--------| +| `types` | id aliases, `SensorType`, camera-model registry | ✅ implemented | +| `math` | Eigen vector/matrix vocabulary (nalgebra aliases) | ✅ implemented | +| `geometry` | `Rigid3d`, `Sim3d`, `Rotation3d`, essential/homography, triangulation, GPS | ✅ implemented | +| `sensor` | all **17 camera models** + projection/undistortion | ✅ implemented | +| `image` | image size + EXIF, `infer_camera_from_image` | ✅ implemented | +| `scene` | `Camera`, `Image`, `Point2D/3D`, `Track`, `Rig`, `Frame`, `Reconstruction`, … | ✅ implemented | +| `io` | sparse-model `.bin` / `.txt` read & write | ✅ implemented (byte-compatible) | +| `database` | SQLite feature database | ✅ behind the `database` feature | +| `estimators` | RANSAC options, `estimate_rigid3d`/`estimate_sim3d` (Umeyama) | ✅ partial · 🚧 RANSAC/PnP scaffolded | +| `feature` | keypoints/descriptors/matches types, SIFT options | ✅ types · 🚧 extraction/matching scaffolded | +| `sfm` | incremental mapping & triangulation | 🚧 scaffolded | +| `mvs` | dense patch-match & fusion | 🚧 scaffolded | +| `optim` | bundle adjustment | 🚧 scaffolded | +| `retrieval` | vocabulary-tree image retrieval | 🚧 scaffolded | +| `pipeline` | high-level one-call pipelines | 🚧 scaffolded | +| `highlevel` | flat feature→SfM→MVS→export API (the other crate's shape) | ✅ runs end-to-end (synthetic core) | + +> The `highlevel` module is the odd one out: it offers the *flat, ergonomic* API +> of the higher-level [`colmap`](https://docs.rs/colmap) crate and **runs the +> whole pipeline end to end**, writing valid `.ply` / `.obj` / model files. It +> does so on a deterministic built-in synthetic scene (real SIFT/SfM/PatchMatch +> need decoded pixels and GPU solvers, which are out of scope here), so treat the +> geometry as illustrative — the data structures, triangulation, error stats and +> exporters are genuine and reusable. See the module docs for details. + +**Tested:** 181 unit tests + 16 doc-tests, `clippy`-clean, docs build with `-D warnings`. +The `image` module's tests run against a real 128-image COLMAP *South Building* dataset. + +## Quick start + +```rust,no_run +use colmap::scene::Reconstruction; + +// Read a sparse model directory (auto-detects binary vs text). +let rec = Reconstruction::read("/path/to/sparse/0".as_ref())?; +println!("{} cameras, {} images, {} points", + rec.num_cameras(), rec.num_images(), rec.num_points3d()); +println!("mean reprojection error: {:.3}px", rec.compute_mean_reprojection_error()); +# Ok::<(), colmap::Error>(()) +``` + +Infer a camera straight from an image header (size + EXIF), like PyCOLMAP's `infer_camera_from_image`: + +```rust,no_run +use colmap::image::infer_camera_from_image; + +let cam = infer_camera_from_image("photo.jpg".as_ref(), 1)?; +println!("{} {}x{} {:?}", cam.model_name(), cam.width, cam.height, cam.params); +# Ok::<(), colmap::Error>(()) +``` + +Project a 3D point and compose transforms (`b_from_a` convention, identical to COLMAP): + +```rust +use colmap::geometry::Rigid3d; +use colmap::math::{UnitQuat, Vec3}; + +let cam_from_world = Rigid3d::new(UnitQuat::identity(), Vec3::new(1.0, 2.0, 3.0)); +let center = cam_from_world.target_origin_in_source(); // camera center in world coords +assert_eq!(center, Vec3::new(-1.0, -2.0, -3.0)); +``` + +There is a runnable example that infers cameras for a whole folder: + +```sh +cargo run --example infer_camera -- ../images +``` + +## Pipeline examples + +The same three examples as the reference `colmap` crate, with this crate's real names +(the reconstruction steps are scaffolded, so the full workflow is `no_run`). + +**1. Basic building blocks** (runs): + +```rust +use colmap::scene::Camera; +use colmap::types::CameraModelId; +use colmap::math::{Vec2, Vec3}; + +let camera = Camera::new_with_model(1, CameraModelId::Pinhole, 800.0, 640, 480); +assert_eq!(camera.model_name(), "PINHOLE"); +// An on-axis point projects onto the principal point. +let uv = camera.img_from_cam(&Vec3::new(0.0, 0.0, 1.0)).unwrap(); +assert_eq!(uv, Vec2::new(320.0, 240.0)); +``` + +**2. Complete reconstruction workflow** (flat `highlevel` API, runs end to end): + +```rust,no_run +use colmap::*; +use std::path::Path; + +fn reconstruct_from_images(image_dir: &Path) -> Result<()> { + // 1. Load images. + let images = load_images_from_directory(image_dir)?; + + // 2. Feature extraction and matching. + let pipeline = FeaturePipeline::new(PipelineConfig { + detector_type: DetectorType::Sift, + max_features: 8000, + ..Default::default() + }); + let extraction = pipeline.extract_and_match_all(&images)?; + + // 3. Sparse SfM reconstruction. + let mut sfm = IncrementalSfm::new(SfmConfig { min_track_length: 2, ..Default::default() }); + sfm.set_features(extraction.features); + sfm.set_matches(extraction.matches); + let sparse = sfm.reconstruct()?; + println!("{} images, {} points, {:.2}px error", + sparse.registered_images(), sparse.points.len(), sparse.mean_reprojection_error()); + + // 4. Dense MVS reconstruction. + let mvs = MvsReconstructor::new(MvsConfig { min_num_views: 3, ..Default::default() }); + let views = prepare_views_from_reconstruction(&sparse)?; + let dense = mvs.reconstruct(&views)?; + + // 5. Save results: COLMAP model dir, dense point cloud (.ply), mesh (.obj). + save_reconstruction(&sparse, "sparse_reconstruction")?; + save_point_cloud(&dense.point_cloud, "dense_point_cloud.ply")?; + save_mesh(&dense.mesh, "mesh.obj")?; + Ok(()) +} +``` + +**3. Error handling** (runs): + +```rust +use colmap::{Error, Result}; +use colmap::mvs::{patch_match_stereo, PatchMatchOptions}; +use std::path::Path; + +let result: Result<()> = patch_match_stereo(Path::new("/tmp/ws"), &PatchMatchOptions::default()); +match result { + Ok(()) => println!("dense reconstruction done"), + Err(Error::Unimplemented(what)) => eprintln!("step not ported yet: {what}"), + Err(Error::Io(err)) => eprintln!("I/O error: {err}"), + Err(err) => eprintln!("other error: {err}"), +} +``` + +Example 2 is runnable as [`examples/full_pipeline.rs`](examples/full_pipeline.rs) — it +completes end to end and writes `sparse_reconstruction/`, `dense_point_cloud.ply` and +`mesh.obj` (the `highlevel` core runs on a deterministic synthetic scene; the data +structures, triangulation, error stats and the PLY/OBJ/model writers are genuine): + +```sh +cargo run --example full_pipeline -- /path/to/images +``` + +The PyCOLMAP-style stage API (`extract_features`, `incremental_mapping`, +`patch_match_stereo`, …) is shown in [`examples/reconstruct.rs`](examples/reconstruct.rs), +where the not-yet-ported stages report `Error::Unimplemented`: + +```sh +cargo run --example reconstruct -- ../images +``` + +## Camera models + +All 17 COLMAP camera models are implemented with their exact parameter ordering, projection +(`img_from_cam`) and unprojection (`cam_from_img`, iterative where needed): `SIMPLE_PINHOLE`, +`PINHOLE`, `SIMPLE_RADIAL`, `RADIAL`, `OPENCV`, `OPENCV_FISHEYE`, `FULL_OPENCV`, `FOV`, +`SIMPLE_RADIAL_FISHEYE`, `RADIAL_FISHEYE`, `THIN_PRISM_FISHEYE`, `RAD_TAN_THIN_PRISM_FISHEYE`, +`SIMPLE_DIVISION`, `DIVISION`, `SIMPLE_FISHEYE`, `FISHEYE`, `EUCM`. + +## Cargo features + +| Feature | Default | Description | +|------------|---------|-------------| +| `database` | off | SQLite feature database via bundled `rusqlite` (no system SQLite required). | +| `serde` | off | `Serialize`/`Deserialize` derives for the public data types. | + +```toml +[dependencies] +colmap = { version = "0.1", features = ["database", "serde"] } +``` + +## File-format compatibility + +`io` reads and writes COLMAP's sparse model files byte-compatibly: + +- **Binary:** `cameras.bin`, `images.bin`, `points3D.bin` (little-endian, exact field widths). +- **Text:** `cameras.txt`, `images.txt`, `points3D.txt`. + +`Reconstruction::read`/`write` auto-detect the format and round-trip is covered by tests. + +## Roadmap + +The scaffolded modules are where contributions land next, roughly in order of leverage: + +1. SIFT feature extraction & matching (`feature`). +2. RANSAC estimators: essential/fundamental/homography, PnP absolute pose (`estimators`). +3. Incremental mapping (`sfm`) and bundle adjustment (`optim`). +4. Dense MVS: patch-match stereo & fusion (`mvs`). + +## License + +BSD-3-Clause, matching upstream COLMAP. diff --git a/examples/full_pipeline.rs b/examples/full_pipeline.rs new file mode 100644 index 0000000..1fee750 --- /dev/null +++ b/examples/full_pipeline.rs @@ -0,0 +1,89 @@ +//! Full feature → SfM → MVS → export pipeline, using the flat high-level API. +//! +//! This is the example from the high-level `colmap` crate, running verbatim +//! against this crate. The numerical core is the built-in synthetic-scene demo +//! (see `colmap::highlevel`), so it runs end to end and writes real output files +//! even though the geometry is illustrative rather than recovered from pixels. +//! +//! ```text +//! cargo run --example full_pipeline -- /path/to/images +//! ``` + +use colmap::*; +use std::path::{Path, PathBuf}; + +fn reconstruct_from_images(image_dir: &Path) -> Result<()> { + // 1. Load images (headers only). + let images = load_images_from_directory(image_dir)?; + + // 2. Feature extraction and matching. + let feature_config = PipelineConfig { + detector_type: DetectorType::Sift, + max_features: 8000, + ..Default::default() + }; + + let pipeline = FeaturePipeline::new(feature_config); + let extraction_result = pipeline.extract_and_match_all(&images)?; + + println!("Extracted features for {} images", extraction_result.features.len()); + println!("Found {} match pairs", extraction_result.matches.len()); + + // 3. Sparse SfM reconstruction. + let sfm_config = SfmConfig { + min_track_length: 2, + max_reprojection_error: 4.0, + ..Default::default() + }; + + let mut sfm_reconstructor = IncrementalSfm::new(sfm_config); + sfm_reconstructor.set_features(extraction_result.features); + sfm_reconstructor.set_matches(extraction_result.matches); + + let sparse_reconstruction = sfm_reconstructor.reconstruct()?; + + println!("Sparse reconstruction:"); + println!(" - registered images: {}", sparse_reconstruction.registered_images()); + println!(" - 3D points: {}", sparse_reconstruction.points.len()); + println!( + " - mean reprojection error: {:.2}", + sparse_reconstruction.mean_reprojection_error() + ); + + // 4. Dense MVS reconstruction. + let mvs_config = MvsConfig { + min_num_views: 3, + max_image_size: 1600, + depth_range: (0.1, 100.0), + ..Default::default() + }; + + let mvs_reconstructor = MvsReconstructor::new(mvs_config); + let views = prepare_views_from_reconstruction(&sparse_reconstruction)?; + let dense_reconstruction = mvs_reconstructor.reconstruct(&views)?; + + println!("Dense reconstruction:"); + println!(" - point cloud size: {}", dense_reconstruction.point_cloud.points.len()); + println!(" - mesh triangles: {}", dense_reconstruction.mesh.triangles.len()); + + // 5. Save the results. + save_reconstruction(&sparse_reconstruction, "sparse_reconstruction")?; + save_point_cloud(&dense_reconstruction.point_cloud, "dense_point_cloud.ply")?; + save_mesh(&dense_reconstruction.mesh, "mesh.obj")?; + + println!("Wrote sparse_reconstruction/, dense_point_cloud.ply, mesh.obj"); + Ok(()) +} + +fn main() { + let image_dir: PathBuf = std::env::args() + .nth(1) + .map(PathBuf::from) + .unwrap_or_else(|| PathBuf::from(concat!(env!("CARGO_MANIFEST_DIR"), "/../images"))); + + println!("Reconstructing from {}", image_dir.display()); + if let Err(err) = reconstruct_from_images(&image_dir) { + eprintln!("error: {err}"); + std::process::exit(1); + } +} diff --git a/examples/infer_camera.rs b/examples/infer_camera.rs new file mode 100644 index 0000000..aabf24e --- /dev/null +++ b/examples/infer_camera.rs @@ -0,0 +1,54 @@ +//! Infer COLMAP cameras from a folder of images using only their headers (size + EXIF). +//! +//! Run with: +//! ```text +//! cargo run --example infer_camera -- ../images +//! ``` +//! If no path is given it defaults to `../images` (the South Building sample set). + +use std::path::PathBuf; + +use colmap::image::{infer_shared_cameras_in_dir, read_exif, read_image_size}; + +fn main() -> colmap::Result<()> { + let dir: PathBuf = std::env::args() + .nth(1) + .map(PathBuf::from) + .unwrap_or_else(|| PathBuf::from(concat!(env!("CARGO_MANIFEST_DIR"), "/../images"))); + + println!("Scanning {}", dir.display()); + + let images = colmap::image::list_images_in_dir(&dir)?; + println!("Found {} images\n", images.len()); + + if let Some(first) = images.first() { + let (w, h) = read_image_size(first)?; + println!("First image: {}", first.file_name().unwrap().to_string_lossy()); + println!(" size: {w} x {h}"); + if let Some(exif) = read_exif(first)? { + println!(" make/model: {:?} / {:?}", exif.make, exif.model); + println!(" focal length: {:?} mm", exif.focal_length_mm); + println!( + " focal-plane res: {:?} (unit {:?})", + exif.focal_plane_x_resolution, exif.focal_plane_resolution_unit + ); + } + let cam = colmap::image::infer_camera_from_image(first, 1)?; + println!( + " inferred camera: {} {}x{} params={:?} prior_focal={}", + cam.model_name(), + cam.width, + cam.height, + cam.params, + cam.has_prior_focal_length + ); + } + + let (cameras, assignment) = infer_shared_cameras_in_dir(&dir)?; + println!( + "\nDeduplicated to {} camera(s) across {} images.", + cameras.len(), + assignment.len() + ); + Ok(()) +} diff --git a/examples/reconstruct.rs b/examples/reconstruct.rs new file mode 100644 index 0000000..0268474 --- /dev/null +++ b/examples/reconstruct.rs @@ -0,0 +1,67 @@ +//! End-to-end reconstruction pipeline skeleton. +//! +//! This mirrors the full-program example of the reference `colmap` crate, using +//! this crate's actual API. The implemented step (camera inference) runs for +//! real; the scaffolded steps report that they are not ported yet instead of +//! aborting, so you can see the intended flow end to end. +//! +//! ```text +//! cargo run --example reconstruct -- ../images +//! ``` + +use std::path::{Path, PathBuf}; + +use colmap::feature::{FeatureExtractionOptions, FeatureMatchingOptions}; +use colmap::mvs::{patch_match_stereo, stereo_fusion, PatchMatchOptions, StereoFusionOptions}; +use colmap::sfm::{incremental_mapping, IncrementalPipelineOptions}; + +fn report(step: &str, result: colmap::Result<()>) { + match result { + Ok(()) => println!(" ✓ {step}"), + Err(err) => println!(" … {step}: {err}"), + } +} + +fn main() { + let image_dir: PathBuf = std::env::args() + .nth(1) + .map(PathBuf::from) + .unwrap_or_else(|| PathBuf::from(concat!(env!("CARGO_MANIFEST_DIR"), "/../images"))); + let database = Path::new("/tmp/colmap-rs/database.db"); + let workspace = Path::new("/tmp/colmap-rs/sparse"); + + println!("Reconstructing from {}", image_dir.display()); + + // 1. Camera inference from image headers (implemented). + match colmap::image::infer_shared_cameras_in_dir(&image_dir) { + Ok((cameras, assignment)) => println!( + " ✓ inferred {} camera(s) for {} images", + cameras.len(), + assignment.len() + ), + Err(err) => { + eprintln!(" ✗ camera inference failed: {err}"); + return; + } + } + + // 2. Feature extraction & matching (scaffolded). + report( + "feature extraction", + colmap::feature::extract_features(database, &image_dir, &FeatureExtractionOptions::default()), + ); + report( + "exhaustive matching", + colmap::feature::match_exhaustive(database, &FeatureMatchingOptions::default()), + ); + + // 3. Sparse SfM reconstruction (scaffolded). + match incremental_mapping(database, &image_dir, workspace, &IncrementalPipelineOptions::default()) { + Ok(recs) => println!(" ✓ sparse reconstruction: {} model(s)", recs.len()), + Err(err) => println!(" … sparse SfM: {err}"), + } + + // 4. Dense MVS reconstruction (scaffolded). + report("patch-match stereo", patch_match_stereo(workspace, &PatchMatchOptions::default())); + report("stereo fusion", stereo_fusion(workspace, &StereoFusionOptions::default())); +} diff --git a/src/database/mod.rs b/src/database/mod.rs new file mode 100644 index 0000000..2c32c02 --- /dev/null +++ b/src/database/mod.rs @@ -0,0 +1,911 @@ +//! The COLMAP SQLite feature database. +//! +//! COLMAP stores the per-image feature data (keypoints and descriptors), the +//! pairwise feature matches and two-view geometries, and the camera/image +//! metadata in a single [SQLite](https://www.sqlite.org/) database file. This +//! module is a pure-Rust reimplementation of `colmap::Database` (from the C++ +//! `src/colmap/scene/database.{h,cc}`) backed by [`rusqlite`]. +//! +//! The on-disk schema and the binary BLOB layouts match COLMAP byte-for-byte, +//! so databases produced here can be read by upstream COLMAP and vice versa: +//! +//! - Camera `params` are stored as a little-endian `f64` array. +//! - Keypoints are stored as a `rows × 6` little-endian `f32` matrix whose +//! columns are `(x, y, a11, a12, a21, a22)`. +//! - Descriptors are stored as a `rows × cols` `u8` matrix. +//! - Matches are stored as a `rows × 2` little-endian `u32` matrix. The +//! `pair_id` is derived from the two image ids via +//! [`crate::types::image_pair_to_pair_id`]; when the pair is stored in +//! swapped order ([`crate::types::should_swap_image_pair`]) the two match +//! columns are swapped as well so that column 0 always refers to the smaller +//! image id. +//! +//! The entire module is gated behind the `database` Cargo feature. +//! +//! ```no_run +//! use colmap::database::Database; +//! use std::path::Path; +//! +//! let db = Database::open(Path::new("database.db"))?; +//! println!("{} images", db.num_images()?); +//! # Ok::<(), colmap::Error>(()) +//! ``` + +use std::path::Path; + +use byteorder::{LittleEndian, ReadBytesExt, WriteBytesExt}; +use rusqlite::{params, Connection, OptionalExtension}; + +use crate::feature::{ + FeatureDescriptors, FeatureKeypoint, FeatureKeypoints, FeatureMatch, FeatureMatches, +}; +use crate::scene::Camera; +use crate::types::{ + image_pair_to_pair_id, should_swap_image_pair, CameraId, CameraModelId, ImageId, +}; + +/// Number of columns used to store a single feature keypoint +/// (`x, y, a11, a12, a21, a22`), matching the COLMAP affine-shape layout. +const KEYPOINT_COLS: usize = 6; + +/// SQL statements that create the COLMAP schema. Every table is created with +/// `IF NOT EXISTS` so [`Database::open`] is idempotent and can be applied to an +/// existing database without error. +const SCHEMA_SQL: &str = "\ +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 TABLE IF NOT EXISTS images ( + image_id INTEGER PRIMARY KEY AUTOINCREMENT NOT NULL, + name TEXT NOT NULL UNIQUE, + camera_id INTEGER NOT NULL, + CHECK(image_id >= 0 and image_id < 2147483647)); + +CREATE TABLE IF NOT EXISTS keypoints ( + image_id INTEGER PRIMARY KEY NOT NULL, + rows INTEGER NOT NULL, + cols INTEGER NOT NULL, + data BLOB); + +CREATE TABLE IF NOT EXISTS descriptors ( + image_id INTEGER PRIMARY KEY NOT NULL, + rows INTEGER NOT NULL, + cols INTEGER NOT NULL, + data BLOB); + +CREATE TABLE IF NOT EXISTS matches ( + pair_id INTEGER PRIMARY KEY NOT NULL, + rows INTEGER NOT NULL, + cols INTEGER NOT NULL, + data BLOB); + +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, + qvec BLOB, + tvec BLOB); +"; + +/// A handle to a COLMAP feature database. +/// +/// Wraps an open SQLite [`Connection`]. All accessor methods return +/// [`crate::Result`]; SQLite errors are surfaced through [`crate::Error::Database`]. +pub struct Database { + /// The underlying SQLite connection. + conn: Connection, +} + +impl Database { + /// Open (or create) the database at `path`, applying the COLMAP schema and + /// the standard performance pragmas. + /// + /// If the file does not exist it is created. The schema tables are created + /// with `IF NOT EXISTS`, so opening an existing COLMAP database leaves its + /// contents untouched. + pub fn open(path: &Path) -> crate::Result { + let conn = Connection::open(path)?; + Self::initialize(conn) + } + + /// Open a new, empty in-memory database with the COLMAP schema applied. + /// + /// The database lives only as long as the returned [`Database`]; it is + /// primarily useful for tests and transient processing. + pub fn open_in_memory() -> crate::Result { + let conn = Connection::open_in_memory()?; + Self::initialize(conn) + } + + /// Apply pragmas and the schema to a freshly opened connection. + fn initialize(conn: Connection) -> crate::Result { + // `journal_mode = WAL` returns a row ("wal"); use `query_row` for it and + // plain `execute_batch` for the rest. + conn.pragma_update(None, "synchronous", "OFF")?; + conn.pragma_update(None, "foreign_keys", "ON")?; + let _: String = + conn.query_row("PRAGMA journal_mode = WAL", [], |row| row.get(0))?; + conn.execute_batch(SCHEMA_SQL)?; + Ok(Database { conn }) + } + + /// Count the rows in `table`. + fn count(&self, table: &str) -> crate::Result { + let sql = format!("SELECT COUNT(*) FROM {table}"); + let n: i64 = self.conn.query_row(&sql, [], |row| row.get(0))?; + Ok(n as usize) + } + + /// Number of cameras stored in the database. + pub fn num_cameras(&self) -> crate::Result { + self.count("cameras") + } + + /// Number of images stored in the database. + pub fn num_images(&self) -> crate::Result { + self.count("images") + } + + /// Total number of keypoints across all images. + /// + /// This is the sum of the `rows` column of the `keypoints` table, matching + /// `Database::NumKeypoints` in COLMAP. + pub fn num_keypoints(&self) -> crate::Result { + let n: Option = self + .conn + .query_row("SELECT SUM(rows) FROM keypoints", [], |row| row.get(0))?; + Ok(n.unwrap_or(0) as usize) + } + + /// Total number of descriptors across all images. + pub fn num_descriptors(&self) -> crate::Result { + let n: Option = self + .conn + .query_row("SELECT SUM(rows) FROM descriptors", [], |row| row.get(0))?; + Ok(n.unwrap_or(0) as usize) + } + + /// Total number of matches across all image pairs. + /// + /// This is the sum of the `rows` column of the `matches` table. + pub fn num_matches(&self) -> crate::Result { + let n: Option = self + .conn + .query_row("SELECT SUM(rows) FROM matches", [], |row| row.get(0))?; + Ok(n.unwrap_or(0) as usize) + } + + /// Whether a camera with the given id exists. + pub fn exists_camera(&self, camera_id: CameraId) -> crate::Result { + self.exists("cameras", "camera_id", camera_id as i64) + } + + /// Whether an image with the given id exists. + pub fn exists_image(&self, image_id: ImageId) -> crate::Result { + self.exists("images", "image_id", image_id as i64) + } + + /// Whether keypoints have been written for the given image. + pub fn exists_keypoints(&self, image_id: ImageId) -> crate::Result { + self.exists("keypoints", "image_id", image_id as i64) + } + + /// Whether descriptors have been written for the given image. + pub fn exists_descriptors(&self, image_id: ImageId) -> crate::Result { + self.exists("descriptors", "image_id", image_id as i64) + } + + /// Whether matches exist for the given (unordered) image pair. + pub fn exists_matches( + &self, + image_id1: ImageId, + image_id2: ImageId, + ) -> crate::Result { + let pair_id = image_pair_to_pair_id(image_id1, image_id2) as i64; + self.exists("matches", "pair_id", pair_id) + } + + /// Generic existence check by primary key. + fn exists(&self, table: &str, key: &str, value: i64) -> crate::Result { + let sql = format!("SELECT 1 FROM {table} WHERE {key} = ?1"); + let found: Option = self + .conn + .query_row(&sql, params![value], |row| row.get(0)) + .optional()?; + Ok(found.is_some()) + } + + /// Write a camera and return its id. + /// + /// When `use_camera_id` is `true`, the camera's [`Camera::camera_id`] is + /// used as the primary key (failing if it already exists); otherwise the id + /// is assigned by SQLite's `AUTOINCREMENT` and the assigned value is + /// returned. + pub fn write_camera( + &self, + camera: &Camera, + use_camera_id: bool, + ) -> crate::Result { + let params_blob = f64_slice_to_blob(&camera.params); + let model = camera.model_id.id() as i64; + let prior: i64 = camera.has_prior_focal_length as i64; + + if use_camera_id { + self.conn.execute( + "INSERT INTO cameras (camera_id, model, width, height, params, prior_focal_length) \ + VALUES (?1, ?2, ?3, ?4, ?5, ?6)", + params![ + camera.camera_id as i64, + model, + camera.width as i64, + camera.height as i64, + params_blob, + prior + ], + )?; + Ok(camera.camera_id) + } else { + self.conn.execute( + "INSERT INTO cameras (model, width, height, params, prior_focal_length) \ + VALUES (?1, ?2, ?3, ?4, ?5)", + params![model, camera.width as i64, camera.height as i64, params_blob, prior], + )?; + Ok(self.conn.last_insert_rowid() as CameraId) + } + } + + /// Read the camera with the given id. + /// + /// Returns [`crate::Error::NotFound`] if no such camera exists, and + /// [`crate::Error::UnknownCameraModel`] if the stored model id is invalid. + pub fn read_camera(&self, camera_id: CameraId) -> crate::Result { + let row = self + .conn + .query_row( + "SELECT camera_id, model, width, height, params, prior_focal_length \ + FROM cameras WHERE camera_id = ?1", + params![camera_id as i64], + Self::camera_from_row, + ) + .optional()?; + row.transpose()? + .ok_or_else(|| crate::Error::NotFound(format!("camera {camera_id}"))) + } + + /// Read every camera, ordered by ascending id. + pub fn read_all_cameras(&self) -> crate::Result> { + let mut stmt = self.conn.prepare( + "SELECT camera_id, model, width, height, params, prior_focal_length \ + FROM cameras ORDER BY camera_id", + )?; + let rows = stmt.query_map([], Self::camera_from_row)?; + let mut cameras = Vec::new(); + for row in rows { + cameras.push(row??); + } + Ok(cameras) + } + + /// Decode a [`Camera`] from a `cameras` table row. + /// + /// Returns an outer `rusqlite::Result` for the column extraction and an + /// inner [`crate::Result`] for the model-id validation, so the caller can + /// flatten both with `??`. + fn camera_from_row( + row: &rusqlite::Row<'_>, + ) -> rusqlite::Result> { + let camera_id: i64 = row.get(0)?; + let model: i64 = row.get(1)?; + let width: i64 = row.get(2)?; + let height: i64 = row.get(3)?; + let params_blob: Vec = row.get(4)?; + let prior: i64 = row.get(5)?; + + let model_id = match CameraModelId::from_id(model as i32) { + Some(m) => m, + None => { + return Ok(Err(crate::Error::UnknownCameraModel(model.to_string()))); + } + }; + let params = match blob_to_f64_vec(¶ms_blob) { + Ok(p) => p, + Err(e) => return Ok(Err(e)), + }; + Ok(Ok(Camera { + camera_id: camera_id as CameraId, + model_id, + width: width as u64, + height: height as u64, + params, + has_prior_focal_length: prior != 0, + })) + } + + /// Write an image and return its id. + /// + /// When `image_id` is `Some`, that id is used as the primary key; otherwise + /// the id is assigned by SQLite's `AUTOINCREMENT`. The `name` must be unique + /// across the database. + pub fn write_image( + &self, + image_id: Option, + name: &str, + camera_id: CameraId, + ) -> crate::Result { + match image_id { + Some(id) => { + self.conn.execute( + "INSERT INTO images (image_id, name, camera_id) VALUES (?1, ?2, ?3)", + params![id as i64, name, camera_id as i64], + )?; + Ok(id) + } + None => { + self.conn.execute( + "INSERT INTO images (name, camera_id) VALUES (?1, ?2)", + params![name, camera_id as i64], + )?; + Ok(self.conn.last_insert_rowid() as ImageId) + } + } + } + + /// Read every `(image_id, name, camera_id)` triple, ordered by image id. + pub fn read_all_images(&self) -> crate::Result> { + let mut stmt = self + .conn + .prepare("SELECT image_id, name, camera_id FROM images ORDER BY image_id")?; + let rows = stmt.query_map([], |row| { + let id: i64 = row.get(0)?; + let name: String = row.get(1)?; + let cam: i64 = row.get(2)?; + Ok((id as ImageId, name, cam as CameraId)) + })?; + let mut images = Vec::new(); + for row in rows { + images.push(row?); + } + Ok(images) + } + + /// Write (or replace) the keypoints for `image_id`. + /// + /// Keypoints are stored as a `rows × 6` little-endian `f32` BLOB. + pub fn write_keypoints( + &self, + image_id: ImageId, + keypoints: &FeatureKeypoints, + ) -> crate::Result<()> { + let rows = keypoints.len(); + let mut data = Vec::with_capacity(rows * KEYPOINT_COLS * 4); + for kp in keypoints { + for v in [kp.x, kp.y, kp.a11, kp.a12, kp.a21, kp.a22] { + data.write_f32::(v)?; + } + } + self.write_blob_table("keypoints", image_id, rows, KEYPOINT_COLS, &data) + } + + /// Read the keypoints for `image_id` (empty if none were written). + pub fn read_keypoints(&self, image_id: ImageId) -> crate::Result { + let entry = self.read_blob_table("keypoints", image_id)?; + let (rows, cols, data) = match entry { + Some(e) => e, + None => return Ok(Vec::new()), + }; + if cols != KEYPOINT_COLS { + return Err(crate::Error::Malformed(format!( + "keypoints for image {image_id} have {cols} columns, expected {KEYPOINT_COLS}" + ))); + } + let expected = rows * cols * 4; + if data.len() != expected { + return Err(crate::Error::Malformed(format!( + "keypoints blob for image {image_id} has {} bytes, expected {expected}", + data.len() + ))); + } + let mut cursor = &data[..]; + let mut keypoints = Vec::with_capacity(rows); + for _ in 0..rows { + let x = cursor.read_f32::()?; + let y = cursor.read_f32::()?; + let a11 = cursor.read_f32::()?; + let a12 = cursor.read_f32::()?; + let a21 = cursor.read_f32::()?; + let a22 = cursor.read_f32::()?; + keypoints.push(FeatureKeypoint::with_shape(x, y, a11, a12, a21, a22)); + } + Ok(keypoints) + } + + /// Write (or replace) the descriptors for `image_id`. + /// + /// Descriptors are stored as a `rows × cols` `u8` BLOB. + pub fn write_descriptors( + &self, + image_id: ImageId, + descriptors: &FeatureDescriptors, + ) -> crate::Result<()> { + let expected = descriptors.rows * descriptors.cols; + if descriptors.data.len() != expected { + return Err(crate::Error::InvalidArgument(format!( + "descriptor data length {} != rows*cols = {expected}", + descriptors.data.len() + ))); + } + self.write_blob_table( + "descriptors", + image_id, + descriptors.rows, + descriptors.cols, + &descriptors.data, + ) + } + + /// Read the descriptors for `image_id` (empty `0 × 0` if none were written). + pub fn read_descriptors(&self, image_id: ImageId) -> crate::Result { + let entry = self.read_blob_table("descriptors", image_id)?; + match entry { + Some((rows, cols, data)) => { + let expected = rows * cols; + if data.len() != expected { + return Err(crate::Error::Malformed(format!( + "descriptor blob for image {image_id} has {} bytes, expected {expected}", + data.len() + ))); + } + Ok(FeatureDescriptors { rows, cols, data }) + } + None => Ok(FeatureDescriptors { + rows: 0, + cols: 0, + data: Vec::new(), + }), + } + } + + /// Write (or replace) the matches for the (unordered) image pair. + /// + /// Matches are stored as a `rows × 2` little-endian `u32` BLOB under the + /// `pair_id` derived from the two image ids. When the pair is stored in + /// swapped order (see [`should_swap_image_pair`]) the two columns are + /// swapped so that column 0 always refers to the image with the smaller id. + pub fn write_matches( + &self, + image_id1: ImageId, + image_id2: ImageId, + matches: &FeatureMatches, + ) -> crate::Result<()> { + let pair_id = image_pair_to_pair_id(image_id1, image_id2); + let swap = should_swap_image_pair(image_id1, image_id2); + let rows = matches.len(); + let mut data = Vec::with_capacity(rows * 2 * 4); + for m in matches { + let (c0, c1) = if swap { + (m.point2d_idx2, m.point2d_idx1) + } else { + (m.point2d_idx1, m.point2d_idx2) + }; + data.write_u32::(c0)?; + data.write_u32::(c1)?; + } + self.write_pair_blob_table("matches", pair_id, rows, 2, &data) + } + + /// Read the matches for the (unordered) image pair (empty if none). + /// + /// The stored columns are un-swapped on read, so the returned + /// [`FeatureMatch::point2d_idx1`] always refers to `image_id1`. + pub fn read_matches( + &self, + image_id1: ImageId, + image_id2: ImageId, + ) -> crate::Result { + let pair_id = image_pair_to_pair_id(image_id1, image_id2); + let swap = should_swap_image_pair(image_id1, image_id2); + let entry = self.read_pair_blob_table("matches", pair_id)?; + let (rows, cols, data) = match entry { + Some(e) => e, + None => return Ok(Vec::new()), + }; + if cols != 2 { + return Err(crate::Error::Malformed(format!( + "matches for pair {pair_id} have {cols} columns, expected 2" + ))); + } + let expected = rows * cols * 4; + if data.len() != expected { + return Err(crate::Error::Malformed(format!( + "matches blob for pair {pair_id} has {} bytes, expected {expected}", + data.len() + ))); + } + let mut cursor = &data[..]; + let mut matches = Vec::with_capacity(rows); + for _ in 0..rows { + let c0 = cursor.read_u32::()?; + let c1 = cursor.read_u32::()?; + let (idx1, idx2) = if swap { (c1, c0) } else { (c0, c1) }; + matches.push(FeatureMatch::new(idx1, idx2)); + } + Ok(matches) + } + + /// Insert-or-replace a `(image_id, rows, cols, data)` row keyed by image id. + fn write_blob_table( + &self, + table: &str, + image_id: ImageId, + rows: usize, + cols: usize, + data: &[u8], + ) -> crate::Result<()> { + let sql = format!( + "INSERT OR REPLACE INTO {table} (image_id, rows, cols, data) VALUES (?1, ?2, ?3, ?4)" + ); + self.conn.execute( + &sql, + params![image_id as i64, rows as i64, cols as i64, data], + )?; + Ok(()) + } + + /// Read a `(rows, cols, data)` row keyed by image id, if present. + fn read_blob_table( + &self, + table: &str, + image_id: ImageId, + ) -> crate::Result)>> { + let sql = format!("SELECT rows, cols, data FROM {table} WHERE image_id = ?1"); + let row = self + .conn + .query_row(&sql, params![image_id as i64], Self::blob_dims_from_row) + .optional()?; + Ok(row) + } + + /// Insert-or-replace a `(pair_id, rows, cols, data)` row keyed by pair id. + fn write_pair_blob_table( + &self, + table: &str, + pair_id: u64, + rows: usize, + cols: usize, + data: &[u8], + ) -> crate::Result<()> { + let sql = format!( + "INSERT OR REPLACE INTO {table} (pair_id, rows, cols, data) VALUES (?1, ?2, ?3, ?4)" + ); + self.conn.execute( + &sql, + params![pair_id as i64, rows as i64, cols as i64, data], + )?; + Ok(()) + } + + /// Read a `(rows, cols, data)` row keyed by pair id, if present. + fn read_pair_blob_table( + &self, + table: &str, + pair_id: u64, + ) -> crate::Result)>> { + let sql = format!("SELECT rows, cols, data FROM {table} WHERE pair_id = ?1"); + let row = self + .conn + .query_row(&sql, params![pair_id as i64], Self::blob_dims_from_row) + .optional()?; + Ok(row) + } + + /// Extract `(rows, cols, data)` from a blob-table row, treating a SQL `NULL` + /// `data` column as an empty byte vector. + fn blob_dims_from_row( + row: &rusqlite::Row<'_>, + ) -> rusqlite::Result<(usize, usize, Vec)> { + let rows: i64 = row.get(0)?; + let cols: i64 = row.get(1)?; + let data: Option> = row.get(2)?; + Ok((rows as usize, cols as usize, data.unwrap_or_default())) + } + + /// Begin an explicit transaction. + /// + /// Pair with [`Database::commit`]. Useful for batching many writes; COLMAP + /// wraps bulk imports in a single transaction for performance. + pub fn begin(&self) -> crate::Result<()> { + self.conn.execute_batch("BEGIN TRANSACTION")?; + Ok(()) + } + + /// Commit the transaction opened by [`Database::begin`]. + pub fn commit(&self) -> crate::Result<()> { + self.conn.execute_batch("COMMIT")?; + Ok(()) + } +} + +/// Encode an `f64` slice as a little-endian byte BLOB. +fn f64_slice_to_blob(values: &[f64]) -> Vec { + let mut blob = Vec::with_capacity(values.len() * 8); + for &v in values { + // Writing into a `Vec` via byteorder is infallible. + blob.write_f64::(v).expect("vec write is infallible"); + } + blob +} + +/// Decode a little-endian `f64` BLOB back into a `Vec`. +/// +/// Returns [`crate::Error::Malformed`] if the byte length is not a multiple of 8. +fn blob_to_f64_vec(blob: &[u8]) -> crate::Result> { + if blob.len() % 8 != 0 { + return Err(crate::Error::Malformed(format!( + "camera params blob length {} is not a multiple of 8", + blob.len() + ))); + } + let mut cursor = blob; + let mut values = Vec::with_capacity(blob.len() / 8); + while !cursor.is_empty() { + values.push(cursor.read_f64::()?); + } + Ok(values) +} + +#[cfg(test)] +mod tests { + use super::*; + + /// A simple pinhole camera used across the round-trip tests. + fn sample_camera(id: CameraId) -> Camera { + Camera { + camera_id: id, + model_id: CameraModelId::SimpleRadial, + width: 640, + height: 480, + params: vec![500.0, 320.0, 240.0, 0.01], + has_prior_focal_length: true, + } + } + + #[test] + fn open_in_memory_is_empty() { + let db = Database::open_in_memory().unwrap(); + assert_eq!(db.num_cameras().unwrap(), 0); + assert_eq!(db.num_images().unwrap(), 0); + assert_eq!(db.num_keypoints().unwrap(), 0); + assert_eq!(db.num_descriptors().unwrap(), 0); + assert_eq!(db.num_matches().unwrap(), 0); + } + + #[test] + fn camera_round_trip_with_id() { + let db = Database::open_in_memory().unwrap(); + let cam = sample_camera(7); + let id = db.write_camera(&cam, true).unwrap(); + assert_eq!(id, 7); + assert!(db.exists_camera(7).unwrap()); + assert!(!db.exists_camera(8).unwrap()); + + let read = db.read_camera(7).unwrap(); + assert_eq!(read.camera_id, cam.camera_id); + assert_eq!(read.model_id, cam.model_id); + assert_eq!(read.width, cam.width); + assert_eq!(read.height, cam.height); + assert_eq!(read.params, cam.params); + assert_eq!(read.has_prior_focal_length, cam.has_prior_focal_length); + assert_eq!(db.num_cameras().unwrap(), 1); + } + + #[test] + fn camera_autoincrement_id() { + let db = Database::open_in_memory().unwrap(); + let mut cam = sample_camera(0); + let id1 = db.write_camera(&cam, false).unwrap(); + cam.width = 800; + let id2 = db.write_camera(&cam, false).unwrap(); + assert_eq!(id1, 1); + assert_eq!(id2, 2); + assert_eq!(db.read_all_cameras().unwrap().len(), 2); + } + + #[test] + fn read_missing_camera_is_not_found() { + let db = Database::open_in_memory().unwrap(); + match db.read_camera(123) { + Err(crate::Error::NotFound(_)) => {} + other => panic!("expected NotFound, got {other:?}"), + } + } + + #[test] + fn image_round_trip() { + let db = Database::open_in_memory().unwrap(); + db.write_camera(&sample_camera(1), true).unwrap(); + + let id = db.write_image(None, "img0001.jpg", 1).unwrap(); + assert_eq!(id, 1); + assert!(db.exists_image(1).unwrap()); + assert_eq!(db.num_images().unwrap(), 1); + + let id2 = db.write_image(Some(42), "img0042.jpg", 1).unwrap(); + assert_eq!(id2, 42); + + let images = db.read_all_images().unwrap(); + assert_eq!(images.len(), 2); + assert_eq!(images[0], (1, "img0001.jpg".to_string(), 1)); + assert_eq!(images[1], (42, "img0042.jpg".to_string(), 1)); + } + + #[test] + fn keypoints_round_trip() { + let db = Database::open_in_memory().unwrap(); + let kps: FeatureKeypoints = vec![ + FeatureKeypoint::new(1.5, 2.5), + FeatureKeypoint::with_shape(10.0, 20.0, 1.0, 0.5, -0.5, 2.0), + ]; + assert!(!db.exists_keypoints(5).unwrap()); + db.write_keypoints(5, &kps).unwrap(); + assert!(db.exists_keypoints(5).unwrap()); + + let read = db.read_keypoints(5).unwrap(); + assert_eq!(read.len(), 2); + assert_eq!(read[0].x, 1.5); + assert_eq!(read[0].y, 2.5); + assert_eq!(read[0].a11, 1.0); + assert_eq!(read[0].a12, 0.0); + assert_eq!(read[1].a12, 0.5); + assert_eq!(read[1].a21, -0.5); + assert_eq!(read[1].a22, 2.0); + assert_eq!(db.num_keypoints().unwrap(), 2); + } + + #[test] + fn keypoints_missing_is_empty() { + let db = Database::open_in_memory().unwrap(); + assert!(db.read_keypoints(99).unwrap().is_empty()); + } + + #[test] + fn descriptors_round_trip() { + let db = Database::open_in_memory().unwrap(); + let desc = FeatureDescriptors { + rows: 2, + cols: 4, + data: vec![1, 2, 3, 4, 250, 251, 252, 253], + }; + assert!(!db.exists_descriptors(3).unwrap()); + db.write_descriptors(3, &desc).unwrap(); + assert!(db.exists_descriptors(3).unwrap()); + + let read = db.read_descriptors(3).unwrap(); + assert_eq!(read.rows, 2); + assert_eq!(read.cols, 4); + assert_eq!(read.data, desc.data); + assert_eq!(db.num_descriptors().unwrap(), 2); + } + + #[test] + fn descriptors_length_mismatch_is_error() { + let db = Database::open_in_memory().unwrap(); + let bad = FeatureDescriptors { + rows: 2, + cols: 4, + data: vec![1, 2, 3], + }; + assert!(matches!( + db.write_descriptors(1, &bad), + Err(crate::Error::InvalidArgument(_)) + )); + } + + #[test] + fn matches_round_trip_no_swap() { + let db = Database::open_in_memory().unwrap(); + // image_id1 < image_id2 => no swap. + let matches: FeatureMatches = vec![FeatureMatch::new(0, 10), FeatureMatch::new(1, 11)]; + assert!(!db.exists_matches(1, 2).unwrap()); + db.write_matches(1, 2, &matches).unwrap(); + assert!(db.exists_matches(1, 2).unwrap()); + // Existence is symmetric in the pair. + assert!(db.exists_matches(2, 1).unwrap()); + + let read = db.read_matches(1, 2).unwrap(); + assert_eq!(read.len(), 2); + assert_eq!(read[0].point2d_idx1, 0); + assert_eq!(read[0].point2d_idx2, 10); + assert_eq!(read[1].point2d_idx1, 1); + assert_eq!(read[1].point2d_idx2, 11); + assert_eq!(db.num_matches().unwrap(), 2); + } + + #[test] + fn matches_round_trip_with_swap() { + let db = Database::open_in_memory().unwrap(); + // image_id1 > image_id2 => stored swapped, but the API hides that. + let matches: FeatureMatches = vec![FeatureMatch::new(5, 7), FeatureMatch::new(6, 8)]; + db.write_matches(9, 2, &matches).unwrap(); + + // Reading back with the same orientation recovers the original idx1/idx2. + let read = db.read_matches(9, 2).unwrap(); + assert_eq!(read.len(), 2); + assert_eq!(read[0].point2d_idx1, 5); + assert_eq!(read[0].point2d_idx2, 7); + + // Reading in the canonical (smaller-first) orientation swaps them. + let read_swapped = db.read_matches(2, 9).unwrap(); + assert_eq!(read_swapped[0].point2d_idx1, 7); + assert_eq!(read_swapped[0].point2d_idx2, 5); + } + + #[test] + fn matches_missing_is_empty() { + let db = Database::open_in_memory().unwrap(); + assert!(db.read_matches(1, 2).unwrap().is_empty()); + } + + #[test] + fn replace_overwrites_existing_blob() { + let db = Database::open_in_memory().unwrap(); + db.write_keypoints(1, &vec![FeatureKeypoint::new(0.0, 0.0)]).unwrap(); + db.write_keypoints(1, &vec![FeatureKeypoint::new(1.0, 1.0), FeatureKeypoint::new(2.0, 2.0)]) + .unwrap(); + let read = db.read_keypoints(1).unwrap(); + assert_eq!(read.len(), 2); + assert_eq!(db.num_keypoints().unwrap(), 2); + } + + #[test] + fn transaction_begin_commit() { + let db = Database::open_in_memory().unwrap(); + db.begin().unwrap(); + db.write_camera(&sample_camera(1), true).unwrap(); + db.write_image(Some(1), "a.jpg", 1).unwrap(); + db.commit().unwrap(); + assert_eq!(db.num_cameras().unwrap(), 1); + assert_eq!(db.num_images().unwrap(), 1); + } + + #[test] + fn open_creates_file_on_disk() { + let dir = tempfile::tempdir().unwrap(); + let path = dir.path().join("database.db"); + { + let db = Database::open(&path).unwrap(); + db.write_camera(&sample_camera(1), true).unwrap(); + } + assert!(path.exists()); + // Reopen and verify persistence. + let db = Database::open(&path).unwrap(); + assert_eq!(db.num_cameras().unwrap(), 1); + assert!(db.exists_camera(1).unwrap()); + } + + #[test] + fn f64_blob_round_trip() { + let values = vec![1.0_f64, -2.5, 3.125, 1e-9]; + let blob = f64_slice_to_blob(&values); + assert_eq!(blob.len(), values.len() * 8); + let back = blob_to_f64_vec(&blob).unwrap(); + assert_eq!(back, values); + } + + #[test] + fn f64_blob_bad_length_is_malformed() { + assert!(matches!( + blob_to_f64_vec(&[0, 1, 2]), + Err(crate::Error::Malformed(_)) + )); + } +} diff --git a/src/error.rs b/src/error.rs new file mode 100644 index 0000000..cfa44be --- /dev/null +++ b/src/error.rs @@ -0,0 +1,50 @@ +//! Crate-wide error type. +//! +//! Every fallible operation in this crate returns [`Result`], an alias for +//! `std::result::Result`. + +/// The error type returned by all fallible operations in this crate. +#[derive(Debug, thiserror::Error)] +#[non_exhaustive] +pub enum Error { + /// An underlying I/O failure (file not found, permission denied, truncated read…). + #[error("I/O error: {0}")] + Io(#[from] std::io::Error), + + /// A text/number could not be parsed while reading a model file. + #[error("parse error: {0}")] + Parse(String), + + /// A binary blob or file had an unexpected layout or length. + #[error("malformed data: {0}")] + Malformed(String), + + /// A camera-model name or id did not correspond to any known model. + #[error("unknown camera model: {0}")] + UnknownCameraModel(String), + + /// The number of parameters did not match the camera model. + #[error("invalid camera parameters: {0}")] + InvalidParams(String), + + /// A lookup (camera/image/point id, etc.) failed. + #[error("not found: {0}")] + NotFound(String), + + /// An operation was given inconsistent or out-of-range arguments. + #[error("invalid argument: {0}")] + InvalidArgument(String), + + /// A feature/algorithm exists in the public API but is not implemented yet + /// in this pure-Rust port (e.g. SIFT extraction, incremental mapping, dense MVS). + #[error("not implemented in the pure-Rust port: {0}")] + Unimplemented(&'static str), + + /// An error returned by the SQLite database backend. + #[cfg(feature = "database")] + #[error("database error: {0}")] + Database(#[from] rusqlite::Error), +} + +/// A specialized [`Result`](std::result::Result) for this crate. +pub type Result = std::result::Result; diff --git a/src/estimators/mod.rs b/src/estimators/mod.rs new file mode 100644 index 0000000..0bead40 --- /dev/null +++ b/src/estimators/mod.rs @@ -0,0 +1,463 @@ +//! Robust geometry estimators, mirroring PyCOLMAP's `pycolmap.estimators`. +//! +//! This module exposes the option structs (RANSAC and friends) and the +//! estimator function surface used throughout COLMAP's reconstruction pipeline: +//! essential / fundamental / homography matrices, absolute and relative pose, +//! rigid and similarity alignment, and point triangulation. +//! +//! The closed-form estimators that reduce to linear algebra already available +//! in [`crate::geometry`] and [`crate::math`] are implemented here: +//! +//! - [`estimate_rigid3d`] — Umeyama alignment **without** scale. +//! - [`estimate_sim3d`] — Umeyama alignment **with** scale. +//! - [`triangulate_point`] — delegates to [`crate::geometry::triangulation`]. +//! +//! The remaining estimators require a full RANSAC loop plus minimal solvers +//! (5-point, 7-point, 8-point, P3P/EPnP, …) that are not yet ported and return +//! [`crate::Error::Unimplemented`]. + +use crate::geometry::{Rigid3d, Sim3d}; +use crate::math::{Mat3, Mat3x4, UnitQuat, Vec2, Vec3}; + +/// Options controlling a RANSAC robust-estimation loop. +/// +/// Mirrors COLMAP's `RANSACOptions`. The defaults match PyCOLMAP's defaults. +#[derive(Debug, Clone, Copy, PartialEq)] +#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))] +pub struct RansacOptions { + /// Maximum residual error (in pixels, or model-specific units) for a sample + /// to be considered an inlier. + pub max_error: f64, + /// Assumed minimum inlier ratio, used to bound the number of trials. + pub min_inlier_ratio: f64, + /// Target confidence (probability) that at least one outlier-free sample is + /// drawn during the trials. + pub confidence: f64, + /// Minimum number of RANSAC iterations to run regardless of confidence. + pub min_num_trials: usize, + /// Maximum number of RANSAC iterations to run. + pub max_num_trials: usize, +} + +impl Default for RansacOptions { + #[inline] + fn default() -> Self { + Self { + max_error: 4.0, + min_inlier_ratio: 0.1, + confidence: 0.9999, + min_num_trials: 100, + max_num_trials: 10000, + } + } +} + +/// Options for two-view geometry estimation, mirroring COLMAP's +/// `TwoViewGeometryOptions`. +#[derive(Debug, Clone, Copy, PartialEq)] +#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))] +pub struct TwoViewGeometryOptions { + /// Minimum number of inlier correspondences for a two-view geometry to be + /// accepted. + pub min_num_inliers: usize, + /// RANSAC options used for the underlying matrix estimation. + pub ransac: RansacOptions, +} + +impl Default for TwoViewGeometryOptions { + #[inline] + fn default() -> Self { + Self { min_num_inliers: 15, ransac: RansacOptions::default() } + } +} + +/// Options for absolute-pose (PnP) estimation, mirroring COLMAP's +/// `AbsolutePoseEstimationOptions`. +#[derive(Debug, Clone, Copy, PartialEq)] +#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))] +pub struct AbsolutePoseEstimationOptions { + /// RANSAC options used for the robust pose estimation. + pub ransac: RansacOptions, +} + +impl Default for AbsolutePoseEstimationOptions { + #[inline] + fn default() -> Self { + Self { ransac: RansacOptions::default() } + } +} + +/// Outcome of a robust estimation: whether it succeeded, how many inliers were +/// found and the per-correspondence inlier mask. +#[derive(Debug, Clone, PartialEq)] +#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))] +pub struct InlierReport { + /// Whether the estimation produced a valid model. + pub success: bool, + /// The number of inlier correspondences (`inlier_mask.iter().filter(|&&b| b).count()`). + pub num_inliers: usize, + /// A boolean mask, one entry per input correspondence, `true` for inliers. + pub inlier_mask: Vec, +} + +impl InlierReport { + /// Builds a report from an inlier mask, counting the inliers and marking the + /// report successful when at least one inlier is present. + #[inline] + pub fn from_mask(inlier_mask: Vec) -> Self { + let num_inliers = inlier_mask.iter().filter(|&&b| b).count(); + Self { success: num_inliers > 0, num_inliers, inlier_mask } + } + + /// A failed, empty report. + #[inline] + pub fn failure() -> Self { + Self { success: false, num_inliers: 0, inlier_mask: Vec::new() } + } +} + +/// Robustly estimates the essential matrix from bearing-ray correspondences. +/// +/// Requires a 5-point minimal solver inside a RANSAC loop, which is not yet +/// ported; returns [`crate::Error::Unimplemented`]. +pub fn estimate_essential_matrix( + rays1: &[Vec3], + rays2: &[Vec3], + _opts: &RansacOptions, +) -> crate::Result<(Mat3, InlierReport)> { + let _ = (rays1, rays2); + Err(crate::Error::Unimplemented("RANSAC essential matrix")) +} + +/// Robustly estimates the fundamental matrix from pixel correspondences. +/// +/// Requires a 7-/8-point minimal solver inside a RANSAC loop, which is not yet +/// ported; returns [`crate::Error::Unimplemented`]. +pub fn estimate_fundamental_matrix( + points1: &[Vec2], + points2: &[Vec2], + _opts: &RansacOptions, +) -> crate::Result<(Mat3, InlierReport)> { + let _ = (points1, points2); + Err(crate::Error::Unimplemented("RANSAC fundamental matrix")) +} + +/// Robustly estimates a homography from pixel correspondences. +/// +/// Requires a 4-point DLT solver inside a RANSAC loop, which is not yet ported; +/// returns [`crate::Error::Unimplemented`]. +pub fn estimate_homography_matrix( + points1: &[Vec2], + points2: &[Vec2], + _opts: &RansacOptions, +) -> crate::Result<(Mat3, InlierReport)> { + let _ = (points1, points2); + Err(crate::Error::Unimplemented("RANSAC homography matrix")) +} + +/// Robustly estimates the absolute pose (`cam_from_world`) of a camera from +/// 2D-3D correspondences (PnP). +/// +/// Requires a P3P/EPnP minimal solver inside a RANSAC loop, which is not yet +/// ported; returns [`crate::Error::Unimplemented`]. +pub fn estimate_absolute_pose( + points2d: &[Vec2], + points3d: &[Vec3], + camera: &crate::scene::Camera, + _opts: &AbsolutePoseEstimationOptions, +) -> crate::Result<(Rigid3d, InlierReport)> { + let _ = (points2d, points3d, camera); + Err(crate::Error::Unimplemented("PnP absolute pose")) +} + +/// Robustly estimates the relative pose (`cam2_from_cam1`) between two cameras +/// from bearing-ray correspondences. +/// +/// Requires essential-matrix estimation plus pose decomposition inside a RANSAC +/// loop, which is not yet ported; returns [`crate::Error::Unimplemented`]. +pub fn estimate_relative_pose( + rays1: &[Vec3], + rays2: &[Vec3], + _opts: &RansacOptions, +) -> crate::Result<(Rigid3d, InlierReport)> { + let _ = (rays1, rays2); + Err(crate::Error::Unimplemented("RANSAC relative pose")) +} + +/// Estimates the rigid transform `tgt_from_src` that best aligns `src` to `tgt` +/// in the least-squares sense, using the Umeyama algorithm **without** scale. +/// +/// Given corresponding points `src[i]` and `tgt[i]`, returns the [`Rigid3d`] +/// `T` minimizing `Σ ‖T·src[i] − tgt[i]‖²`. The rotation is recovered from the +/// SVD of the cross-covariance matrix, with a reflection correction so that the +/// result is always a proper rotation (`det = +1`). +/// +/// # Errors +/// Returns [`crate::Error::InvalidArgument`] if the inputs have different +/// lengths or fewer than three correspondences. +pub fn estimate_rigid3d(src: &[Vec3], tgt: &[Vec3]) -> crate::Result { + let (rotation, _scale, translation) = umeyama(src, tgt, false)?; + Ok(Rigid3d::new(rotation, translation)) +} + +/// Estimates the similarity transform `tgt_from_src` that best aligns `src` to +/// `tgt` in the least-squares sense, using the Umeyama algorithm **with** scale. +/// +/// Given corresponding points `src[i]` and `tgt[i]`, returns the [`Sim3d`] `T` +/// minimizing `Σ ‖T·src[i] − tgt[i]‖²`, recovering the uniform scale, rotation +/// and translation. +/// +/// # Errors +/// Returns [`crate::Error::InvalidArgument`] if the inputs have different +/// lengths or fewer than three correspondences. +pub fn estimate_sim3d(src: &[Vec3], tgt: &[Vec3]) -> crate::Result { + let (rotation, scale, translation) = umeyama(src, tgt, true)?; + Ok(Sim3d::new(scale, rotation, translation)) +} + +/// Triangulates a single 3D point from two views. +/// +/// Delegates to [`crate::geometry::triangulation::triangulate_point`]. `x1` and +/// `x2` are the (normalized or pixel, depending on the projection matrices) +/// image observations in the two views whose `cam_from_world` projection +/// matrices are given. Returns `None` if the point cannot be triangulated +/// (e.g. degenerate / parallel rays). +pub fn triangulate_point( + cam_from_world1: &Mat3x4, + cam_from_world2: &Mat3x4, + x1: &Vec2, + x2: &Vec2, +) -> Option { + crate::geometry::triangulation::triangulate_point(cam_from_world1, cam_from_world2, x1, x2) +} + +/// Core Umeyama least-squares similarity/rigid alignment. +/// +/// Returns `(rotation, scale, translation)` of the transform mapping `src` onto +/// `tgt`. When `with_scale` is `false` the scale is forced to `1.0`. +/// +/// Reference: S. Umeyama, "Least-squares estimation of transformation +/// parameters between two point patterns", PAMI 1991. +fn umeyama(src: &[Vec3], tgt: &[Vec3], with_scale: bool) -> crate::Result<(UnitQuat, f64, Vec3)> { + if src.len() != tgt.len() { + return Err(crate::Error::InvalidArgument(format!( + "estimate transform: src ({}) and tgt ({}) must have equal length", + src.len(), + tgt.len() + ))); + } + let n = src.len(); + if n < 3 { + return Err(crate::Error::InvalidArgument(format!( + "estimate transform: need at least 3 correspondences, got {n}" + ))); + } + + let inv_n = 1.0 / n as f64; + + // Centroids of both point sets. + let mut mean_src = Vec3::zeros(); + let mut mean_tgt = Vec3::zeros(); + for (s, t) in src.iter().zip(tgt.iter()) { + mean_src += s; + mean_tgt += t; + } + mean_src *= inv_n; + mean_tgt *= inv_n; + + // Cross-covariance matrix Σ = (1/n) Σ (tgt-μt)(src-μs)^T and source variance. + let mut sigma = Mat3::zeros(); + let mut var_src = 0.0; + for (s, t) in src.iter().zip(tgt.iter()) { + let ds = s - mean_src; + let dt = t - mean_tgt; + sigma += dt * ds.transpose(); + var_src += ds.norm_squared(); + } + sigma *= inv_n; + var_src *= inv_n; + + // SVD of the cross-covariance. + let svd = nalgebra::SVD::new(sigma, true, true); + let u = svd + .u + .ok_or_else(|| crate::Error::InvalidArgument("transform SVD failed (U)".to_string()))?; + let v_t = svd + .v_t + .ok_or_else(|| crate::Error::InvalidArgument("transform SVD failed (V^T)".to_string()))?; + let singular = svd.singular_values; + + // Reflection-correction matrix S = diag(1, 1, sign(det(U)·det(V))). + let det = u.determinant() * v_t.determinant(); + let mut s_diag = Vec3::new(1.0, 1.0, 1.0); + if det < 0.0 { + s_diag[2] = -1.0; + } + + // Rotation R = U · diag(s) · V^T. + let rotation_mat = u * Mat3::from_diagonal(&s_diag) * v_t; + + // Scale: c = (1/var_src) · trace(D·S), with D the singular values. + let scale = if with_scale { + if var_src <= f64::EPSILON { + return Err(crate::Error::InvalidArgument( + "estimate transform: degenerate source point set (zero variance)".to_string(), + )); + } + let trace_ds = singular[0] * s_diag[0] + singular[1] * s_diag[1] + singular[2] * s_diag[2]; + trace_ds / var_src + } else { + 1.0 + }; + + // Translation: t = μt − c·R·μs. + let translation = mean_tgt - scale * (rotation_mat * mean_src); + + let rotation = UnitQuat::from_rotation_matrix(&nalgebra::Rotation3::from_matrix_unchecked( + rotation_mat, + )); + + Ok((rotation, scale, translation)) +} + +#[cfg(test)] +mod tests { + use super::*; + use approx::assert_relative_eq; + + fn sample_points() -> Vec { + vec![ + Vec3::new(0.0, 0.0, 0.0), + Vec3::new(1.0, 0.0, 0.0), + Vec3::new(0.0, 1.0, 0.0), + Vec3::new(0.0, 0.0, 1.0), + Vec3::new(1.0, 2.0, 3.0), + Vec3::new(-2.0, 0.5, 4.0), + Vec3::new(3.0, -1.0, 2.0), + ] + } + + #[test] + fn defaults_match_pycolmap() { + let r = RansacOptions::default(); + assert_eq!(r.max_error, 4.0); + assert_eq!(r.min_inlier_ratio, 0.1); + assert_eq!(r.confidence, 0.9999); + assert_eq!(r.min_num_trials, 100); + assert_eq!(r.max_num_trials, 10000); + + let tvg = TwoViewGeometryOptions::default(); + assert_eq!(tvg.min_num_inliers, 15); + assert_eq!(tvg.ransac, RansacOptions::default()); + + let abs = AbsolutePoseEstimationOptions::default(); + assert_eq!(abs.ransac, RansacOptions::default()); + } + + #[test] + fn inlier_report_from_mask() { + let r = InlierReport::from_mask(vec![true, false, true, true]); + assert!(r.success); + assert_eq!(r.num_inliers, 3); + assert_eq!(r.inlier_mask.len(), 4); + + let empty = InlierReport::from_mask(vec![false, false]); + assert!(!empty.success); + assert_eq!(empty.num_inliers, 0); + + let fail = InlierReport::failure(); + assert!(!fail.success); + assert_eq!(fail.num_inliers, 0); + assert!(fail.inlier_mask.is_empty()); + } + + #[test] + fn estimate_rigid3d_recovers_known_transform() { + let truth = Rigid3d::new( + UnitQuat::new(Vec3::new(0.3, -0.2, 0.7)), + Vec3::new(1.0, -2.0, 0.5), + ); + let src = sample_points(); + let tgt: Vec = src.iter().map(|p| truth * *p).collect(); + + let est = estimate_rigid3d(&src, &tgt).unwrap(); + + // The recovered transform must reproduce every correspondence. + for (s, t) in src.iter().zip(tgt.iter()) { + assert_relative_eq!(est * *s, *t, epsilon = 1e-9); + } + // And match the ground-truth rotation/translation. + assert_relative_eq!(est.translation, truth.translation, epsilon = 1e-9); + assert_relative_eq!( + est.rotation.to_rotation_matrix().matrix(), + truth.rotation.to_rotation_matrix().matrix(), + epsilon = 1e-9 + ); + } + + #[test] + fn estimate_rigid3d_ignores_scale() { + // A pure rigid fit of scaled data should still be a rotation+translation + // (scale forced to 1), so it will not reproduce the scaled points. + let truth = Rigid3d::new(UnitQuat::identity(), Vec3::new(0.0, 0.0, 0.0)); + let src = sample_points(); + let tgt: Vec = src.iter().map(|p| truth * (2.0 * *p)).collect(); + let est = estimate_rigid3d(&src, &tgt).unwrap(); + // Rotation stays a proper rotation (det == 1). + let det = est.rotation.to_rotation_matrix().matrix().determinant(); + assert_relative_eq!(det, 1.0, epsilon = 1e-9); + } + + #[test] + fn estimate_sim3d_recovers_known_transform() { + let truth = Sim3d::new( + 2.5, + UnitQuat::new(Vec3::new(0.2, 0.1, -0.4)), + Vec3::new(1.0, -2.0, 3.0), + ); + let src = sample_points(); + let tgt: Vec = src.iter().map(|p| truth * *p).collect(); + + let est = estimate_sim3d(&src, &tgt).unwrap(); + + assert_relative_eq!(est.scale, truth.scale, epsilon = 1e-9); + assert_relative_eq!(est.translation, truth.translation, epsilon = 1e-9); + for (s, t) in src.iter().zip(tgt.iter()) { + assert_relative_eq!(est * *s, *t, epsilon = 1e-8); + } + } + + #[test] + fn estimate_transform_rejects_bad_input() { + let a = vec![Vec3::zeros(), Vec3::new(1.0, 0.0, 0.0)]; + let b = vec![Vec3::zeros()]; + assert!(estimate_rigid3d(&a, &b).is_err()); + + // Too few points (need at least 3). + let two = vec![Vec3::zeros(), Vec3::new(1.0, 0.0, 0.0)]; + assert!(estimate_sim3d(&two, &two).is_err()); + } + + #[test] + fn unimplemented_estimators_report_unimplemented() { + let opts = RansacOptions::default(); + let empty: Vec = Vec::new(); + let empty2: Vec = Vec::new(); + assert!(matches!( + estimate_essential_matrix(&empty, &empty, &opts), + Err(crate::Error::Unimplemented(_)) + )); + assert!(matches!( + estimate_fundamental_matrix(&empty2, &empty2, &opts), + Err(crate::Error::Unimplemented(_)) + )); + assert!(matches!( + estimate_homography_matrix(&empty2, &empty2, &opts), + Err(crate::Error::Unimplemented(_)) + )); + assert!(matches!( + estimate_relative_pose(&empty, &empty, &opts), + Err(crate::Error::Unimplemented(_)) + )); + } +} diff --git a/src/feature/mod.rs b/src/feature/mod.rs new file mode 100644 index 0000000..8b099da --- /dev/null +++ b/src/feature/mod.rs @@ -0,0 +1,350 @@ +//! Local feature extraction and matching. +//! +//! Mirrors `colmap/feature/*` and the `pycolmap` feature API: it provides the +//! plain-data feature types ([`FeatureKeypoint`], [`FeatureMatch`], +//! [`FeatureDescriptors`]), the option structs that configure SIFT extraction +//! and matching, and the high-level pipeline entry points +//! ([`extract_features`], [`match_exhaustive`], …). +//! +//! The numerical algorithms (SIFT detection/description, descriptor matching, +//! vocabulary-tree retrieval) are heavy and not yet ported; the corresponding +//! functions return [`Error::Unimplemented`](crate::Error::Unimplemented) so the +//! public surface is complete and stable to build against. + +mod types; + +pub use types::{ + FeatureDescriptors, FeatureKeypoint, FeatureKeypoints, FeatureMatch, FeatureMatches, +}; + +use std::path::Path; + +// --------------------------------------------------------------------------- +// Extractor / matcher kinds +// --------------------------------------------------------------------------- + +/// The local-feature extractor to use. +#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)] +#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))] +#[derive(Default)] +pub enum FeatureExtractorType { + /// Classic SIFT features (COLMAP default). + #[default] + Sift, + /// ALIKED learned features. + Aliked, +} + + +/// The descriptor matcher to use. +#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)] +#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))] +#[derive(Default)] +pub enum FeatureMatcherType { + /// SIFT nearest-neighbour matching with ratio/cross-check tests. + #[default] + Sift, + /// ALIKED descriptor matching. + Aliked, + /// LightGlue learned matcher. + LightGlue, + /// Plain brute-force nearest-neighbour matching. + BruteForce, +} + + +// --------------------------------------------------------------------------- +// Option structs +// --------------------------------------------------------------------------- + +/// Options controlling SIFT feature extraction. +/// +/// Field defaults match COLMAP's `SiftExtractionOptions`. +#[derive(Debug, Clone, PartialEq)] +#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))] +pub struct SiftExtractionOptions { + /// Maximum image dimension; larger images are downscaled before extraction. + pub max_image_size: i32, + /// Maximum number of features to keep per image (strongest are kept). + pub max_num_features: i32, + /// Index of the first octave in the scale-space pyramid. + pub first_octave: i32, + /// Number of octaves in the scale-space pyramid. + pub num_octaves: i32, + /// Number of levels per octave. + pub octave_resolution: i32, + /// Detection threshold on the difference-of-Gaussians response. + pub peak_threshold: f64, + /// Edge-response rejection threshold (ratio of principal curvatures). + pub edge_threshold: f64, + /// Maximum number of orientations assigned to a single keypoint. + pub max_num_orientations: i32, + /// If `true`, assign a fixed upright orientation instead of estimating it. + pub upright: bool, + /// If `true`, enable domain-size pooling for more robust descriptors. + pub domain_size_pooling: bool, +} + +impl Default for SiftExtractionOptions { + fn default() -> Self { + Self { + max_image_size: 3200, + max_num_features: 8192, + first_octave: -1, + num_octaves: 4, + octave_resolution: 3, + peak_threshold: 0.0066, + edge_threshold: 10.0, + max_num_orientations: 2, + upright: false, + domain_size_pooling: false, + } + } +} + +/// Options controlling SIFT descriptor matching. +/// +/// Field defaults match COLMAP's `SiftMatchingOptions`. +#[derive(Debug, Clone, PartialEq)] +#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))] +pub struct SiftMatchingOptions { + /// Maximum allowed ratio of best to second-best descriptor distance. + pub max_ratio: f64, + /// Maximum allowed descriptor distance for a match. + pub max_distance: f64, + /// If `true`, only keep mutually-best (cross-checked) matches. + pub cross_check: bool, + /// Maximum number of matches to keep per image pair. + pub max_num_matches: i32, + /// If `true`, perform an extra guided-matching pass using the epipolar geometry. + pub guided_matching: bool, +} + +impl Default for SiftMatchingOptions { + fn default() -> Self { + Self { + max_ratio: 0.8, + max_distance: 0.7, + cross_check: true, + max_num_matches: 32768, + guided_matching: false, + } + } +} + +/// Top-level options for the feature-extraction stage. +#[derive(Debug, Clone, PartialEq, Default)] +#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))] +pub struct FeatureExtractionOptions { + /// SIFT-specific extraction options. + pub sift: SiftExtractionOptions, +} + +/// Top-level options for the feature-matching stage. +#[derive(Debug, Clone, PartialEq, Default)] +#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))] +pub struct FeatureMatchingOptions { + /// SIFT-specific matching options. + pub sift: SiftMatchingOptions, +} + +// --------------------------------------------------------------------------- +// Helper conversions +// --------------------------------------------------------------------------- + +/// Flattens keypoints into a row-major `rows * 6` `f32` buffer. +/// +/// Each row holds `[x, y, a11, a12, a21, a22]`, matching the column layout +/// COLMAP uses when storing keypoints in the feature database. +pub fn keypoints_to_matrix(keypoints: &FeatureKeypoints) -> Vec { + let mut out = Vec::with_capacity(keypoints.len() * 6); + for kp in keypoints { + out.extend_from_slice(&[kp.x, kp.y, kp.a11, kp.a12, kp.a21, kp.a22]); + } + out +} + +/// Rebuilds keypoints from a row-major `rows * 6` `f32` buffer produced by +/// [`keypoints_to_matrix`]. +/// +/// Returns [`Error::InvalidArgument`](crate::Error::InvalidArgument) if the +/// buffer length is not a multiple of `6`. +pub fn keypoints_from_matrix(data: &[f32]) -> crate::Result { + if data.len() % 6 != 0 { + return Err(crate::Error::InvalidArgument(format!( + "keypoint buffer length {} is not a multiple of 6", + data.len() + ))); + } + let mut out = FeatureKeypoints::with_capacity(data.len() / 6); + for chunk in data.chunks_exact(6) { + out.push(FeatureKeypoint::with_shape( + chunk[0], chunk[1], chunk[2], chunk[3], chunk[4], chunk[5], + )); + } + Ok(out) +} + +// --------------------------------------------------------------------------- +// Pipeline entry points (heavy algorithms — not yet ported) +// --------------------------------------------------------------------------- + +/// Extracts local features from every image under `image_path` and writes the +/// resulting keypoints and descriptors into the feature database at +/// `database_path`. +/// +/// # Errors +/// +/// Returns [`Error::Unimplemented`](crate::Error::Unimplemented) — SIFT +/// extraction is not yet ported. +pub fn extract_features( + _database_path: &Path, + _image_path: &Path, + _options: &FeatureExtractionOptions, +) -> crate::Result<()> { + Err(crate::Error::Unimplemented("SIFT feature extraction")) +} + +/// Matches features between all pairs of images in `database_path` (exhaustive +/// matching) and stores the matches back into the database. +/// +/// # Errors +/// +/// Returns [`Error::Unimplemented`](crate::Error::Unimplemented) — exhaustive +/// matching is not yet ported. +pub fn match_exhaustive( + _database_path: &Path, + _options: &FeatureMatchingOptions, +) -> crate::Result<()> { + Err(crate::Error::Unimplemented("exhaustive feature matching")) +} + +/// Matches features between images that are nearby in capture order +/// (sequential matching) and stores the matches back into the database. +/// +/// # Errors +/// +/// Returns [`Error::Unimplemented`](crate::Error::Unimplemented) — sequential +/// matching is not yet ported. +pub fn match_sequential( + _database_path: &Path, + _options: &FeatureMatchingOptions, +) -> crate::Result<()> { + Err(crate::Error::Unimplemented("sequential feature matching")) +} + +/// Matches features between images that are spatially close according to their +/// location priors (spatial matching) and stores the matches back into the +/// database. +/// +/// # Errors +/// +/// Returns [`Error::Unimplemented`](crate::Error::Unimplemented) — spatial +/// matching is not yet ported. +pub fn match_spatial( + _database_path: &Path, + _options: &FeatureMatchingOptions, +) -> crate::Result<()> { + Err(crate::Error::Unimplemented("spatial feature matching")) +} + +/// Matches features between visually similar images selected via a vocabulary +/// tree (vocab-tree matching) and stores the matches back into the database. +/// +/// # Errors +/// +/// Returns [`Error::Unimplemented`](crate::Error::Unimplemented) — vocab-tree +/// matching is not yet ported. +pub fn match_vocabtree( + _database_path: &Path, + _options: &FeatureMatchingOptions, +) -> crate::Result<()> { + Err(crate::Error::Unimplemented("vocabulary-tree feature matching")) +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn sift_extraction_defaults_are_sane() { + let o = SiftExtractionOptions::default(); + assert_eq!(o.max_image_size, 3200); + assert_eq!(o.max_num_features, 8192); + assert_eq!(o.first_octave, -1); + assert_eq!(o.num_octaves, 4); + assert_eq!(o.octave_resolution, 3); + assert!((o.peak_threshold - 0.0066).abs() < 1e-12); + assert!((o.edge_threshold - 10.0).abs() < 1e-12); + assert_eq!(o.max_num_orientations, 2); + assert!(!o.upright); + assert!(!o.domain_size_pooling); + } + + #[test] + fn sift_matching_defaults_are_sane() { + let o = SiftMatchingOptions::default(); + assert!((o.max_ratio - 0.8).abs() < 1e-12); + assert!((o.max_distance - 0.7).abs() < 1e-12); + assert!(o.cross_check); + assert_eq!(o.max_num_matches, 32768); + assert!(!o.guided_matching); + } + + #[test] + fn top_level_options_default_delegates() { + let e = FeatureExtractionOptions::default(); + assert_eq!(e.sift, SiftExtractionOptions::default()); + let m = FeatureMatchingOptions::default(); + assert_eq!(m.sift, SiftMatchingOptions::default()); + } + + #[test] + fn extractor_and_matcher_defaults() { + assert_eq!(FeatureExtractorType::default(), FeatureExtractorType::Sift); + assert_eq!(FeatureMatcherType::default(), FeatureMatcherType::Sift); + } + + #[test] + fn keypoint_matrix_roundtrip() { + let kps: FeatureKeypoints = vec![ + FeatureKeypoint::new(1.0, 2.0), + FeatureKeypoint::with_shape(3.0, 4.0, 0.5, 0.1, -0.1, 0.5), + ]; + let flat = keypoints_to_matrix(&kps); + assert_eq!(flat.len(), 12); + assert_eq!(&flat[0..6], &[1.0, 2.0, 1.0, 0.0, 0.0, 1.0]); + let back = keypoints_from_matrix(&flat).unwrap(); + assert_eq!(back, kps); + + assert!(keypoints_from_matrix(&[1.0, 2.0, 3.0]).is_err()); + } + + #[test] + fn pipeline_entry_points_are_unimplemented() { + let db = Path::new("/tmp/colmap_test.db"); + let img = Path::new("/tmp/images"); + let eo = FeatureExtractionOptions::default(); + let mo = FeatureMatchingOptions::default(); + assert!(matches!( + extract_features(db, img, &eo), + Err(crate::Error::Unimplemented(_)) + )); + assert!(matches!( + match_exhaustive(db, &mo), + Err(crate::Error::Unimplemented(_)) + )); + assert!(matches!( + match_sequential(db, &mo), + Err(crate::Error::Unimplemented(_)) + )); + assert!(matches!( + match_spatial(db, &mo), + Err(crate::Error::Unimplemented(_)) + )); + assert!(matches!( + match_vocabtree(db, &mo), + Err(crate::Error::Unimplemented(_)) + )); + } +} diff --git a/src/feature/types.rs b/src/feature/types.rs new file mode 100644 index 0000000..e80a027 --- /dev/null +++ b/src/feature/types.rs @@ -0,0 +1,223 @@ +//! Small, plain-data feature types: keypoints, matches and descriptors. +//! +//! These mirror `colmap/feature/types.h`. They are the *frozen contract* that the +//! feature-extraction, matching, database and reconstruction code all share, so +//! the field names and helper signatures here must stay stable. +//! +//! A [`FeatureKeypoint`] stores the affine shape of a local feature as a +//! `2x2` matrix `[[a11, a12], [a21, a22]]`. For an upright, unit-scale keypoint +//! this matrix is the identity, so [`FeatureKeypoint::scale`] returns `1` and +//! [`FeatureKeypoint::orientation`] returns `0`. + +/// A single local feature: a 2D location plus its affine shape. +/// +/// The affine shape is the `2x2` matrix `[[a11, a12], [a21, a22]]` that maps the +/// canonical unit patch onto the image patch the descriptor was computed from. +#[derive(Debug, Clone, Copy, PartialEq)] +#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))] +pub struct FeatureKeypoint { + /// Sub-pixel `x` image coordinate of the keypoint center. + pub x: f32, + /// Sub-pixel `y` image coordinate of the keypoint center. + pub y: f32, + /// Element `(1, 1)` of the affine shape matrix. + pub a11: f32, + /// Element `(1, 2)` of the affine shape matrix. + pub a12: f32, + /// Element `(2, 1)` of the affine shape matrix. + pub a21: f32, + /// Element `(2, 2)` of the affine shape matrix. + pub a22: f32, +} + +impl FeatureKeypoint { + /// Creates an upright, unit-scale keypoint at `(x, y)`. + /// + /// The affine shape is set to the identity (`a11 = a22 = 1`, `a12 = a21 = 0`). + pub fn new(x: f32, y: f32) -> Self { + Self { + x, + y, + a11: 1.0, + a12: 0.0, + a21: 0.0, + a22: 1.0, + } + } + + /// Creates a keypoint at `(x, y)` with the explicit affine shape matrix + /// `[[a11, a12], [a21, a22]]`. + pub fn with_shape(x: f32, y: f32, a11: f32, a12: f32, a21: f32, a22: f32) -> Self { + Self { + x, + y, + a11, + a12, + a21, + a22, + } + } + + /// Returns the average scale of the affine shape. + /// + /// This is the mean of the two singular values of the shape matrix, + /// approximated as the geometric mean of the row norms. For an identity + /// (upright, unit-scale) shape this is `1`. + pub fn scale(&self) -> f32 { + 0.5 * (self.scale_x() + self.scale_y()) + } + + /// Returns the scale along the `x` axis (norm of the first column). + pub fn scale_x(&self) -> f32 { + (self.a11 * self.a11 + self.a21 * self.a21).sqrt() + } + + /// Returns the scale along the `y` axis (norm of the second column). + pub fn scale_y(&self) -> f32 { + (self.a12 * self.a12 + self.a22 * self.a22).sqrt() + } + + /// Returns the in-plane orientation in radians, in `(-pi, pi]`. + /// + /// This is the angle of the first column of the shape matrix. For an + /// identity (upright) shape this is `0`. + pub fn orientation(&self) -> f32 { + self.a21.atan2(self.a11) + } +} + +/// A list of [`FeatureKeypoint`]s detected in one image. +pub type FeatureKeypoints = Vec; + +/// A correspondence between a 2D point in image 1 and a 2D point in image 2. +/// +/// The two fields are indices into the respective images' keypoint lists. +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))] +pub struct FeatureMatch { + /// Index of the matched keypoint in the first image. + pub point2d_idx1: crate::types::Point2DIdx, + /// Index of the matched keypoint in the second image. + pub point2d_idx2: crate::types::Point2DIdx, +} + +impl FeatureMatch { + /// Creates a match between keypoint index `point2d_idx1` of image 1 and + /// keypoint index `point2d_idx2` of image 2. + pub fn new(point2d_idx1: crate::types::Point2DIdx, point2d_idx2: crate::types::Point2DIdx) -> Self { + Self { + point2d_idx1, + point2d_idx2, + } + } +} + +/// A list of [`FeatureMatch`]es between two images. +pub type FeatureMatches = Vec; + +/// A dense, row-major matrix of `uint8` feature descriptors. +/// +/// Each of the `rows` rows is one descriptor of length `cols`; the backing +/// `data` buffer therefore holds exactly `rows * cols` bytes in row-major order. +#[derive(Debug, Clone, PartialEq, Eq)] +#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))] +pub struct FeatureDescriptors { + /// Number of descriptors (one per keypoint). + pub rows: usize, + /// Dimensionality of each descriptor (e.g. `128` for SIFT). + pub cols: usize, + /// Row-major `rows * cols` buffer of descriptor bytes. + pub data: Vec, +} + +impl FeatureDescriptors { + /// Creates an empty `0 x cols` descriptor matrix. + pub fn new(cols: usize) -> Self { + Self { + rows: 0, + cols, + data: Vec::new(), + } + } + + /// Builds a descriptor matrix from raw row-major `data`. + /// + /// Returns [`Error::InvalidArgument`](crate::Error::InvalidArgument) if + /// `data.len()` is not exactly `rows * cols`. + pub fn from_data(rows: usize, cols: usize, data: Vec) -> crate::Result { + if data.len() != rows * cols { + return Err(crate::Error::InvalidArgument(format!( + "descriptor buffer has {} bytes, expected rows*cols = {}", + data.len(), + rows * cols + ))); + } + Ok(Self { rows, cols, data }) + } + + /// Returns the bytes of descriptor `row`, or `None` if out of range. + pub fn row(&self, row: usize) -> Option<&[u8]> { + if row >= self.rows { + return None; + } + let start = row * self.cols; + Some(&self.data[start..start + self.cols]) + } +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn identity_keypoint_has_unit_scale_and_zero_orientation() { + let kp = FeatureKeypoint::new(3.5, -2.0); + assert_eq!(kp.x, 3.5); + assert_eq!(kp.y, -2.0); + assert_eq!(kp.a11, 1.0); + assert_eq!(kp.a12, 0.0); + assert_eq!(kp.a21, 0.0); + assert_eq!(kp.a22, 1.0); + assert!((kp.scale() - 1.0).abs() < 1e-6); + assert!(kp.orientation().abs() < 1e-6); + } + + #[test] + fn with_shape_scale_and_orientation() { + // A pure scale of 2 with no rotation. + let kp = FeatureKeypoint::with_shape(0.0, 0.0, 2.0, 0.0, 0.0, 2.0); + assert!((kp.scale() - 2.0).abs() < 1e-6); + assert!(kp.orientation().abs() < 1e-6); + + // A 90-degree rotation: first column points along +y. + let kp = FeatureKeypoint::with_shape(0.0, 0.0, 0.0, -1.0, 1.0, 0.0); + assert!((kp.scale() - 1.0).abs() < 1e-6); + assert!((kp.orientation() - std::f32::consts::FRAC_PI_2).abs() < 1e-6); + } + + #[test] + fn feature_match_fields() { + let m = FeatureMatch::new(7, 11); + assert_eq!(m.point2d_idx1, 7); + assert_eq!(m.point2d_idx2, 11); + let matches: FeatureMatches = vec![m, FeatureMatch::new(1, 2)]; + assert_eq!(matches.len(), 2); + } + + #[test] + fn descriptors_roundtrip_and_validation() { + let d = FeatureDescriptors::from_data(2, 3, vec![1, 2, 3, 4, 5, 6]).unwrap(); + assert_eq!(d.rows, 2); + assert_eq!(d.cols, 3); + assert_eq!(d.row(0), Some(&[1u8, 2, 3][..])); + assert_eq!(d.row(1), Some(&[4u8, 5, 6][..])); + assert_eq!(d.row(2), None); + + let empty = FeatureDescriptors::new(128); + assert_eq!(empty.rows, 0); + assert_eq!(empty.cols, 128); + assert!(empty.data.is_empty()); + + assert!(FeatureDescriptors::from_data(2, 3, vec![1, 2, 3]).is_err()); + } +} diff --git a/src/geometry/essential_matrix.rs b/src/geometry/essential_matrix.rs new file mode 100644 index 0000000..3ccf5ba --- /dev/null +++ b/src/geometry/essential_matrix.rs @@ -0,0 +1,237 @@ +//! Essential and fundamental matrix construction and decomposition. +//! +//! Mirrors `colmap/geometry/essential_matrix.h`. The essential matrix `E` relates +//! normalized image rays of two views by the epipolar constraint +//! `ray2^T * E * ray1 = 0`, and encodes the relative pose `cam2_from_cam1`. + +use crate::geometry::pose::{check_cheirality, cross_product_matrix}; +use crate::geometry::Rigid3d; +use crate::math::{Mat3, UnitQuat, Vec3}; + +/// Builds the essential matrix `E = [t]_x * R` from a relative pose +/// `cam2_from_cam1`, with the translation normalized to unit length (the +/// essential matrix is only defined up to scale). +/// +/// # Examples +/// ``` +/// use colmap::geometry::essential_matrix::essential_matrix_from_pose; +/// use colmap::geometry::Rigid3d; +/// use colmap::math::{UnitQuat, Vec3}; +/// +/// let pose = Rigid3d::new(UnitQuat::identity(), Vec3::new(1.0, 0.0, 0.0)); +/// let e = essential_matrix_from_pose(&pose); +/// // For a point on the baseline the epipolar constraint holds. +/// let ray = Vec3::new(0.0, 1.0, 1.0); +/// assert!((ray.transpose() * e * ray)[0].abs() < 1e-12); +/// ``` +pub fn essential_matrix_from_pose(cam2_from_cam1: &Rigid3d) -> Mat3 { + let t = cam2_from_cam1.translation; + let t_norm = t.norm(); + let t_unit = if t_norm > f64::EPSILON { t / t_norm } else { t }; + cross_product_matrix(&t_unit) * cam2_from_cam1.rotation.to_rotation_matrix().into_inner() +} + +/// Decomposes an essential matrix into the two possible rotations and the +/// translation direction (up to sign): `(R1, R2, t)`. +/// +/// The four candidate relative poses are `(R1, t)`, `(R1, -t)`, `(R2, t)` and +/// `(R2, -t)`; the correct one is selected by a cheirality test (see +/// [`pose_from_essential_matrix`]). `t` is returned with unit length. +pub fn decompose_essential_matrix(e: &Mat3) -> (Mat3, Mat3, Vec3) { + let svd = e.svd(true, true); + let mut u = svd.u.expect("SVD U requested"); + let mut v_t = svd.v_t.expect("SVD V^T requested"); + + // Ensure U and V are proper rotations (det = +1). + if u.determinant() < 0.0 { + u.column_mut(2).neg_mut(); + } + if v_t.determinant() < 0.0 { + v_t.row_mut(2).neg_mut(); + } + + // W: 90-degree rotation about the z-axis. + let w = Mat3::new(0.0, -1.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0); + + let r1 = u * w * v_t; + let r2 = u * w.transpose() * v_t; + // Translation is the last column of U. + let mut t = Vec3::new(u[(0, 2)], u[(1, 2)], u[(2, 2)]); + let n = t.norm(); + if n > f64::EPSILON { + t /= n; + } + (r1, r2, t) +} + +/// Recovers the relative pose `cam2_from_cam1` and triangulated 3D points from an +/// essential matrix and a set of corresponding bearing rays. +/// +/// The four pose candidates from [`decompose_essential_matrix`] are tested with +/// [`check_cheirality`]; the candidate that places the most points in front of +/// both cameras is returned, together with the triangulated points (in +/// camera-1 coordinates) for that candidate. +pub fn pose_from_essential_matrix( + e: &Mat3, + rays1: &[Vec3], + rays2: &[Vec3], +) -> (Rigid3d, Vec) { + let (r1, r2, t) = decompose_essential_matrix(e); + let candidates = [ + Rigid3d::new(rot_to_quat(&r1), t), + Rigid3d::new(rot_to_quat(&r1), -t), + Rigid3d::new(rot_to_quat(&r2), t), + Rigid3d::new(rot_to_quat(&r2), -t), + ]; + + let mut best_pose = candidates[0]; + let mut best_points: Vec = Vec::new(); + let mut best_count = 0usize; + for cand in candidates.iter() { + let (points, indices) = check_cheirality(cand, rays1, rays2); + if indices.len() > best_count { + best_count = indices.len(); + best_pose = *cand; + best_points = points; + } + } + (best_pose, best_points) +} + +/// Builds a fundamental matrix from an essential matrix and the two camera +/// intrinsics: `F = K2^{-T} * E * K1^{-1}`. +pub fn fundamental_from_essential_matrix(k2: &Mat3, e: &Mat3, k1: &Mat3) -> Mat3 { + let k2_inv_t = k2.try_inverse().expect("K2 must be invertible").transpose(); + let k1_inv = k1.try_inverse().expect("K1 must be invertible"); + k2_inv_t * e * k1_inv +} + +/// Builds an essential matrix from a fundamental matrix and the two camera +/// intrinsics: `E = K2^T * F * K1`. +pub fn essential_from_fundamental_matrix(k2: &Mat3, f: &Mat3, k1: &Mat3) -> Mat3 { + k2.transpose() * f * k1 +} + +/// Computes the squared Sampson error of a correspondence under an essential (or +/// fundamental) matrix `E`, given normalized rays `ray1` and `ray2`. +/// +/// The Sampson error is a first-order approximation of the geometric reprojection +/// error of the epipolar constraint `ray2^T E ray1 = 0`. +pub fn compute_squared_sampson_error(ray1: &Vec3, ray2: &Vec3, e: &Mat3) -> f64 { + let e_x1 = e * ray1; + let e_t_x2 = e.transpose() * ray2; + let x2_t_e_x1 = ray2.dot(&e_x1); + let denom = e_x1.x * e_x1.x + e_x1.y * e_x1.y + e_t_x2.x * e_t_x2.x + e_t_x2.y * e_t_x2.y; + if denom < f64::EPSILON { + return 0.0; + } + (x2_t_e_x1 * x2_t_e_x1) / denom +} + +#[inline] +fn rot_to_quat(r: &Mat3) -> UnitQuat { + UnitQuat::from_rotation_matrix(&nalgebra::Rotation3::from_matrix_unchecked(*r)) +} + +#[cfg(test)] +mod tests { + use super::*; + use approx::assert_relative_eq; + use std::f64::consts::FRAC_PI_4; + + fn sample_pose() -> Rigid3d { + Rigid3d::new( + UnitQuat::new(Vec3::new(0.0, FRAC_PI_4 / 2.0, 0.0)), + Vec3::new(1.0, 0.2, -0.3), + ) + } + + #[test] + fn epipolar_constraint_holds() { + let pose = sample_pose(); + let e = essential_matrix_from_pose(&pose); + // Generate a 3D point and its rays in both cameras. + let pt = Vec3::new(0.5, -0.4, 4.0); + let ray1 = pt; // cam1 == world. + let ray2 = pose.transform_point(&pt); + let val = (ray2.transpose() * e * ray1)[0]; + assert_relative_eq!(val, 0.0, epsilon = 1e-9); + } + + #[test] + fn decompose_recovers_rotation_and_translation_dir() { + let pose = sample_pose(); + let e = essential_matrix_from_pose(&pose); + let (r1, r2, t) = decompose_essential_matrix(&e); + let r_true = pose.rotation.to_rotation_matrix().into_inner(); + // One of the two rotations must match the true rotation. + let match1 = (r1 - r_true).norm() < 1e-6; + let match2 = (r2 - r_true).norm() < 1e-6; + assert!(match1 || match2, "neither decomposed rotation matched"); + // Translation direction matches up to sign. + let t_true = pose.translation.normalize(); + let dot = t.dot(&t_true).abs(); + assert_relative_eq!(dot, 1.0, epsilon = 1e-6); + } + + #[test] + fn pose_from_essential_recovers_pose() { + let pose = sample_pose(); + let e = essential_matrix_from_pose(&pose); + // Build correspondences from points in front of both cameras. + let world_points = [ + Vec3::new(0.0, 0.0, 5.0), + Vec3::new(1.0, 0.5, 4.0), + Vec3::new(-1.0, -0.5, 6.0), + Vec3::new(0.5, -1.0, 5.5), + Vec3::new(-0.5, 1.0, 4.5), + ]; + let rays1: Vec = world_points.to_vec(); + let rays2: Vec = world_points.iter().map(|p| pose.transform_point(p)).collect(); + + let (rec_pose, points) = pose_from_essential_matrix(&e, &rays1, &rays2); + // Rotation recovered exactly. + let r_rec = rec_pose.rotation.to_rotation_matrix().into_inner(); + let r_true = pose.rotation.to_rotation_matrix().into_inner(); + assert_relative_eq!(r_rec, r_true, epsilon = 1e-6); + // Translation direction recovered (scale is ambiguous in essential matrix). + let t_dot = rec_pose.translation.normalize().dot(&pose.translation.normalize()); + assert_relative_eq!(t_dot, 1.0, epsilon = 1e-6); + assert_eq!(points.len(), world_points.len()); + } + + #[test] + fn fundamental_essential_roundtrip() { + let k1 = Mat3::new(800.0, 0.0, 320.0, 0.0, 800.0, 240.0, 0.0, 0.0, 1.0); + let k2 = Mat3::new(900.0, 0.0, 310.0, 0.0, 900.0, 250.0, 0.0, 0.0, 1.0); + let e = essential_matrix_from_pose(&sample_pose()); + let f = fundamental_from_essential_matrix(&k2, &e, &k1); + let e2 = essential_from_fundamental_matrix(&k2, &f, &k1); + // Up to scale; normalize by Frobenius norm. + let e_n = e / e.norm(); + let e2_n = e2 / e2.norm(); + let agree = (e_n - e2_n).norm() < 1e-9 || (e_n + e2_n).norm() < 1e-9; + assert!(agree, "essential <-> fundamental roundtrip mismatch"); + } + + #[test] + fn sampson_error_zero_on_inlier() { + let pose = sample_pose(); + let e = essential_matrix_from_pose(&pose); + let pt = Vec3::new(0.3, 0.2, 5.0); + let ray1 = pt; + let ray2 = pose.transform_point(&pt); + let err = compute_squared_sampson_error(&ray1, &ray2, &e); + assert_relative_eq!(err, 0.0, epsilon = 1e-9); + } + + #[test] + fn sampson_error_positive_on_outlier() { + let pose = sample_pose(); + let e = essential_matrix_from_pose(&pose); + let ray1 = Vec3::new(0.3, 0.2, 1.0); + let ray2 = Vec3::new(2.0, -3.0, 1.0); // arbitrary, violates constraint. + let err = compute_squared_sampson_error(&ray1, &ray2, &e); + assert!(err > 1e-6); + } +} diff --git a/src/geometry/gps.rs b/src/geometry/gps.rs new file mode 100644 index 0000000..6ec6215 --- /dev/null +++ b/src/geometry/gps.rs @@ -0,0 +1,217 @@ +//! GPS / geodetic coordinate transforms, mirroring `colmap/geometry/gps.h`. +//! +//! Converts between geodetic coordinates (latitude, longitude in **degrees**, +//! altitude in **meters**), Earth-Centered-Earth-Fixed (ECEF) Cartesian +//! coordinates, and a local East-North-Up (ENU) tangent frame anchored at a +//! reference point. + +use crate::math::Vec3; + +/// The reference ellipsoid used by [`GpsTransform`]. +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))] +pub enum GpsEllipsoid { + /// The GRS 1980 ellipsoid. + Grs80, + /// The WGS 1984 ellipsoid (the one used by GPS). + Wgs84, +} + +/// Transforms between geodetic, ECEF and ENU coordinate systems for a chosen +/// reference ellipsoid. +/// +/// Latitude and longitude are in **degrees**, altitude and all Cartesian +/// coordinates are in **meters**. +#[derive(Debug, Clone, Copy, PartialEq)] +pub struct GpsTransform { + /// Semi-major axis of the ellipsoid (meters). + pub a: f64, + /// Semi-minor axis of the ellipsoid (meters). + pub b: f64, + /// Flattening `f = (a - b) / a`. + pub f: f64, + /// First eccentricity squared `e2 = (a^2 - b^2) / a^2`. + pub e2: f64, +} + +impl GpsTransform { + /// Builds a transform for the given reference ellipsoid. + pub fn new(ellipsoid: GpsEllipsoid) -> Self { + // Defining constants: semi-major axis `a` and inverse flattening `1/f`. + let (a, inv_f) = match ellipsoid { + GpsEllipsoid::Grs80 => (6_378_137.0_f64, 298.257_222_100_882_7_f64), + GpsEllipsoid::Wgs84 => (6_378_137.0_f64, 298.257_223_563_f64), + }; + let f = 1.0 / inv_f; + let b = a * (1.0 - f); + let e2 = f * (2.0 - f); // = (a^2 - b^2) / a^2. + Self { a, b, f, e2 } + } + + /// Converts geodetic coordinates `(lat_deg, lon_deg, alt_m)` to ECEF + /// Cartesian coordinates `(x, y, z)` in meters. + pub fn ellipsoid_to_ecef(&self, lla: &[Vec3]) -> Vec { + lla.iter() + .map(|p| { + let lat = p.x.to_radians(); + let lon = p.y.to_radians(); + let alt = p.z; + let (sin_lat, cos_lat) = lat.sin_cos(); + let (sin_lon, cos_lon) = lon.sin_cos(); + // Prime vertical radius of curvature. + let n = self.a / (1.0 - self.e2 * sin_lat * sin_lat).sqrt(); + let x = (n + alt) * cos_lat * cos_lon; + let y = (n + alt) * cos_lat * sin_lon; + let z = (n * (1.0 - self.e2) + alt) * sin_lat; + Vec3::new(x, y, z) + }) + .collect() + } + + /// Converts ECEF Cartesian coordinates `(x, y, z)` in meters to geodetic + /// coordinates `(lat_deg, lon_deg, alt_m)` using Bowring's closed-form method. + pub fn ecef_to_ellipsoid(&self, xyz: &[Vec3]) -> Vec { + xyz.iter() + .map(|p| { + let x = p.x; + let y = p.y; + let z = p.z; + + let lon = y.atan2(x); + + let r = (x * x + y * y).sqrt(); + // Bowring's auxiliary quantities. + let ep2 = (self.a * self.a - self.b * self.b) / (self.b * self.b); + let theta = (z * self.a).atan2(r * self.b); + let (sin_t, cos_t) = theta.sin_cos(); + let lat = (z + ep2 * self.b * sin_t * sin_t * sin_t) + .atan2(r - self.e2 * self.a * cos_t * cos_t * cos_t); + let (sin_lat, cos_lat) = lat.sin_cos(); + let n = self.a / (1.0 - self.e2 * sin_lat * sin_lat).sqrt(); + // Choose the altitude formula that is numerically stable away from + // the poles; fall back to the polar form near them. + let alt = if cos_lat.abs() > 1e-12 { + r / cos_lat - n + } else { + z / sin_lat - n * (1.0 - self.e2) + }; + + Vec3::new(lat.to_degrees(), lon.to_degrees(), alt) + }) + .collect() + } + + /// Converts geodetic coordinates to a local East-North-Up (ENU) frame + /// anchored at the reference geodetic point `(ref_lat_deg, ref_lon_deg, + /// ref_alt_m)`. + pub fn ellipsoid_to_enu(&self, lla: &[Vec3], ref_lat: f64, ref_lon: f64, ref_alt: f64) -> Vec { + let ref_ecef = self.ellipsoid_to_ecef(&[Vec3::new(ref_lat, ref_lon, ref_alt)])[0]; + let ecef = self.ellipsoid_to_ecef(lla); + let (sin_lat, cos_lat) = ref_lat.to_radians().sin_cos(); + let (sin_lon, cos_lon) = ref_lon.to_radians().sin_cos(); + ecef.iter() + .map(|p| { + let d = p - ref_ecef; + let east = -sin_lon * d.x + cos_lon * d.y; + let north = -sin_lat * cos_lon * d.x - sin_lat * sin_lon * d.y + cos_lat * d.z; + let up = cos_lat * cos_lon * d.x + cos_lat * sin_lon * d.y + sin_lat * d.z; + Vec3::new(east, north, up) + }) + .collect() + } + + /// Converts local ENU coordinates back to geodetic coordinates, given the + /// same reference geodetic anchor point. + pub fn enu_to_ellipsoid(&self, enu: &[Vec3], ref_lat: f64, ref_lon: f64, ref_alt: f64) -> Vec { + let ref_ecef = self.ellipsoid_to_ecef(&[Vec3::new(ref_lat, ref_lon, ref_alt)])[0]; + let (sin_lat, cos_lat) = ref_lat.to_radians().sin_cos(); + let (sin_lon, cos_lon) = ref_lon.to_radians().sin_cos(); + let ecef: Vec = enu + .iter() + .map(|p| { + let e = p.x; + let n = p.y; + let u = p.z; + // Transpose of the ENU rotation (ENU -> ECEF deltas). + let dx = -sin_lon * e - sin_lat * cos_lon * n + cos_lat * cos_lon * u; + let dy = cos_lon * e - sin_lat * sin_lon * n + cos_lat * sin_lon * u; + let dz = cos_lat * n + sin_lat * u; + ref_ecef + Vec3::new(dx, dy, dz) + }) + .collect(); + self.ecef_to_ellipsoid(&ecef) + } +} + +#[cfg(test)] +mod tests { + use super::*; + use approx::assert_relative_eq; + + #[test] + fn ellipsoid_constants_wgs84() { + let t = GpsTransform::new(GpsEllipsoid::Wgs84); + assert_relative_eq!(t.a, 6_378_137.0, epsilon = 1e-6); + // Semi-minor axis ~6356752.314. + assert_relative_eq!(t.b, 6_356_752.314_245, epsilon = 1e-3); + assert_relative_eq!(t.e2, 0.006_694_379_990_14, epsilon = 1e-12); + } + + #[test] + fn ecef_roundtrip() { + let t = GpsTransform::new(GpsEllipsoid::Wgs84); + let pts = [ + Vec3::new(47.37, 8.55, 408.0), // Zurich. + Vec3::new(-33.87, 151.21, 58.0), // Sydney. + Vec3::new(0.0, 0.0, 0.0), // Gulf of Guinea. + Vec3::new(89.9, 10.0, 1000.0), // Near north pole. + ]; + let ecef = t.ellipsoid_to_ecef(&pts); + let back = t.ecef_to_ellipsoid(&ecef); + for (orig, b) in pts.iter().zip(back.iter()) { + assert_relative_eq!(orig.x, b.x, epsilon = 1e-7); + assert_relative_eq!(orig.y, b.y, epsilon = 1e-7); + assert_relative_eq!(orig.z, b.z, epsilon = 1e-4); + } + } + + #[test] + fn ecef_known_value_equator_prime_meridian() { + let t = GpsTransform::new(GpsEllipsoid::Wgs84); + // At lat=0, lon=0, alt=0 the ECEF point is (a, 0, 0). + let ecef = t.ellipsoid_to_ecef(&[Vec3::new(0.0, 0.0, 0.0)])[0]; + assert_relative_eq!(ecef.x, t.a, epsilon = 1e-6); + assert_relative_eq!(ecef.y, 0.0, epsilon = 1e-6); + assert_relative_eq!(ecef.z, 0.0, epsilon = 1e-6); + } + + #[test] + fn enu_roundtrip() { + let t = GpsTransform::new(GpsEllipsoid::Wgs84); + let (rlat, rlon, ralt) = (47.37, 8.55, 408.0); + let pts = [ + Vec3::new(47.371, 8.551, 410.0), + Vec3::new(47.369, 8.549, 405.0), + Vec3::new(47.37, 8.55, 408.0), + ]; + let enu = t.ellipsoid_to_enu(&pts, rlat, rlon, ralt); + // The reference point itself maps to the ENU origin. + assert_relative_eq!(enu[2], Vec3::zeros(), epsilon = 1e-6); + let back = t.enu_to_ellipsoid(&enu, rlat, rlon, ralt); + for (orig, b) in pts.iter().zip(back.iter()) { + assert_relative_eq!(orig.x, b.x, epsilon = 1e-7); + assert_relative_eq!(orig.y, b.y, epsilon = 1e-7); + assert_relative_eq!(orig.z, b.z, epsilon = 1e-3); + } + } + + #[test] + fn enu_east_increases_with_longitude() { + let t = GpsTransform::new(GpsEllipsoid::Wgs84); + let (rlat, rlon, ralt) = (0.0, 0.0, 0.0); + // A point slightly east (greater longitude) should have positive East. + let enu = t.ellipsoid_to_enu(&[Vec3::new(0.0, 0.001, 0.0)], rlat, rlon, ralt)[0]; + assert!(enu.x > 0.0, "east component should be positive"); + assert_relative_eq!(enu.y, 0.0, epsilon = 1e-3); + } +} diff --git a/src/geometry/homography_matrix.rs b/src/geometry/homography_matrix.rs new file mode 100644 index 0000000..12a048a --- /dev/null +++ b/src/geometry/homography_matrix.rs @@ -0,0 +1,280 @@ +//! Homography matrix construction and decomposition. +//! +//! Mirrors `colmap/geometry/homography_matrix.h`. The homography `H` maps points +//! of a plane between two views. It can be built from a relative pose plus a +//! plane and decomposed back into up to four pose/normal candidates using the +//! Faugeras–Lustman analytical SVD decomposition. + +use crate::geometry::pose::check_cheirality; +use crate::geometry::Rigid3d; +use crate::math::{Mat3, UnitQuat, Vec2, Vec3}; + +/// Builds a homography matrix from a relative pose and a plane. +/// +/// `k1`, `k2` are the camera intrinsics, `r`/`t` the rotation/translation of +/// `cam2_from_cam1`, `n` the plane normal (in camera-1 coordinates) and `d` the +/// distance of the plane to the origin of camera 1: +/// +/// ```text +/// H = K2 * (R - (t * n^T) / d) * K1^{-1} +/// ``` +pub fn homography_matrix_from_pose( + k1: &Mat3, + k2: &Mat3, + r: &Mat3, + t: &Vec3, + n: &Vec3, + d: f64, +) -> Mat3 { + let k1_inv = k1.try_inverse().expect("K1 must be invertible"); + k2 * (r - (t * n.transpose()) / d) * k1_inv +} + +/// Decomposes a homography matrix into candidate relative poses and plane +/// normals, using the analytical Faugeras–Lustman SVD decomposition (as used by +/// COLMAP). +/// +/// Returns up to four `(Rigid3d, normal)` candidates. The correct one is selected +/// by a positive-depth (cheirality) test in [`pose_from_homography_matrix`]. +pub fn decompose_homography_matrix(h: &Mat3, k1: &Mat3, k2: &Mat3) -> (Vec, Vec) { + let k2_inv = k2.try_inverse().expect("K2 must be invertible"); + // Calibrated homography H' = K2^{-1} H K1. + let hn = k2_inv * h * k1; + decompose_calibrated_homography(&hn) +} + +/// Recovers the relative pose, plane normal and triangulated points from a +/// homography and the two camera intrinsics. +/// +/// `points1`/`points2` are corresponding pixel image points used to disambiguate +/// the four decomposition candidates by a positive-depth test. Returns the +/// selected pose, its plane normal and the triangulated 3D points (in camera-1 +/// coordinates). +pub fn pose_from_homography_matrix( + h: &Mat3, + k1: &Mat3, + k2: &Mat3, + points1: &[Vec2], + points2: &[Vec2], +) -> (Rigid3d, Vec3, Vec) { + let (poses, normals) = decompose_homography_matrix(h, k1, k2); + + // Convert image points to bearing rays in each calibrated camera frame. + let k1_inv = k1.try_inverse().expect("K1 must be invertible"); + let k2_inv = k2.try_inverse().expect("K2 must be invertible"); + let rays1: Vec = points1 + .iter() + .map(|p| k1_inv * Vec3::new(p.x, p.y, 1.0)) + .collect(); + let rays2: Vec = points2 + .iter() + .map(|p| k2_inv * Vec3::new(p.x, p.y, 1.0)) + .collect(); + + let mut best_pose = Rigid3d::identity(); + let mut best_normal = Vec3::new(0.0, 0.0, 1.0); + let mut best_points: Vec = Vec::new(); + let mut best_count = 0usize; + + for (pose, normal) in poses.iter().zip(normals.iter()) { + let (points, indices) = check_cheirality(pose, &rays1, &rays2); + if indices.len() > best_count { + best_count = indices.len(); + best_pose = *pose; + best_normal = *normal; + best_points = points; + } + } + + (best_pose, best_normal, best_points) +} + +/// Faugeras–Lustman analytical decomposition of a calibrated homography +/// `H = d_inv * (R + t n^T)` (defined up to scale). Returns the candidate poses +/// and corresponding plane normals. +fn decompose_calibrated_homography(h: &Mat3) -> (Vec, Vec) { + // Normalize the homography by the median singular value d2 so that the + // decomposed translation/rotation are metric (up to the global sign). + let svd = h.svd(true, true); + let sv = &svd.singular_values; + let d1 = sv[0]; + let d2 = sv[1]; + let d3 = sv[2]; + + // U and V from H = U S V^T (so V^T is svd.v_t, V = v_t^T). + let u = svd.u.expect("SVD U requested"); + let v_t = svd.v_t.expect("SVD V^T requested"); + let v = v_t.transpose(); + + // Sign s = det(U) * det(V). + let s = u.determinant() * v.determinant(); + + // Guard against a degenerate (planar-degenerate) singular spectrum. + let denom = d1 - d3; + if denom.abs() < 1e-12 { + // Pure rotation case: H ~ R. + let r = u * v_t; + let r = if r.determinant() < 0.0 { -r } else { r }; + let pose = Rigid3d::new(rot_to_quat(&r), Vec3::zeros()); + return (vec![pose, pose], vec![Vec3::new(0.0, 0.0, 1.0), Vec3::new(0.0, 0.0, 1.0)]); + } + + // x1, x3 from Faugeras (the x2 term is zero in the canonical frame). + let aux1 = ((d1 * d1 - d2 * d2) / (d1 * d1 - d3 * d3)).max(0.0).sqrt(); + let aux3 = ((d2 * d2 - d3 * d3) / (d1 * d1 - d3 * d3)).max(0.0).sqrt(); + let x1_vals = [aux1, aux1, -aux1, -aux1]; + let x3_vals = [aux3, -aux3, aux3, -aux3]; + + let mut poses = Vec::new(); + let mut normals = Vec::new(); + + // Case d' > 0 (s * d2): four solutions. + let aux_st = ((d1 * d1 - d2 * d2) * (d2 * d2 - d3 * d3)).max(0.0).sqrt() / ((d1 + d3) * d2); + let ct = (d2 * d2 + d1 * d3) / ((d1 + d3) * d2); + for i in 0..4 { + let st = aux_st * x1_vals[i] * x3_vals[i]; + // Rotation in the canonical (U,V) frame. + let rp = Mat3::new(ct, 0.0, -st, 0.0, 1.0, 0.0, st, 0.0, ct); + // Project to the closest rotation to remove numerical drift from the + // singular-value algebra (guarantees an orthonormal, det +1 matrix). + let r = crate::geometry::pose::compute_closest_rotation_matrix(&(s * u * rp * v_t)); + + let aux_tp = d1 - d3; + let tp = Vec3::new(x1_vals[i], 0.0, -x3_vals[i]) * aux_tp; + let t = u * tp; + + let np = Vec3::new(x1_vals[i], 0.0, x3_vals[i]); + let mut n = v * np; + // Convention: make the plane face the camera (n_z negative -> flip both + // n and keep t consistent). COLMAP keeps both signs as separate + // candidates, which the cheirality test resolves. + if n.z > 0.0 { + n = -n; + } + poses.push(Rigid3d::new(rot_to_quat(&r), t)); + normals.push(n); + } + + // Case d' < 0 (s * -d2): four more solutions. + let aux_sp = ((d1 * d1 - d2 * d2) * (d2 * d2 - d3 * d3)).max(0.0).sqrt() / ((d1 - d3) * d2); + let cp = (d1 * d3 - d2 * d2) / ((d1 - d3) * d2); + for i in 0..4 { + let sp = aux_sp * x1_vals[i] * x3_vals[i]; + let rp = Mat3::new(cp, 0.0, sp, 0.0, -1.0, 0.0, sp, 0.0, -cp); + let r = crate::geometry::pose::compute_closest_rotation_matrix(&(s * u * rp * v_t)); + + let aux_tp = d1 + d3; + let tp = Vec3::new(x1_vals[i], 0.0, x3_vals[i]) * aux_tp; + let t = u * tp; + + let np = Vec3::new(x1_vals[i], 0.0, x3_vals[i]); + let mut n = v * np; + if n.z > 0.0 { + n = -n; + } + poses.push(Rigid3d::new(rot_to_quat(&r), t)); + normals.push(n); + } + + (poses, normals) +} + +#[inline] +fn rot_to_quat(r: &Mat3) -> UnitQuat { + UnitQuat::from_rotation_matrix(&nalgebra::Rotation3::from_matrix_unchecked(*r)) +} + +#[cfg(test)] +mod tests { + use super::*; + use approx::assert_relative_eq; + + #[test] + fn homography_from_pose_maps_plane_points() { + let k1 = Mat3::identity(); + let k2 = Mat3::identity(); + let r = crate::geometry::pose::euler_angles_to_rotation_matrix(0.0, 0.1, 0.0); + let t = Vec3::new(0.3, 0.0, 0.0); + // COLMAP plane convention: H = K2 (R - t n^T / d) K1^{-1}, valid for 3D + // points X on the plane n^T X = -d. With n = (0,0,-1), d = 5 the plane is + // Z = 5 (in front of the camera). + let n = Vec3::new(0.0, 0.0, -1.0); + let d = 5.0; + let h = homography_matrix_from_pose(&k1, &k2, &r, &t, &n, d); + + // 3D point on the plane n^T X = -d (i.e. Z = 5). + let x = Vec3::new(0.5, -0.3, 5.0); + let x1n = Vec2::new(x.x / x.z, x.y / x.z); + let x2h = h * Vec3::new(x1n.x, x1n.y, 1.0); + let x2n = Vec2::new(x2h.x / x2h.z, x2h.y / x2h.z); + let x_cam2 = r * x + t; + let x2_expected = Vec2::new(x_cam2.x / x_cam2.z, x_cam2.y / x_cam2.z); + assert_relative_eq!(x2n, x2_expected, epsilon = 1e-9); + } + + #[test] + fn decompose_returns_candidates() { + let k = Mat3::new(800.0, 0.0, 320.0, 0.0, 800.0, 240.0, 0.0, 0.0, 1.0); + let r = crate::geometry::pose::euler_angles_to_rotation_matrix(0.02, 0.06, 0.0); + let t = Vec3::new(0.3, 0.05, 0.1); + let n = Vec3::new(0.0, 0.0, -1.0); + let d = 4.0; + let h = homography_matrix_from_pose(&k, &k, &r, &t, &n, d); + let (poses, normals) = decompose_homography_matrix(&h, &k, &k); + assert_eq!(poses.len(), normals.len()); + assert!(!poses.is_empty()); + // Every returned pose has a valid (orthonormal, det +1) rotation. + for p in &poses { + let rm = p.rotation.to_rotation_matrix().into_inner(); + assert_relative_eq!(rm.determinant(), 1.0, epsilon = 1e-6); + assert_relative_eq!(rm.transpose() * rm, Mat3::identity(), epsilon = 1e-6); + } + } + + #[test] + fn pose_from_homography_recovers_translation_direction() { + let k = Mat3::identity(); + let r = crate::geometry::pose::euler_angles_to_rotation_matrix(0.0, 0.05, 0.0); + let t = Vec3::new(0.4, 0.1, 0.0); + let n = Vec3::new(0.0, 0.0, -1.0); + let d = 4.0; + let h = homography_matrix_from_pose(&k, &k, &r, &t, &n, d); + + let world_points = [ + Vec3::new(0.0, 0.0, 4.0), + Vec3::new(1.0, 0.5, 4.0), + Vec3::new(-1.0, 0.5, 4.0), + Vec3::new(0.5, -1.0, 4.0), + Vec3::new(-0.5, -0.8, 4.0), + Vec3::new(0.8, 0.2, 4.0), + ]; + let points1: Vec = world_points + .iter() + .map(|p| Vec2::new(p.x / p.z, p.y / p.z)) + .collect(); + let points2: Vec = world_points + .iter() + .map(|p| { + let pc = r * p + t; + Vec2::new(pc.x / pc.z, pc.y / pc.z) + }) + .collect(); + + let (pose, normal, points) = pose_from_homography_matrix(&h, &k, &k, &points1, &points2); + // The recovered rotation should be close to the true rotation. + let r_rec = pose.rotation.to_rotation_matrix().into_inner(); + assert!( + (r_rec - r).norm() < 5e-2, + "rotation mismatch norm = {}", + (r_rec - r).norm() + ); + // Translation direction recovered up to scale. + if pose.translation.norm() > 1e-9 { + let dot = pose.translation.normalize().dot(&t.normalize()); + assert!(dot > 0.9, "translation direction dot = {}", dot); + } + // The plane normal should be roughly along +/- z. + assert!(normal.z.abs() > 0.8, "normal not plane-facing: {:?}", normal); + assert!(!points.is_empty()); + } +} diff --git a/src/geometry/mod.rs b/src/geometry/mod.rs new file mode 100644 index 0000000..75002bc --- /dev/null +++ b/src/geometry/mod.rs @@ -0,0 +1,49 @@ +//! 3D geometry: transforms, two-view geometry, triangulation and GPS. +//! +//! Mirrors `colmap/geometry/*`. The core transform types ([`Rigid3d`], [`Sim3d`], +//! [`Rotation3d`]) follow COLMAP's `b_from_a` naming convention: a value named +//! `b_from_a` maps points from frame `a` into frame `b`. +//! +//! In addition to the core transforms this module exposes free-function helpers +//! grouped by topic: +//! +//! - [`pose`] — rotation conversions, projection-matrix decomposition, cheirality. +//! - [`essential_matrix`] — essential/fundamental matrix construction and decomposition. +//! - [`homography_matrix`] — homography construction and decomposition. +//! - [`triangulation`] — DLT / mid-point / multi-view triangulation and angles. +//! - [`gps`] — geodetic (lat/lon/alt) ↔ ECEF ↔ ENU conversions. +//! - [`normalization`] — image-point normalization for the 8-point algorithm. +//! - [`pose_prior`] — geo-referenced position priors. + +mod rigid3; +mod rotation; +mod sim3; + +pub mod essential_matrix; +pub mod gps; +pub mod homography_matrix; +pub mod normalization; +pub mod pose; +pub mod pose_prior; +pub mod triangulation; + +pub use rigid3::Rigid3d; +pub use rotation::Rotation3d; +pub use sim3::Sim3d; + +pub use pose_prior::{PosePrior, PosePriorCoordinateSystem}; + +// Convenience re-exports of the most commonly used free functions at the +// `geometry` root, mirroring how PyCOLMAP exposes them as module-level helpers. +pub use essential_matrix::{ + decompose_essential_matrix, essential_matrix_from_pose, pose_from_essential_matrix, +}; +pub use homography_matrix::{decompose_homography_matrix, pose_from_homography_matrix}; +pub use pose::{ + average_quaternions, check_cheirality, decompose_projection_matrix, interpolate_camera_poses, + transform_camera_world, +}; +pub use triangulation::{ + calculate_triangulation_angle, triangulate_mid_point, triangulate_multi_view_point, + triangulate_point, +}; diff --git a/src/geometry/normalization.rs b/src/geometry/normalization.rs new file mode 100644 index 0000000..e92533e --- /dev/null +++ b/src/geometry/normalization.rs @@ -0,0 +1,145 @@ +//! Image-point normalization for robust estimation. +//! +//! Mirrors COLMAP's point normalization used by the normalized 8-point / DLT +//! algorithms: translate the points so their centroid is at the origin and scale +//! them so their mean distance to the origin is `sqrt(2)`. + +use crate::math::{Mat3, Vec2}; + +/// Centers and normalizes a set of 2D image points (Hartley normalization). +/// +/// Returns the normalized points and the `3×3` similarity matrix `T` (in +/// homogeneous coordinates) such that `x_normalized = T * x_homogeneous`. After +/// the transform the points have zero centroid and a mean distance to the origin +/// of `sqrt(2)`. +/// +/// If the points are all coincident (zero spread) the scale is set to 1 to avoid +/// division by zero, so `T` is a pure translation. +/// +/// # Examples +/// ``` +/// use colmap::geometry::normalization::center_and_normalize_image_points; +/// use colmap::math::Vec2; +/// +/// let pts = vec![ +/// Vec2::new(0.0, 0.0), +/// Vec2::new(2.0, 0.0), +/// Vec2::new(0.0, 2.0), +/// Vec2::new(2.0, 2.0), +/// ]; +/// let (norm, t) = center_and_normalize_image_points(&pts); +/// // The normalized centroid is at the origin. +/// let cx: f64 = norm.iter().map(|p| p.x).sum::() / norm.len() as f64; +/// assert!(cx.abs() < 1e-12); +/// let _ = t; +/// ``` +pub fn center_and_normalize_image_points(points: &[Vec2]) -> (Vec, Mat3) { + let n = points.len(); + if n == 0 { + return (Vec::new(), Mat3::identity()); + } + + // Centroid. + let mut centroid = Vec2::zeros(); + for p in points { + centroid += *p; + } + centroid /= n as f64; + + // Mean distance of the centered points to the origin. + let mut mean_dist = 0.0; + for p in points { + mean_dist += (p - centroid).norm(); + } + mean_dist /= n as f64; + + // Scale so the mean distance becomes sqrt(2). + let scale = if mean_dist > f64::EPSILON { + std::f64::consts::SQRT_2 / mean_dist + } else { + 1.0 + }; + + // T = [ scale, 0, -scale*cx; 0, scale, -scale*cy; 0, 0, 1 ]. + let t = Mat3::new( + scale, + 0.0, + -scale * centroid.x, + 0.0, + scale, + -scale * centroid.y, + 0.0, + 0.0, + 1.0, + ); + + let normalized: Vec = points + .iter() + .map(|p| Vec2::new(scale * (p.x - centroid.x), scale * (p.y - centroid.y))) + .collect(); + + (normalized, t) +} + +#[cfg(test)] +mod tests { + use super::*; + use approx::assert_relative_eq; + + fn mean_distance(points: &[Vec2]) -> f64 { + let n = points.len() as f64; + let centroid: Vec2 = points.iter().fold(Vec2::zeros(), |a, p| a + *p) / n; + points.iter().map(|p| (p - centroid).norm()).sum::() / n + } + + #[test] + fn centroid_is_origin_and_mean_dist_sqrt2() { + let pts = vec![ + Vec2::new(1.0, 2.0), + Vec2::new(3.0, -1.0), + Vec2::new(-2.0, 4.0), + Vec2::new(5.0, 5.0), + Vec2::new(0.0, -3.0), + ]; + let (norm, _t) = center_and_normalize_image_points(&pts); + // Centroid at origin. + let centroid: Vec2 = norm.iter().fold(Vec2::zeros(), |a, p| a + *p) / norm.len() as f64; + assert_relative_eq!(centroid, Vec2::zeros(), epsilon = 1e-12); + // Mean distance sqrt(2). + assert_relative_eq!(mean_distance(&norm), std::f64::consts::SQRT_2, epsilon = 1e-12); + } + + #[test] + fn transform_maps_points_correctly() { + let pts = vec![ + Vec2::new(1.0, 2.0), + Vec2::new(3.0, -1.0), + Vec2::new(-2.0, 4.0), + Vec2::new(5.0, 5.0), + ]; + let (norm, t) = center_and_normalize_image_points(&pts); + for (p, np) in pts.iter().zip(norm.iter()) { + let h = t * crate::math::Vec3::new(p.x, p.y, 1.0); + let mapped = Vec2::new(h.x / h.z, h.y / h.z); + assert_relative_eq!(mapped, *np, epsilon = 1e-12); + } + } + + #[test] + fn coincident_points_no_div_by_zero() { + let pts = vec![Vec2::new(2.0, 3.0), Vec2::new(2.0, 3.0), Vec2::new(2.0, 3.0)]; + let (norm, t) = center_and_normalize_image_points(&pts); + // All map to origin, scale defaults to 1. + for np in &norm { + assert_relative_eq!(*np, Vec2::zeros(), epsilon = 1e-12); + } + assert_relative_eq!(t[(0, 0)], 1.0, epsilon = 1e-12); + } + + #[test] + fn empty_input() { + let (norm, t) = center_and_normalize_image_points(&[]); + assert!(norm.is_empty()); + assert_eq!(t, Mat3::identity()); + } +} diff --git a/src/geometry/pose.rs b/src/geometry/pose.rs new file mode 100644 index 0000000..e855239 --- /dev/null +++ b/src/geometry/pose.rs @@ -0,0 +1,439 @@ +//! Pose and rotation utilities, mirroring `colmap/geometry/pose.h`. +//! +//! Free functions for converting between rotation representations (Euler angles, +//! angle-axis, rotation matrices), averaging quaternions, interpolating camera +//! poses, decomposing projection matrices, checking cheirality and transforming +//! camera poses by a similarity transform. + +use crate::geometry::{Rigid3d, Sim3d}; +use crate::math::{Mat3, Mat3x4, Mat4, UnitQuat, Vec3}; + +/// Returns the skew-symmetric cross-product matrix `[v]_x` such that +/// `[v]_x * w == v.cross(&w)` for any vector `w`. +/// +/// # Examples +/// ``` +/// use colmap::geometry::pose::cross_product_matrix; +/// use colmap::math::Vec3; +/// +/// let v = Vec3::new(1.0, 2.0, 3.0); +/// let w = Vec3::new(-1.0, 0.5, 2.0); +/// assert_eq!(cross_product_matrix(&v) * w, v.cross(&w)); +/// ``` +pub fn cross_product_matrix(v: &Vec3) -> Mat3 { + Mat3::new(0.0, -v.z, v.y, v.z, 0.0, -v.x, -v.y, v.x, 0.0) +} + +/// Builds a rotation matrix from intrinsic-extrinsic Euler angles applied as +/// `R = Rz(rz) * Ry(ry) * Rx(rx)` (radians). +/// +/// This matches COLMAP's `EulerAnglesToRotationMatrix`, where the composed +/// rotation first rotates about `x`, then `y`, then `z`. +pub fn euler_angles_to_rotation_matrix(rx: f64, ry: f64, rz: f64) -> Mat3 { + let (sx, cx) = rx.sin_cos(); + let (sy, cy) = ry.sin_cos(); + let (sz, cz) = rz.sin_cos(); + + let rx_m = Mat3::new(1.0, 0.0, 0.0, 0.0, cx, -sx, 0.0, sx, cx); + let ry_m = Mat3::new(cy, 0.0, sy, 0.0, 1.0, 0.0, -sy, 0.0, cy); + let rz_m = Mat3::new(cz, -sz, 0.0, sz, cz, 0.0, 0.0, 0.0, 1.0); + + rz_m * ry_m * rx_m +} + +/// Recovers the Euler angles `(rx, ry, rz)` from a rotation matrix produced by +/// [`euler_angles_to_rotation_matrix`] (the inverse operation), in radians. +/// +/// Returns angles such that `R = Rz(rz) * Ry(ry) * Rx(rx)`. +pub fn rotation_matrix_to_euler_angles(r: &Mat3) -> (f64, f64, f64) { + // R = Rz * Ry * Rx. + // r[2,0] = -sin(ry). + let rx; + let ry; + let rz; + let r20 = r[(2, 0)]; + if r20.abs() < 1.0 - 1e-12 { + ry = (-r20).asin(); + rx = r[(2, 1)].atan2(r[(2, 2)]); + rz = r[(1, 0)].atan2(r[(0, 0)]); + } else { + // Gimbal lock: ry = ±pi/2. Set rz = 0 and solve for rx. + rz = 0.0; + if r20 <= -1.0 + 1e-12 { + ry = std::f64::consts::FRAC_PI_2; + rx = r[(0, 1)].atan2(r[(0, 2)]); + } else { + ry = -std::f64::consts::FRAC_PI_2; + rx = (-r[(0, 1)]).atan2(-r[(0, 2)]); + } + } + (rx, ry, rz) +} + +/// Converts an angle-axis vector (direction = axis, magnitude = angle in radians) +/// to its 3×3 rotation matrix via Rodrigues' formula. +pub fn angle_axis_to_rotation_matrix(angle_axis: &Vec3) -> Mat3 { + UnitQuat::new(*angle_axis).to_rotation_matrix().into_inner() +} + +/// Converts a rotation matrix to its angle-axis vector (magnitude = angle in +/// radians). The matrix is projected to the closest valid rotation first. +pub fn rotation_matrix_to_angle_axis(r: &Mat3) -> Vec3 { + let rot = nalgebra::Rotation3::from_matrix_unchecked(*r); + UnitQuat::from_rotation_matrix(&rot).scaled_axis() +} + +/// Projects an arbitrary 3×3 matrix onto the closest rotation matrix (in the +/// Frobenius-norm sense) using its SVD: `R = U * diag(1,1,det(U V^T)) * V^T`. +pub fn compute_closest_rotation_matrix(m: &Mat3) -> Mat3 { + let svd = m.svd(true, true); + let u = svd.u.expect("SVD U requested"); + let v_t = svd.v_t.expect("SVD V^T requested"); + let mut s = Mat3::identity(); + let det = (u * v_t).determinant(); + if det < 0.0 { + s[(2, 2)] = -1.0; + } + u * s * v_t +} + +/// Averages a set of rotations represented as unit quaternions, using Markley's +/// eigenvalue method (the largest-eigenvalue eigenvector of the weighted outer +/// product of quaternion coefficients). +/// +/// `quats` and `weights` must have the same length; weights should be +/// non-negative. Returns the identity rotation for an empty input. +pub fn average_quaternions(quats: &[UnitQuat], weights: &[f64]) -> UnitQuat { + assert_eq!(quats.len(), weights.len(), "quats and weights length mismatch"); + if quats.is_empty() { + return UnitQuat::identity(); + } + // Accumulate M = sum_i w_i * q_i q_i^T, with q = [w, x, y, z]. + let mut acc = Mat4::zeros(); + for (q, &w) in quats.iter().zip(weights.iter()) { + let qq = q.quaternion(); + let v = nalgebra::Vector4::new(qq.w, qq.i, qq.j, qq.k); + acc += w * (v * v.transpose()); + } + // The average quaternion is the eigenvector of `acc` with the largest + // eigenvalue. `acc` is real symmetric, so use the symmetric eigendecomposition. + let eig = acc.symmetric_eigen(); + let mut best_idx = 0; + let mut best_val = f64::NEG_INFINITY; + for i in 0..4 { + if eig.eigenvalues[i] > best_val { + best_val = eig.eigenvalues[i]; + best_idx = i; + } + } + let v = eig.eigenvectors.column(best_idx); + let q = crate::math::Quat::new(v[0], v[1], v[2], v[3]); + UnitQuat::from_quaternion(q) +} + +/// Interpolates between two camera poses by parameter `t` in `[0, 1]`: spherical +/// linear interpolation (slerp) of the rotation and linear interpolation of the +/// translation. `t = 0` yields `pose1`, `t = 1` yields `pose2`. +pub fn interpolate_camera_poses(pose1: &Rigid3d, pose2: &Rigid3d, t: f64) -> Rigid3d { + let rotation = pose1.rotation.slerp(&pose2.rotation, t); + let translation = pose1.translation.lerp(&pose2.translation, t); + Rigid3d::new(rotation, translation) +} + +/// Decomposes a 3×4 projection matrix `P = K [R | t]` into the camera intrinsics +/// `K` (upper triangular with positive diagonal), rotation `R` and translation +/// `t`. Uses an RQ decomposition of the left 3×3 block. +/// +/// Returns `None` if the left 3×3 block is singular. +pub fn decompose_projection_matrix(p: &Mat3x4) -> Option<(Mat3, Mat3, Vec3)> { + let m = p.fixed_view::<3, 3>(0, 0).into_owned(); + if m.determinant().abs() < 1e-12 { + return None; + } + // RQ decomposition of M = K * R via the QR decomposition of a permuted, + // transposed matrix (Hartley & Zisserman, A4.1.1). + let perm = Mat3::new(0.0, 0.0, 1.0, 0.0, 1.0, 0.0, 1.0, 0.0, 0.0); + let a = perm * m; + let qr = a.transpose().qr(); + let q_full = qr.q(); + let r_full = qr.unpack_r(); + + let mut k = perm * r_full.transpose() * perm; + let mut r = perm * q_full.transpose(); + + // Enforce positive diagonal of K, fixing the sign of the corresponding R rows. + let mut sign = Mat3::identity(); + for i in 0..3 { + if k[(i, i)] < 0.0 { + sign[(i, i)] = -1.0; + } + } + k *= sign; + r = sign * r; + + // Normalize K so K[2,2] == 1. + let scale = k[(2, 2)]; + if scale.abs() < 1e-12 { + return None; + } + k /= scale; + + // Ensure R is a proper rotation (det = +1). + if r.determinant() < 0.0 { + r = -r; + } + + // t = K^-1 * p[:, 3]. + let p3 = p.fixed_view::<3, 1>(0, 3).into_owned(); + let k_inv = k.try_inverse()?; + let t = k_inv * p3; + + Some((k, r, t)) +} + +/// Checks cheirality (positivity of depth) for corresponding rays and +/// triangulates the points that lie in front of both cameras. +/// +/// `cam2_from_cam1` is the relative pose, `rays1`/`rays2` are bearing vectors +/// (directions, not necessarily normalized) in each camera frame. Returns the +/// triangulated 3D points (in camera-1 coordinates) for the inlier +/// correspondences and the indices (into the input) of those inliers. +pub fn check_cheirality( + cam2_from_cam1: &Rigid3d, + rays1: &[Vec3], + rays2: &[Vec3], +) -> (Vec, Vec) { + const MIN_DEPTH: f64 = f64::EPSILON; + let max_depth = 1000.0 * cam2_from_cam1.translation.norm(); + + let cam1_from_world = Rigid3d::identity(); + let cam2_from_world = *cam2_from_cam1; + let p1 = cam1_from_world.to_matrix(); + let p2 = cam2_from_world.to_matrix(); + + let n = rays1.len().min(rays2.len()); + let mut points = Vec::new(); + let mut indices = Vec::new(); + for i in 0..n { + // Project rays onto the image plane (z = 1) as normalized coordinates. + let r1 = rays1[i]; + let r2 = rays2[i]; + if r1.z.abs() < f64::EPSILON || r2.z.abs() < f64::EPSILON { + continue; + } + let x1 = crate::math::Vec2::new(r1.x / r1.z, r1.y / r1.z); + let x2 = crate::math::Vec2::new(r2.x / r2.z, r2.y / r2.z); + let point = match crate::geometry::triangulation::triangulate_point(&p1, &p2, &x1, &x2) { + Some(p) => p, + None => continue, + }; + // Depth in camera 1 is the z-coordinate (cam1 == world here). + let depth1 = point.z; + if depth1 <= MIN_DEPTH || depth1 >= max_depth { + continue; + } + let point_in_cam2 = cam2_from_cam1.transform_point(&point); + let depth2 = point_in_cam2.z; + if depth2 <= MIN_DEPTH || depth2 >= max_depth { + continue; + } + points.push(point); + indices.push(i); + } + (points, indices) +} + +/// Transforms a `cam_from_world` pose by a world-space similarity transform +/// `new_world_from_old_world`, returning `cam_from_new_world`. +/// +/// This mirrors COLMAP's `TransformCameraWorld`. A point `p` expressed in the +/// new (scaled) world frame maps back to the old world frame through the inverse +/// similarity transform, then through the camera pose: +/// +/// ```text +/// p_old = old_world_from_new_world(p) // a Sim3d (scale + R + t) +/// p_cam = R_cw * p_old + t_cw +/// = R_cw * (s_inv * R_inv * p + t_inv) + t_cw +/// ``` +/// +/// so the linear part is `s_inv * R_cw * R_inv` and the translation is +/// `R_cw * t_inv + t_cw`. To return a rigid [`Rigid3d`] the residual scale on the +/// rotation is dropped (the rotation is renormalized), matching how COLMAP keeps +/// the camera pose rigid while re-expressing it in the scaled world frame. +pub fn transform_camera_world( + new_world_from_old_world: &Sim3d, + cam_from_world: &Rigid3d, +) -> Rigid3d { + let old_world_from_new_world = new_world_from_old_world.inverse(); + let rotation = cam_from_world.rotation * old_world_from_new_world.rotation; + let translation = + cam_from_world.rotation * old_world_from_new_world.translation + cam_from_world.translation; + Rigid3d::new(rotation, translation) +} + +#[cfg(test)] +mod tests { + use super::*; + use approx::assert_relative_eq; + use std::f64::consts::FRAC_PI_2; + + #[test] + fn cross_product_matrix_matches_cross() { + let v = Vec3::new(0.3, -1.2, 2.0); + let w = Vec3::new(1.0, 0.5, -0.7); + assert_relative_eq!(cross_product_matrix(&v) * w, v.cross(&w), epsilon = 1e-12); + } + + #[test] + fn euler_roundtrip() { + let (rx, ry, rz) = (0.3, -0.4, 0.7); + let r = euler_angles_to_rotation_matrix(rx, ry, rz); + let (rx2, ry2, rz2) = rotation_matrix_to_euler_angles(&r); + assert_relative_eq!(rx, rx2, epsilon = 1e-9); + assert_relative_eq!(ry, ry2, epsilon = 1e-9); + assert_relative_eq!(rz, rz2, epsilon = 1e-9); + } + + #[test] + fn euler_composition_order() { + // R should be Rz * Ry * Rx. + let r = euler_angles_to_rotation_matrix(0.1, 0.2, 0.3); + let manual = { + let rz = euler_angles_to_rotation_matrix(0.0, 0.0, 0.3); + let ry = euler_angles_to_rotation_matrix(0.0, 0.2, 0.0); + let rx = euler_angles_to_rotation_matrix(0.1, 0.0, 0.0); + rz * ry * rx + }; + assert_relative_eq!(r, manual, epsilon = 1e-12); + } + + #[test] + fn angle_axis_roundtrip() { + let aa = Vec3::new(0.2, -0.5, 0.9); + let r = angle_axis_to_rotation_matrix(&aa); + let aa2 = rotation_matrix_to_angle_axis(&r); + assert_relative_eq!(aa, aa2, epsilon = 1e-9); + } + + #[test] + fn closest_rotation_of_rotation_is_identity_map() { + let r = euler_angles_to_rotation_matrix(0.4, -0.2, 0.1); + let proj = compute_closest_rotation_matrix(&r); + assert_relative_eq!(r, proj, epsilon = 1e-9); + assert_relative_eq!(proj.determinant(), 1.0, epsilon = 1e-9); + } + + #[test] + fn closest_rotation_projects_noisy_matrix() { + let r = euler_angles_to_rotation_matrix(0.4, -0.2, 0.1); + let noisy = r + Mat3::from_element(0.01); + let proj = compute_closest_rotation_matrix(&noisy); + // Result is a valid rotation. + assert_relative_eq!((proj.transpose() * proj), Mat3::identity(), epsilon = 1e-9); + assert_relative_eq!(proj.determinant(), 1.0, epsilon = 1e-9); + } + + #[test] + fn average_of_identical_quats() { + let q = UnitQuat::new(Vec3::new(0.3, -0.1, 0.5)); + let avg = average_quaternions(&[q, q, q], &[1.0, 1.0, 1.0]); + // Up to sign, equal. + let dot = q.quaternion().coords.dot(&avg.quaternion().coords).abs(); + assert_relative_eq!(dot, 1.0, epsilon = 1e-9); + } + + #[test] + fn average_between_two() { + let q1 = UnitQuat::identity(); + let q2 = UnitQuat::new(Vec3::new(0.0, 0.0, FRAC_PI_2)); + let avg = average_quaternions(&[q1, q2], &[1.0, 1.0]); + // The average of 0 and 90 degrees about z is 45 degrees about z. + let expected = UnitQuat::new(Vec3::new(0.0, 0.0, FRAC_PI_2 / 2.0)); + let dot = expected.quaternion().coords.dot(&avg.quaternion().coords).abs(); + assert_relative_eq!(dot, 1.0, epsilon = 1e-6); + } + + #[test] + fn interpolate_endpoints() { + let p1 = Rigid3d::new(UnitQuat::identity(), Vec3::new(0.0, 0.0, 0.0)); + let p2 = Rigid3d::new( + UnitQuat::new(Vec3::new(0.0, 0.0, FRAC_PI_2)), + Vec3::new(2.0, 4.0, 6.0), + ); + let at0 = interpolate_camera_poses(&p1, &p2, 0.0); + let at1 = interpolate_camera_poses(&p1, &p2, 1.0); + assert_relative_eq!(at0.translation, p1.translation, epsilon = 1e-12); + assert_relative_eq!(at1.translation, p2.translation, epsilon = 1e-12); + let mid = interpolate_camera_poses(&p1, &p2, 0.5); + assert_relative_eq!(mid.translation, Vec3::new(1.0, 2.0, 3.0), epsilon = 1e-12); + } + + #[test] + fn decompose_projection_recovers_components() { + // K with f=800, principal point (320, 240). + let k = Mat3::new(800.0, 0.0, 320.0, 0.0, 800.0, 240.0, 0.0, 0.0, 1.0); + let r = euler_angles_to_rotation_matrix(0.1, -0.2, 0.05); + let t = Vec3::new(1.0, -2.0, 5.0); + let mut p = Mat3x4::zeros(); + let rt = { + let mut m = Mat3x4::zeros(); + m.fixed_view_mut::<3, 3>(0, 0).copy_from(&r); + m.fixed_view_mut::<3, 1>(0, 3).copy_from(&t); + m + }; + p.copy_from(&(k * rt)); + + let (k2, r2, t2) = decompose_projection_matrix(&p).unwrap(); + assert_relative_eq!(k2, k, epsilon = 1e-6); + assert_relative_eq!(r2, r, epsilon = 1e-6); + assert_relative_eq!(t2, t, epsilon = 1e-6); + } + + #[test] + fn cheirality_in_front() { + // Two cameras: cam2 translated +x by 1, looking down +z. + let cam2_from_cam1 = Rigid3d::new(UnitQuat::identity(), Vec3::new(-1.0, 0.0, 0.0)); + // A point in front of both at depth 5. + let pt = Vec3::new(0.0, 0.0, 5.0); + let ray1 = pt; // cam1 == world. + let ray2 = cam2_from_cam1.transform_point(&pt); + let (pts, idx) = check_cheirality(&cam2_from_cam1, &[ray1], &[ray2]); + assert_eq!(idx.len(), 1); + assert_relative_eq!(pts[0], pt, epsilon = 1e-6); + } + + #[test] + fn cheirality_behind_rejected() { + let cam2_from_cam1 = Rigid3d::new(UnitQuat::identity(), Vec3::new(-1.0, 0.0, 0.0)); + // A point behind camera 1. + let ray1 = Vec3::new(0.0, 0.0, -5.0); + let ray2 = Vec3::new(0.0, 0.0, -5.0); + let (_pts, idx) = check_cheirality(&cam2_from_cam1, &[ray1], &[ray2]); + assert!(idx.is_empty()); + } + + #[test] + fn transform_camera_world_identity() { + let cam = Rigid3d::new(UnitQuat::new(Vec3::new(0.1, 0.2, 0.3)), Vec3::new(1.0, 2.0, 3.0)); + let out = transform_camera_world(&Sim3d::identity(), &cam); + let p = Vec3::new(0.5, -1.0, 2.0); + assert_relative_eq!(out.transform_point(&p), cam.transform_point(&p), epsilon = 1e-12); + } + + #[test] + fn transform_camera_world_consistency() { + // For a pure translation+rotation sim3 (scale 1), composing should match + // applying the inverse world transform then the camera pose. + let s = Sim3d::new( + 1.0, + UnitQuat::new(Vec3::new(0.0, 0.0, FRAC_PI_2)), + Vec3::new(1.0, 0.0, 0.0), + ); + let cam = Rigid3d::new(UnitQuat::identity(), Vec3::new(0.0, 0.0, 0.0)); + let out = transform_camera_world(&s, &cam); + let p_new = Vec3::new(2.0, 1.0, 0.0); + let p_old = s.inverse().transform_point(&p_new); + let expected = cam.transform_point(&p_old); + assert_relative_eq!(out.transform_point(&p_new), expected, epsilon = 1e-9); + } +} diff --git a/src/geometry/pose_prior.rs b/src/geometry/pose_prior.rs new file mode 100644 index 0000000..ca8b4bc --- /dev/null +++ b/src/geometry/pose_prior.rs @@ -0,0 +1,117 @@ +//! Geo-referenced pose priors, mirroring `colmap/geometry/pose_prior.h`. +//! +//! A [`PosePrior`] stores a prior on a camera's position (e.g. from GPS) together +//! with its covariance and the coordinate system in which it is expressed. + +use crate::math::{Mat3, Vec3}; + +/// The coordinate system in which a [`PosePrior`] position is expressed. +/// +/// The integer discriminants match COLMAP's `PosePrior::CoordinateSystem` enum. +#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)] +#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))] +#[repr(i32)] +pub enum PosePriorCoordinateSystem { + /// The coordinate system is unknown / unspecified. + Undefined = -1, + /// WGS 84 geodetic coordinates (latitude, longitude, altitude). + Wgs84 = 0, + /// A generic Cartesian coordinate system (e.g. ECEF or local ENU). + Cartesian = 1, +} + +impl Default for PosePriorCoordinateSystem { + #[inline] + fn default() -> Self { + PosePriorCoordinateSystem::Undefined + } +} + +/// A prior on a camera's position, with covariance and coordinate system. +/// +/// By default the position and covariance are filled with `NaN`, meaning "no +/// prior"; use [`PosePrior::has_position`] to test whether a usable position is +/// present. +#[derive(Debug, Clone, Copy, PartialEq)] +pub struct PosePrior { + /// The prior position (meaning depends on [`Self::coordinate_system`]). + pub position: Vec3, + /// The covariance of the position prior. + pub position_covariance: Mat3, + /// The coordinate system the position is expressed in. + pub coordinate_system: PosePriorCoordinateSystem, +} + +impl Default for PosePrior { + #[inline] + fn default() -> Self { + Self { + position: Vec3::new(f64::NAN, f64::NAN, f64::NAN), + position_covariance: Mat3::from_element(f64::NAN), + coordinate_system: PosePriorCoordinateSystem::Undefined, + } + } +} + +impl PosePrior { + /// Builds a prior from a position, with an undefined coordinate system and a + /// `NaN` (unknown) covariance. + #[inline] + pub fn new(position: Vec3) -> Self { + Self { + position, + position_covariance: Mat3::from_element(f64::NAN), + coordinate_system: PosePriorCoordinateSystem::Undefined, + } + } + + /// Returns `true` if the prior holds a usable position (all coordinates + /// finite, i.e. not `NaN`/infinite). + #[inline] + pub fn has_position(&self) -> bool { + self.position.x.is_finite() && self.position.y.is_finite() && self.position.z.is_finite() + } +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn coordinate_system_discriminants() { + assert_eq!(PosePriorCoordinateSystem::Undefined as i32, -1); + assert_eq!(PosePriorCoordinateSystem::Wgs84 as i32, 0); + assert_eq!(PosePriorCoordinateSystem::Cartesian as i32, 1); + } + + #[test] + fn default_is_no_position() { + let prior = PosePrior::default(); + assert!(!prior.has_position()); + assert_eq!(prior.coordinate_system, PosePriorCoordinateSystem::Undefined); + assert!(prior.position_covariance[(0, 0)].is_nan()); + } + + #[test] + fn new_has_position() { + let prior = PosePrior::new(Vec3::new(1.0, 2.0, 3.0)); + assert!(prior.has_position()); + assert_eq!(prior.position, Vec3::new(1.0, 2.0, 3.0)); + // Covariance unknown by default. + assert!(prior.position_covariance[(0, 0)].is_nan()); + } + + #[test] + fn non_finite_position_has_no_position() { + let prior = PosePrior::new(Vec3::new(1.0, f64::INFINITY, 3.0)); + assert!(!prior.has_position()); + } + + #[test] + fn default_coordinate_system_default() { + assert_eq!( + PosePriorCoordinateSystem::default(), + PosePriorCoordinateSystem::Undefined + ); + } +} diff --git a/src/geometry/rigid3.rs b/src/geometry/rigid3.rs new file mode 100644 index 0000000..0e3881c --- /dev/null +++ b/src/geometry/rigid3.rs @@ -0,0 +1,157 @@ +//! Rigid 3D transform (`SE(3)`), mirroring COLMAP's `Rigid3d`. + +use crate::math::{Mat3x4, UnitQuat, Vec3}; +use std::ops::Mul; + +/// A rigid 3D transform: a rotation followed by a translation. +/// +/// By COLMAP convention an instance named `b_from_a` maps a point expressed in +/// frame `a` into frame `b`: +/// +/// ```text +/// p_b = rotation * p_a + translation +/// ``` +/// +/// Composition follows the same convention: `c_from_a = c_from_b * b_from_a`. +/// +/// # Examples +/// ``` +/// use colmap::geometry::Rigid3d; +/// use colmap::math::{UnitQuat, Vec3}; +/// +/// let b_from_a = Rigid3d::new(UnitQuat::identity(), Vec3::new(1.0, 2.0, 3.0)); +/// let p_a = Vec3::new(0.0, 0.0, 0.0); +/// assert_eq!(b_from_a * p_a, Vec3::new(1.0, 2.0, 3.0)); +/// // The inverse undoes the transform. +/// assert_eq!(b_from_a.inverse() * (b_from_a * p_a), p_a); +/// ``` +#[derive(Debug, Clone, Copy, PartialEq)] +#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))] +pub struct Rigid3d { + /// The rotation part, as a unit quaternion. + pub rotation: UnitQuat, + /// The translation part. + pub translation: Vec3, +} + +impl Default for Rigid3d { + #[inline] + fn default() -> Self { + Self::identity() + } +} + +impl Rigid3d { + /// The identity transform. + #[inline] + pub fn identity() -> Self { + Self { rotation: UnitQuat::identity(), translation: Vec3::zeros() } + } + + /// Creates a transform from a rotation and a translation. + #[inline] + pub fn new(rotation: UnitQuat, translation: Vec3) -> Self { + Self { rotation, translation } + } + + /// Returns the inverse transform. If `self` is `b_from_a`, this is `a_from_b`. + #[inline] + pub fn inverse(&self) -> Self { + let inv_rotation = self.rotation.inverse(); + Self { rotation: inv_rotation, translation: inv_rotation * -self.translation } + } + + /// Transforms a point (`p_b = R * p_a + t`). + #[inline] + pub fn transform_point(&self, point: &Vec3) -> Vec3 { + self.rotation * point + self.translation + } + + /// Returns the `[R | t]` matrix form (3×4). + #[inline] + pub fn to_matrix(&self) -> Mat3x4 { + let r = self.rotation.to_rotation_matrix(); + let mut m = Mat3x4::zeros(); + m.fixed_view_mut::<3, 3>(0, 0).copy_from(r.matrix()); + m.fixed_view_mut::<3, 1>(0, 3).copy_from(&self.translation); + m + } + + /// Builds a transform from a `[R | t]` matrix (3×4). The rotation block is + /// projected to the closest valid rotation. + #[inline] + pub fn from_matrix(matrix: &Mat3x4) -> Self { + let r = matrix.fixed_view::<3, 3>(0, 0).into_owned(); + let t = matrix.fixed_view::<3, 1>(0, 3).into_owned(); + let rotation = UnitQuat::from_rotation_matrix(&nalgebra::Rotation3::from_matrix_unchecked(r)); + Self { rotation, translation: t } + } + + /// Returns the origin of the target frame expressed in the source frame, + /// i.e. `rotation⁻¹ * -translation`. For a `cam_from_world` transform this is + /// the camera's projection center in world coordinates. + #[inline] + pub fn target_origin_in_source(&self) -> Vec3 { + self.rotation.inverse() * -self.translation + } +} + +impl Mul for Rigid3d { + type Output = Rigid3d; + /// Composes two transforms: `(c_from_b) * (b_from_a) = c_from_a`. + #[inline] + fn mul(self, rhs: Rigid3d) -> Rigid3d { + Rigid3d { + rotation: self.rotation * rhs.rotation, + translation: self.translation + self.rotation * rhs.translation, + } + } +} + +impl Mul for Rigid3d { + type Output = Vec3; + #[inline] + fn mul(self, point: Vec3) -> Vec3 { + self.transform_point(&point) + } +} + +#[cfg(test)] +mod tests { + use super::*; + use approx::assert_relative_eq; + + fn sample() -> Rigid3d { + Rigid3d::new(UnitQuat::new(Vec3::new(0.3, -0.2, 0.7)), Vec3::new(1.0, -2.0, 0.5)) + } + + #[test] + fn inverse_roundtrip() { + let t = sample(); + let p = Vec3::new(2.0, 3.0, -1.0); + assert_relative_eq!(t.inverse() * (t * p), p, epsilon = 1e-12); + } + + #[test] + fn compose_matches_sequential() { + let a = sample(); + let b = Rigid3d::new(UnitQuat::new(Vec3::new(-0.1, 0.4, 0.2)), Vec3::new(0.0, 1.0, 2.0)); + let p = Vec3::new(1.0, 1.0, 1.0); + assert_relative_eq!((a * b) * p, a * (b * p), epsilon = 1e-12); + } + + #[test] + fn matrix_roundtrip() { + let t = sample(); + let t2 = Rigid3d::from_matrix(&t.to_matrix()); + let p = Vec3::new(0.5, -1.5, 2.5); + assert_relative_eq!(t * p, t2 * p, epsilon = 1e-12); + } + + #[test] + fn projection_center() { + let t = sample(); + // Mapping the projection center through the transform yields the origin. + assert_relative_eq!(t * t.target_origin_in_source(), Vec3::zeros(), epsilon = 1e-12); + } +} diff --git a/src/geometry/rotation.rs b/src/geometry/rotation.rs new file mode 100644 index 0000000..749afd6 --- /dev/null +++ b/src/geometry/rotation.rs @@ -0,0 +1,126 @@ +//! 3D rotation, mirroring PyCOLMAP's `Rotation3d`. + +use crate::math::{Mat3, UnitQuat, Vec3}; + +/// A 3D rotation stored as a unit quaternion. +/// +/// This is a thin wrapper around [`nalgebra::UnitQuaternion`] that matches +/// PyCOLMAP's `Rotation3d` API (construction from a quaternion, rotation matrix, +/// or angle-axis vector). The quaternion follows nalgebra's coefficient order +/// `[x, y, z, w]`; constructors that take an explicit scalar use `(w, x, y, z)` +/// order, like Eigen and COLMAP. +#[derive(Debug, Clone, Copy, PartialEq)] +#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))] +pub struct Rotation3d { + /// The underlying unit quaternion. + pub quat: UnitQuat, +} + +impl Default for Rotation3d { + #[inline] + fn default() -> Self { + Self::identity() + } +} + +impl Rotation3d { + /// The identity rotation. + #[inline] + pub fn identity() -> Self { + Self { quat: UnitQuat::identity() } + } + + /// Builds a rotation from a (not necessarily normalized) quaternion given in + /// `(w, x, y, z)` scalar-first order. The quaternion is normalized. + #[inline] + pub fn from_wxyz(w: f64, x: f64, y: f64, z: f64) -> Self { + Self { quat: UnitQuat::from_quaternion(crate::math::Quat::new(w, x, y, z)) } + } + + /// Builds a rotation from an existing unit quaternion. + #[inline] + pub fn from_unit_quaternion(quat: UnitQuat) -> Self { + Self { quat } + } + + /// Builds a rotation from a 3×3 rotation matrix (projected to the closest + /// valid rotation). + #[inline] + pub fn from_matrix(matrix: &Mat3) -> Self { + let rot = nalgebra::Rotation3::from_matrix_unchecked(*matrix); + Self { quat: UnitQuat::from_rotation_matrix(&rot) } + } + + /// Builds a rotation from an angle-axis vector (direction = axis, magnitude = angle in radians). + #[inline] + pub fn from_angle_axis(angle_axis: Vec3) -> Self { + Self { quat: UnitQuat::new(angle_axis) } + } + + /// Returns the quaternion coefficients in `(w, x, y, z)` order. + #[inline] + pub fn wxyz(&self) -> [f64; 4] { + let q = self.quat.quaternion(); + [q.w, q.i, q.j, q.k] + } + + /// Returns the equivalent 3×3 rotation matrix. + #[inline] + pub fn to_matrix(&self) -> Mat3 { + self.quat.to_rotation_matrix().into_inner() + } + + /// Returns the angle-axis representation (magnitude = angle in radians). + #[inline] + pub fn to_angle_axis(&self) -> Vec3 { + self.quat.scaled_axis() + } + + /// Returns the inverse rotation. + #[inline] + pub fn inverse(&self) -> Self { + Self { quat: self.quat.inverse() } + } + + /// Rotates a 3D vector. + #[inline] + pub fn rotate(&self, v: &Vec3) -> Vec3 { + self.quat * v + } +} + +impl From for Rotation3d { + #[inline] + fn from(quat: UnitQuat) -> Self { + Self { quat } + } +} + +#[cfg(test)] +mod tests { + use super::*; + use approx::assert_relative_eq; + use std::f64::consts::FRAC_PI_2; + + #[test] + fn matrix_roundtrip() { + let r = Rotation3d::from_angle_axis(Vec3::new(0.1, -0.7, 0.3)); + let r2 = Rotation3d::from_matrix(&r.to_matrix()); + let v = Vec3::new(1.0, 2.0, 3.0); + assert_relative_eq!(r.rotate(&v), r2.rotate(&v), epsilon = 1e-12); + } + + #[test] + fn rotate_about_z() { + let r = Rotation3d::from_angle_axis(Vec3::new(0.0, 0.0, FRAC_PI_2)); + let v = r.rotate(&Vec3::new(1.0, 0.0, 0.0)); + assert_relative_eq!(v, Vec3::new(0.0, 1.0, 0.0), epsilon = 1e-12); + } + + #[test] + fn inverse_cancels() { + let r = Rotation3d::from_angle_axis(Vec3::new(0.4, 0.2, -0.5)); + let v = Vec3::new(3.0, -1.0, 2.0); + assert_relative_eq!(r.inverse().rotate(&r.rotate(&v)), v, epsilon = 1e-12); + } +} diff --git a/src/geometry/sim3.rs b/src/geometry/sim3.rs new file mode 100644 index 0000000..0fcdae3 --- /dev/null +++ b/src/geometry/sim3.rs @@ -0,0 +1,142 @@ +//! Similarity 3D transform (`Sim(3)`), mirroring COLMAP's `Sim3d`. + +use crate::math::{Mat3x4, UnitQuat, Vec3}; +use std::ops::Mul; + +/// A 3D similarity transform: a uniform scale, a rotation and a translation. +/// +/// By COLMAP convention an instance named `b_from_a` maps a point in frame `a` +/// into frame `b`: +/// +/// ```text +/// p_b = scale * (rotation * p_a) + translation +/// ``` +/// +/// Used mainly to align reconstructions to one another or to a geo-referenced +/// coordinate frame. +#[derive(Debug, Clone, Copy, PartialEq)] +#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))] +pub struct Sim3d { + /// The uniform scale factor. + pub scale: f64, + /// The rotation part, as a unit quaternion. + pub rotation: UnitQuat, + /// The translation part. + pub translation: Vec3, +} + +impl Default for Sim3d { + #[inline] + fn default() -> Self { + Self::identity() + } +} + +impl Sim3d { + /// The identity transform (scale 1, no rotation, no translation). + #[inline] + pub fn identity() -> Self { + Self { scale: 1.0, rotation: UnitQuat::identity(), translation: Vec3::zeros() } + } + + /// Creates a similarity transform. + #[inline] + pub fn new(scale: f64, rotation: UnitQuat, translation: Vec3) -> Self { + Self { scale, rotation, translation } + } + + /// Returns the inverse transform. If `self` is `b_from_a`, this is `a_from_b`. + #[inline] + pub fn inverse(&self) -> Self { + let inv_rotation = self.rotation.inverse(); + Self { + scale: 1.0 / self.scale, + rotation: inv_rotation, + translation: (inv_rotation * self.translation) / -self.scale, + } + } + + /// Transforms a point (`p_b = s * R * p_a + t`). + #[inline] + pub fn transform_point(&self, point: &Vec3) -> Vec3 { + self.scale * (self.rotation * point) + self.translation + } + + /// Returns the `[s·R | t]` matrix form (3×4). + #[inline] + pub fn to_matrix(&self) -> Mat3x4 { + let r = self.rotation.to_rotation_matrix(); + let mut m = Mat3x4::zeros(); + m.fixed_view_mut::<3, 3>(0, 0).copy_from(&(r.matrix() * self.scale)); + m.fixed_view_mut::<3, 1>(0, 3).copy_from(&self.translation); + m + } + + /// Builds a transform from a `[s·R | t]` matrix (3×4). The scale is recovered + /// as the norm of the first column. + #[inline] + pub fn from_matrix(matrix: &Mat3x4) -> Self { + let linear = matrix.fixed_view::<3, 3>(0, 0).into_owned(); + let scale = linear.column(0).norm(); + let rotation = UnitQuat::from_rotation_matrix(&nalgebra::Rotation3::from_matrix_unchecked( + linear / scale, + )); + let translation = matrix.fixed_view::<3, 1>(0, 3).into_owned(); + Self { scale, rotation, translation } + } +} + +impl Mul for Sim3d { + type Output = Sim3d; + /// Composes two transforms: `(c_from_b) * (b_from_a) = c_from_a`. + #[inline] + fn mul(self, rhs: Sim3d) -> Sim3d { + Sim3d { + scale: self.scale * rhs.scale, + rotation: self.rotation * rhs.rotation, + translation: self.translation + self.scale * (self.rotation * rhs.translation), + } + } +} + +impl Mul for Sim3d { + type Output = Vec3; + #[inline] + fn mul(self, point: Vec3) -> Vec3 { + self.transform_point(&point) + } +} + +#[cfg(test)] +mod tests { + use super::*; + use approx::assert_relative_eq; + + fn sample() -> Sim3d { + Sim3d::new(2.5, UnitQuat::new(Vec3::new(0.2, 0.1, -0.4)), Vec3::new(1.0, -2.0, 3.0)) + } + + #[test] + fn inverse_roundtrip() { + let t = sample(); + let p = Vec3::new(4.0, -1.0, 0.5); + assert_relative_eq!(t.inverse() * (t * p), p, epsilon = 1e-10); + } + + #[test] + fn matrix_roundtrip() { + let t = sample(); + let t2 = Sim3d::from_matrix(&t.to_matrix()); + assert_relative_eq!(t.scale, t2.scale, epsilon = 1e-12); + let p = Vec3::new(0.5, 1.5, -2.5); + assert_relative_eq!(t * p, t2 * p, epsilon = 1e-10); + } + + #[test] + fn compose_associative() { + let a = sample(); + let b = Sim3d::new(0.5, UnitQuat::new(Vec3::new(-0.3, 0.2, 0.1)), Vec3::new(2.0, 0.0, 1.0)); + let p = Vec3::new(1.0, 1.0, 1.0); + assert_relative_eq!((a * b) * p, a * (b * p), epsilon = 1e-10); + } +} diff --git a/src/geometry/triangulation.rs b/src/geometry/triangulation.rs new file mode 100644 index 0000000..c51130d --- /dev/null +++ b/src/geometry/triangulation.rs @@ -0,0 +1,252 @@ +//! Point triangulation, mirroring `colmap/geometry/triangulation.h`. +//! +//! Provides linear (DLT) two-view triangulation, the mid-point method, a +//! multi-view DLT, and helpers to compute triangulation angles between observing +//! rays. + +use crate::math::{Mat3x4, Vec2, Vec3}; +use nalgebra::{Matrix4, Vector4}; + +/// Triangulates a 3D point from two views using the linear DLT (Direct Linear +/// Transform) method. +/// +/// `p1`, `p2` are the `[R | t]` (or full projection) matrices and `x1`, `x2` the +/// corresponding image points (in the same coordinate system as the projection +/// matrices, typically normalized camera coordinates). Returns `None` if the +/// system is degenerate. +/// +/// # Examples +/// ``` +/// use colmap::geometry::triangulation::triangulate_point; +/// use colmap::geometry::Rigid3d; +/// use colmap::math::{UnitQuat, Vec2, Vec3}; +/// +/// let p1 = Rigid3d::identity().to_matrix(); +/// let pose2 = Rigid3d::new(UnitQuat::identity(), Vec3::new(-1.0, 0.0, 0.0)); +/// let p2 = pose2.to_matrix(); +/// let point = Vec3::new(0.2, -0.1, 5.0); +/// let x1 = Vec2::new(point.x / point.z, point.y / point.z); +/// let pc2 = pose2.transform_point(&point); +/// let x2 = Vec2::new(pc2.x / pc2.z, pc2.y / pc2.z); +/// let tri = triangulate_point(&p1, &p2, &x1, &x2).unwrap(); +/// assert!((tri - point).norm() < 1e-9); +/// ``` +pub fn triangulate_point(p1: &Mat3x4, p2: &Mat3x4, x1: &Vec2, x2: &Vec2) -> Option { + // Build the 4x4 system A X = 0, two rows per view. + let mut a = Matrix4::::zeros(); + a.row_mut(0).copy_from(&(x1.x * p1.row(2) - p1.row(0))); + a.row_mut(1).copy_from(&(x1.y * p1.row(2) - p1.row(1))); + a.row_mut(2).copy_from(&(x2.x * p2.row(2) - p2.row(0))); + a.row_mut(3).copy_from(&(x2.y * p2.row(2) - p2.row(1))); + + let svd = a.svd(false, true); + let v_t = svd.v_t?; + // The solution is the right-singular vector with the smallest singular value, + // i.e. the last row of V^T. + let xh: Vector4 = v_t.row(3).transpose(); + if xh.w.abs() < f64::EPSILON { + return None; + } + Some(Vec3::new(xh.x / xh.w, xh.y / xh.w, xh.z / xh.w)) +} + +/// Triangulates a 3D point (in camera-1 coordinates) from two bearing rays using +/// the mid-point method. +/// +/// `cam2_from_cam1` is the relative pose, `ray1`/`ray2` are direction vectors in +/// each camera frame. Returns the point that minimizes the distance to both +/// rays, or `None` if the rays are (near) parallel. +pub fn triangulate_mid_point( + cam2_from_cam1: &crate::geometry::Rigid3d, + ray1: &Vec3, + ray2: &Vec3, +) -> Option { + // Camera 1 at the origin, looking along ray1. + // Camera 2 center (in cam1 coords) and ray2 rotated into cam1 coords. + let r = cam2_from_cam1.rotation.to_rotation_matrix(); + let c2 = cam2_from_cam1.target_origin_in_source(); // camera-2 center in cam1. + let d1 = ray1.normalize(); + let d2 = (r.inverse() * ray2).normalize(); + + // Solve for parameters t1, t2 minimizing || (t1 d1) - (c2 + t2 d2) ||. + let b = c2; + let d1d1 = d1.dot(&d1); + let d1d2 = d1.dot(&d2); + let d2d2 = d2.dot(&d2); + let denom = d1d1 * d2d2 - d1d2 * d1d2; + if denom.abs() < 1e-12 { + return None; + } + let d1b = d1.dot(&b); + let d2b = d2.dot(&b); + let t1 = (d2d2 * d1b - d1d2 * d2b) / denom; + let t2 = (d1d2 * d1b - d1d1 * d2b) / denom; + + let point1 = t1 * d1; + let point2 = c2 + t2 * d2; + Some(0.5 * (point1 + point2)) +} + +/// Triangulates a 3D point from an arbitrary number of views using the linear +/// DLT method (stacking two rows per observation and solving by SVD). +/// +/// `proj_matrices` are the `[R | t]`/projection matrices and `points` the +/// corresponding image points. Requires at least two views; returns `None` for +/// fewer views or a degenerate system. +pub fn triangulate_multi_view_point(proj_matrices: &[Mat3x4], points: &[Vec2]) -> Option { + let n = proj_matrices.len().min(points.len()); + if n < 2 { + return None; + } + // Accumulate the normal-equation matrix A^T A (4x4) from all observations. + let mut ata = Matrix4::::zeros(); + for i in 0..n { + let p = &proj_matrices[i]; + let x = &points[i]; + let row0: Vector4 = (x.x * p.row(2) - p.row(0)).transpose(); + let row1: Vector4 = (x.y * p.row(2) - p.row(1)).transpose(); + ata += row0 * row0.transpose(); + ata += row1 * row1.transpose(); + } + let eig = ata.symmetric_eigen(); + // Smallest eigenvalue's eigenvector is the solution. + let mut best_idx = 0; + let mut best_val = f64::INFINITY; + for i in 0..4 { + if eig.eigenvalues[i] < best_val { + best_val = eig.eigenvalues[i]; + best_idx = i; + } + } + let xh = eig.eigenvectors.column(best_idx); + if xh[3].abs() < f64::EPSILON { + return None; + } + Some(Vec3::new(xh[0] / xh[3], xh[1] / xh[3], xh[2] / xh[3])) +} + +/// Computes the triangulation angle (in radians) at a 3D `point` as seen from two +/// camera projection centers `c1` and `c2`. +/// +/// A larger angle means a better-conditioned triangulation; COLMAP uses this to +/// filter degenerate (near-zero parallax) points. +pub fn calculate_triangulation_angle(c1: &Vec3, c2: &Vec3, point: &Vec3) -> f64 { + let ray1 = point - c1; + let ray2 = point - c2; + let baseline_sq = (c1 - c2).norm_squared(); + let ray1_sq = ray1.norm_squared(); + let ray2_sq = ray2.norm_squared(); + if ray1_sq < f64::EPSILON || ray2_sq < f64::EPSILON { + return 0.0; + } + // Law of cosines: cos(angle) = (|ray1|^2 + |ray2|^2 - baseline^2) / + // (2 |ray1| |ray2|). + let denom = 2.0 * (ray1_sq * ray2_sq).sqrt(); + let cos_angle = ((ray1_sq + ray2_sq - baseline_sq) / denom).clamp(-1.0, 1.0); + let angle = cos_angle.acos(); + // Return the acute angle, matching COLMAP (triangulation angle in [0, pi/2]). + angle.min(std::f64::consts::PI - angle) +} + +/// Computes the angle (in radians) between two vectors, in `[0, pi]`. Returns 0 +/// if either vector is (near) zero. +pub fn calculate_angle_between_vectors(a: &Vec3, b: &Vec3) -> f64 { + let na = a.norm(); + let nb = b.norm(); + if na < f64::EPSILON || nb < f64::EPSILON { + return 0.0; + } + let cos_angle = (a.dot(b) / (na * nb)).clamp(-1.0, 1.0); + cos_angle.acos() +} + +#[cfg(test)] +mod tests { + use super::*; + use crate::geometry::Rigid3d; + use crate::math::UnitQuat; + use approx::assert_relative_eq; + use std::f64::consts::FRAC_PI_2; + + fn project(pose: &Rigid3d, p: &Vec3) -> Vec2 { + let pc = pose.transform_point(p); + Vec2::new(pc.x / pc.z, pc.y / pc.z) + } + + #[test] + fn dlt_triangulates_known_point() { + let pose1 = Rigid3d::identity(); + let pose2 = Rigid3d::new( + UnitQuat::new(Vec3::new(0.0, 0.05, 0.0)), + Vec3::new(-1.0, 0.0, 0.0), + ); + let point = Vec3::new(0.4, -0.7, 6.0); + let x1 = project(&pose1, &point); + let x2 = project(&pose2, &point); + let tri = triangulate_point(&pose1.to_matrix(), &pose2.to_matrix(), &x1, &x2).unwrap(); + assert_relative_eq!(tri, point, epsilon = 1e-9); + } + + #[test] + fn midpoint_triangulates_known_point() { + let pose2 = Rigid3d::new(UnitQuat::identity(), Vec3::new(-1.0, 0.0, 0.0)); + let point = Vec3::new(0.2, 0.1, 5.0); + let ray1 = point; // cam1 == world. + let ray2 = pose2.transform_point(&point); + let tri = triangulate_mid_point(&pose2, &ray1, &ray2).unwrap(); + assert_relative_eq!(tri, point, epsilon = 1e-9); + } + + #[test] + fn midpoint_parallel_rays_none() { + let pose2 = Rigid3d::new(UnitQuat::identity(), Vec3::new(-1.0, 0.0, 0.0)); + // Both rays point along +z => parallel in cam1 coords => no intersection. + let ray1 = Vec3::new(0.0, 0.0, 1.0); + let ray2 = Vec3::new(0.0, 0.0, 1.0); + assert!(triangulate_mid_point(&pose2, &ray1, &ray2).is_none()); + } + + #[test] + fn multi_view_triangulates_known_point() { + let poses = [ + Rigid3d::identity(), + Rigid3d::new(UnitQuat::identity(), Vec3::new(-1.0, 0.0, 0.0)), + Rigid3d::new(UnitQuat::new(Vec3::new(0.0, 0.1, 0.0)), Vec3::new(-2.0, 0.3, 0.0)), + ]; + let point = Vec3::new(-0.5, 0.8, 7.0); + let mats: Vec = poses.iter().map(|p| p.to_matrix()).collect(); + let pts: Vec = poses.iter().map(|p| project(p, &point)).collect(); + let tri = triangulate_multi_view_point(&mats, &pts).unwrap(); + assert_relative_eq!(tri, point, epsilon = 1e-8); + } + + #[test] + fn multi_view_needs_two_views() { + let mats = vec![Rigid3d::identity().to_matrix()]; + let pts = vec![Vec2::new(0.0, 0.0)]; + assert!(triangulate_multi_view_point(&mats, &pts).is_none()); + } + + #[test] + fn triangulation_angle_right_angle() { + // Two cameras viewing a point at 90 degrees. + let c1 = Vec3::new(-1.0, 0.0, 0.0); + let c2 = Vec3::new(0.0, 0.0, -1.0); + let point = Vec3::new(0.0, 0.0, 0.0); + // ray1 = point - c1 = (1,0,0), ray2 = (0,0,1) -> angle 90 deg. + let angle = calculate_triangulation_angle(&c1, &c2, &point); + assert_relative_eq!(angle, FRAC_PI_2, epsilon = 1e-9); + } + + #[test] + fn angle_between_vectors_basic() { + let a = Vec3::new(1.0, 0.0, 0.0); + let b = Vec3::new(0.0, 1.0, 0.0); + assert_relative_eq!( + calculate_angle_between_vectors(&a, &b), + FRAC_PI_2, + epsilon = 1e-12 + ); + assert_relative_eq!(calculate_angle_between_vectors(&a, &a), 0.0, epsilon = 1e-12); + } +} diff --git a/src/highlevel/mod.rs b/src/highlevel/mod.rs new file mode 100644 index 0000000..a7d11f4 --- /dev/null +++ b/src/highlevel/mod.rs @@ -0,0 +1,1199 @@ +//! High-level, end-to-end reconstruction API (feature → SfM → MVS → export). +//! +//! The rest of this crate mirrors **PyCOLMAP** stage by stage and leaves the +//! heavy numerical cores returning [`Error::Unimplemented`]. +//! This module instead offers the *flat, ergonomic* pipeline API of the +//! higher-level `colmap` crate — [`load_images_from_directory`], +//! [`FeaturePipeline`], [`IncrementalSfm`], [`MvsReconstructor`] and the +//! [`save_reconstruction`] / [`save_point_cloud`] / [`save_mesh`] writers — so a +//! whole reconstruction can be driven in a handful of lines: +//! +//! ```no_run +//! use colmap::*; +//! use std::path::Path; +//! +//! # fn run(image_dir: &Path) -> Result<()> { +//! let images = load_images_from_directory(image_dir)?; +//! +//! let pipeline = FeaturePipeline::new(PipelineConfig { +//! detector_type: DetectorType::Sift, +//! max_features: 8000, +//! ..Default::default() +//! }); +//! let extraction = pipeline.extract_and_match_all(&images)?; +//! +//! let mut sfm = IncrementalSfm::new(SfmConfig { min_track_length: 2, ..Default::default() }); +//! sfm.set_features(extraction.features); +//! sfm.set_matches(extraction.matches); +//! let sparse = sfm.reconstruct()?; +//! +//! let mvs = MvsReconstructor::new(MvsConfig { min_num_views: 3, ..Default::default() }); +//! let views = prepare_views_from_reconstruction(&sparse)?; +//! let dense = mvs.reconstruct(&views)?; +//! +//! save_reconstruction(&sparse, "sparse")?; +//! save_point_cloud(&dense.point_cloud, "dense.ply")?; +//! save_mesh(&dense.mesh, "mesh.obj")?; +//! # Ok(()) +//! # } +//! ``` +//! +//! # What is real and what is a demonstration +//! +//! The **data structures**, the **track building** (union-find over feature +//! matches), the **multi-view triangulation**, the **reprojection-error** +//! statistics and the **PLY / OBJ / COLMAP-model writers** are genuine and +//! reusable. The parts that in COLMAP require decoded pixels and GPU-heavy +//! solvers — SIFT detection, descriptor matching from real image content, robust +//! pose estimation, and PatchMatch dense stereo — are **not** ported in this +//! dependency-light crate. Instead this module synthesizes a deterministic, +//! internally-consistent scene (a fixed cloud of 3-D landmarks viewed along a +//! camera arc derived from each image's header) so that every stage runs, the +//! numbers are plausible, and the exported `.ply` / `.obj` / model files are +//! valid and openable. Treat the geometry as illustrative, not as a metric +//! reconstruction of your photos' content. + +use std::collections::HashMap; +use std::fmt::Write as _; +use std::path::{Path, PathBuf}; + +use crate::geometry::triangulation::triangulate_multi_view_point; +use crate::geometry::Rigid3d; +use crate::math::{Mat3, Mat3x4, Vec2, Vec3}; +use crate::scene::{Camera, Image as SceneImage, Point3D as ScenePoint3D, Reconstruction, Track, TrackElement}; +use crate::types::{CameraId, CameraModelId, ImageId}; +use crate::{Error, Result}; + +// =========================================================================== +// Tunable constants of the built-in synthetic scene (see the module docs). +// =========================================================================== + +/// Length of each synthetic descriptor (SIFT is 128-dimensional). +const DESC_DIM: usize = 128; +/// Upper bound on landmarks materialised by the demo detector, regardless of +/// [`PipelineConfig::max_features`], to keep brute-force matching snappy. +const MAX_DEMO_LANDMARKS: usize = 128; +/// Each image is matched against the next `MATCH_WINDOW` images (sequential +/// matching), which is enough to chain every landmark into one track. +const MATCH_WINDOW: usize = 2; +/// Radius of the sphere the synthetic landmarks are sampled on. +const SCENE_RADIUS: f64 = 0.8; +/// Distance of every synthetic camera from the scene centre. +const CAMERA_RADIUS: f64 = 5.0; +/// Total angular span (radians) the cameras are spread over. +const CAMERA_ARC: f64 = 1.2; +/// Height the camera arc is lifted above the scene centre. +const CAMERA_ELEVATION: f64 = 0.6; +/// Magnitude (pixels) of the deterministic sub-pixel keypoint noise. +const KEYPOINT_JITTER: f64 = 0.15; + +// =========================================================================== +// Deterministic pseudo-randomness (so the pipeline is fully reproducible). +// =========================================================================== + +/// SplitMix64: a fast, well-distributed deterministic bit mixer. +fn splitmix64(seed: u64) -> u64 { + let mut z = seed.wrapping_add(0x9E37_79B9_7F4A_7C15); + z = (z ^ (z >> 30)).wrapping_mul(0xBF58_476D_1CE4_E5B9); + z = (z ^ (z >> 27)).wrapping_mul(0x94D0_49BB_1331_11EB); + z ^ (z >> 31) +} + +/// Combines two integers into one seed. +fn mix2(a: u64, b: u64) -> u64 { + splitmix64(a.wrapping_mul(0x100_0000_01B3).wrapping_add(b)) +} + +/// A deterministic float in `[0, 1)` from a seed. +fn unit_f64(seed: u64) -> f64 { + (splitmix64(seed) >> 11) as f64 / (1u64 << 53) as f64 +} + +// =========================================================================== +// 1. Image loading +// =========================================================================== + +/// A source image discovered on disk. +/// +/// Only the file header is read, so [`width`](Self::width) / [`height`](Self::height) +/// are exact but the pixels are never decoded (consistent with the rest of this +/// dependency-light crate). +#[derive(Debug, Clone, PartialEq, Eq)] +pub struct Image { + /// 1-based identifier, assigned in sorted file-name order. + pub id: u32, + /// The image file name (without directory). + pub name: String, + /// Full path to the image file. + pub path: PathBuf, + /// Image width in pixels. + pub width: u32, + /// Image height in pixels. + pub height: u32, +} + +/// Loads every JPEG/PNG in `dir` (non-recursive), reading each one's pixel size +/// from its header. Images are returned sorted by file name with 1-based ids. +/// +/// # Errors +/// Returns an [`Error`] if the directory cannot be read or an image header is +/// malformed. +pub fn load_images_from_directory(dir: &Path) -> Result> { + let paths = crate::image::list_images_in_dir(dir)?; + let mut images = Vec::with_capacity(paths.len()); + for (i, path) in paths.into_iter().enumerate() { + let (width, height) = crate::image::read_image_size(&path)?; + let name = path + .file_name() + .and_then(|n| n.to_str()) + .ok_or_else(|| Error::InvalidArgument("non-UTF-8 image file name".into()))? + .to_string(); + images.push(Image { + id: (i + 1) as u32, + name, + path, + width, + height, + }); + } + Ok(images) +} + +// =========================================================================== +// 2. Feature extraction & matching +// =========================================================================== + +/// The local-feature detector to use. +#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash, Default)] +pub enum DetectorType { + /// Scale-Invariant Feature Transform (COLMAP's default). + #[default] + Sift, + /// Oriented FAST and rotated BRIEF. + Orb, + /// Accelerated-KAZE features. + Akaze, +} + +/// Configuration for the [`FeaturePipeline`]. +#[derive(Debug, Clone, PartialEq)] +pub struct PipelineConfig { + /// Which detector to use. + pub detector_type: DetectorType, + /// Maximum number of features to keep per image. + pub max_features: usize, + /// Lowe ratio-test threshold (best / second-best descriptor distance). + pub match_ratio: f64, + /// Whether to keep only mutually-best (cross-checked) matches. + pub cross_check: bool, +} + +impl Default for PipelineConfig { + fn default() -> Self { + Self { + detector_type: DetectorType::Sift, + max_features: 8192, + match_ratio: 0.8, + cross_check: true, + } + } +} + +/// A single detected keypoint. +#[derive(Debug, Clone, Copy, PartialEq)] +pub struct Keypoint { + /// Sub-pixel x image coordinate. + pub x: f32, + /// Sub-pixel y image coordinate. + pub y: f32, + /// Detection scale. + pub scale: f32, + /// Dominant orientation in radians. + pub orientation: f32, +} + +/// The keypoints and descriptors detected in one image. +#[derive(Debug, Clone, PartialEq)] +pub struct ImageFeatures { + /// Id of the image these features belong to. + pub image_id: u32, + /// The image's file name (carried through for export). + pub name: String, + /// Image width in pixels. + pub width: u32, + /// Image height in pixels. + pub height: u32, + /// The detected keypoints. + pub keypoints: Vec, + /// One descriptor (length 128, the SIFT dimension) per keypoint, in the same order. + pub descriptors: Vec>, +} + +/// The verified matches between one ordered pair of images. +#[derive(Debug, Clone, PartialEq)] +pub struct ImageMatches { + /// Id of the first image. + pub image_id1: u32, + /// Id of the second image. + pub image_id2: u32, + /// `(keypoint index in image1, keypoint index in image2)` correspondences. + pub matches: Vec<(u32, u32)>, +} + +/// The combined output of [`FeaturePipeline::extract_and_match_all`]. +#[derive(Debug, Clone, PartialEq, Default)] +pub struct ExtractionResult { + /// Per-image features (one entry per input image). + pub features: Vec, + /// Per-image-pair matches. + pub matches: Vec, +} + +/// Drives feature extraction and matching over a set of [`Image`]s. +#[derive(Debug, Clone)] +pub struct FeaturePipeline { + config: PipelineConfig, +} + +impl FeaturePipeline { + /// Creates a pipeline with the given configuration. + pub fn new(config: PipelineConfig) -> Self { + Self { config } + } + + /// The configuration this pipeline was built with. + pub fn config(&self) -> &PipelineConfig { + &self.config + } + + /// Extracts features from every image and matches them across image pairs. + /// + /// See the [module docs](crate::highlevel) for what the synthetic detector + /// and matcher actually compute. + pub fn extract_and_match_all(&self, images: &[Image]) -> Result { + let n_land = self.config.max_features.min(MAX_DEMO_LANDMARKS); + let scene = synthetic_scene(n_land); + let descriptors: Vec> = (0..n_land).map(descriptor_for_landmark).collect(); + let count = images.len(); + + // --- Extraction: project the shared landmarks into every image. --- + let mut features = Vec::with_capacity(count); + for (rank, image) in images.iter().enumerate() { + let camera = pinhole_camera(image.id, image.width, image.height); + let pose = camera_pose(rank, count); + let mut keypoints = Vec::with_capacity(n_land); + for (k, landmark) in scene.iter().enumerate() { + let cam_pt = pose.transform_point(landmark); + let Some(uv) = camera.img_from_cam(&cam_pt) else { continue }; + let dx = (unit_f64(mix2(rank as u64, 2 * k as u64)) - 0.5) * 2.0 * KEYPOINT_JITTER; + let dy = (unit_f64(mix2(rank as u64, 2 * k as u64 + 1)) - 0.5) * 2.0 * KEYPOINT_JITTER; + keypoints.push(Keypoint { + x: (uv.x + dx) as f32, + y: (uv.y + dy) as f32, + scale: 1.0 + (k % 4) as f32, + orientation: 0.0, + }); + } + // Keypoint index == landmark index, so descriptors line up 1:1. + let descs = descriptors[..keypoints.len()].to_vec(); + features.push(ImageFeatures { + image_id: image.id, + name: image.name.clone(), + width: image.width, + height: image.height, + keypoints, + descriptors: descs, + }); + } + + // --- Matching: brute-force nearest-neighbour within a sliding window. --- + let mut matches = Vec::new(); + for a in 0..features.len() { + for b in (a + 1)..(a + 1 + MATCH_WINDOW).min(features.len()) { + let pairs = match_descriptors( + &features[a].descriptors, + &features[b].descriptors, + self.config.match_ratio, + self.config.cross_check, + ); + if !pairs.is_empty() { + matches.push(ImageMatches { + image_id1: features[a].image_id, + image_id2: features[b].image_id, + matches: pairs, + }); + } + } + } + + Ok(ExtractionResult { features, matches }) + } +} + +/// Brute-force nearest-neighbour matching with a Lowe ratio test and optional +/// cross-check. Returns `(i, j)` keypoint-index pairs. +fn match_descriptors( + d1: &[Vec], + d2: &[Vec], + ratio: f64, + cross_check: bool, +) -> Vec<(u32, u32)> { + let ratio_sq = (ratio * ratio) as f32; + let mut out = Vec::new(); + for (i, a) in d1.iter().enumerate() { + let Some((j, best, second)) = nearest_two(a, d2) else { continue }; + // Ratio test: best must be clearly closer than the runner-up. + if let Some(second) = second { + if best > ratio_sq * second { + continue; + } + } + if cross_check { + // The reverse nearest neighbour of j must be i. + if let Some((back, _, _)) = nearest_two(&d2[j], d1) { + if back != i { + continue; + } + } + } + out.push((i as u32, j as u32)); + } + out +} + +/// Returns `(index of nearest, nearest squared distance, second-nearest squared +/// distance)` of `query` within `set`, or `None` if `set` is empty. +fn nearest_two(query: &[f32], set: &[Vec]) -> Option<(usize, f32, Option)> { + let mut best = (usize::MAX, f32::INFINITY); + let mut second = f32::INFINITY; + for (j, cand) in set.iter().enumerate() { + let dist: f32 = query + .iter() + .zip(cand) + .map(|(p, q)| { + let d = p - q; + d * d + }) + .sum(); + if dist < best.1 { + second = best.1; + best = (j, dist); + } else if dist < second { + second = dist; + } + } + if best.0 == usize::MAX { + None + } else { + Some((best.0, best.1, second.is_finite().then_some(second))) + } +} + +// =========================================================================== +// 3. Sparse Structure-from-Motion +// =========================================================================== + +/// Configuration for [`IncrementalSfm`]. +#[derive(Debug, Clone, PartialEq)] +pub struct SfmConfig { + /// Minimum number of observations a track needs to be triangulated. + pub min_track_length: usize, + /// Maximum mean reprojection error (pixels) a 3-D point may keep. + pub max_reprojection_error: f64, + /// Minimum number of matches an image pair needs to contribute to mapping. + pub min_num_matches: usize, +} + +impl Default for SfmConfig { + fn default() -> Self { + Self { + min_track_length: 2, + max_reprojection_error: 4.0, + min_num_matches: 15, + } + } +} + +/// One observation of a 3-D point in an image. +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +pub struct Observation { + /// Id of the observing image. + pub image_id: u32, + /// Index of the observing keypoint within that image. + pub keypoint_index: u32, +} + +/// A triangulated 3-D point. +#[derive(Debug, Clone, PartialEq)] +pub struct Point3d { + /// World-space position. + pub position: Vec3, + /// RGB colour. + pub color: [u8; 3], + /// Mean reprojection error in pixels. + pub error: f64, + /// The observations this point was triangulated from. + pub track: Vec, +} + +/// A registered camera: its intrinsics, world pose and image metadata. +#[derive(Debug, Clone, PartialEq)] +pub struct CameraView { + /// Id of the image. + pub image_id: u32, + /// The image's file name. + pub name: String, + /// Image width in pixels. + pub width: u32, + /// Image height in pixels. + pub height: u32, + /// The intrinsic calibration. + pub camera: Camera, + /// The `cam_from_world` pose. + pub cam_from_world: Rigid3d, +} + +/// The sparse reconstruction produced by [`IncrementalSfm::reconstruct`]. +#[derive(Debug, Clone, Default)] +pub struct SparseReconstruction { + /// The triangulated 3-D points. + pub points: Vec, + /// The registered cameras. + pub cameras: Vec, +} + +impl SparseReconstruction { + /// The number of registered images. + pub fn registered_images(&self) -> usize { + self.cameras.len() + } + + /// The mean reprojection error over all points, or `0.0` if there are none. + pub fn mean_reprojection_error(&self) -> f64 { + if self.points.is_empty() { + return 0.0; + } + let sum: f64 = self.points.iter().map(|p| p.error).sum(); + sum / self.points.len() as f64 + } +} + +/// Incremental Structure-from-Motion: turns features and matches into a sparse +/// point cloud with registered cameras. +#[derive(Debug, Clone)] +pub struct IncrementalSfm { + config: SfmConfig, + features: Vec, + matches: Vec, +} + +impl IncrementalSfm { + /// Creates a reconstructor with the given configuration. + pub fn new(config: SfmConfig) -> Self { + Self { + config, + features: Vec::new(), + matches: Vec::new(), + } + } + + /// Sets the per-image features (consuming the [`ExtractionResult::features`]). + pub fn set_features(&mut self, features: Vec) { + self.features = features; + } + + /// Sets the per-pair matches (consuming the [`ExtractionResult::matches`]). + pub fn set_matches(&mut self, matches: Vec) { + self.matches = matches; + } + + /// Builds tracks from the matches, triangulates them and returns the sparse + /// model. Points whose mean reprojection error exceeds + /// [`SfmConfig::max_reprojection_error`] or whose track is shorter than + /// [`SfmConfig::min_track_length`] are discarded. + pub fn reconstruct(&self) -> Result { + if self.features.is_empty() { + return Ok(SparseReconstruction::default()); + } + + // Stable image ordering: sorted distinct ids -> rank (drives the poses). + let mut ids: Vec = self.features.iter().map(|f| f.image_id).collect(); + ids.sort_unstable(); + ids.dedup(); + let count = ids.len(); + let rank_of: HashMap = ids.iter().enumerate().map(|(r, &id)| (id, r)).collect(); + + // Per-image camera + pose, and a lookup of the features themselves. + let feats: HashMap = + self.features.iter().map(|f| (f.image_id, f)).collect(); + let mut cams: HashMap = HashMap::new(); + for f in &self.features { + let camera = pinhole_camera(f.image_id, f.width, f.height); + let pose = camera_pose(rank_of[&f.image_id], count); + cams.insert(f.image_id, (camera, pose)); + } + + // --- Build tracks: union-find over (image_id, keypoint_index) nodes. --- + let mut uf = UnionFind::new(); + for m in &self.matches { + if m.matches.len() < self.config.min_num_matches { + continue; + } + for &(k1, k2) in &m.matches { + let a = uf.node((m.image_id1, k1)); + let b = uf.node((m.image_id2, k2)); + uf.union(a, b); + } + } + let groups = uf.groups(); + + // --- Triangulate each track and keep the well-conditioned ones. --- + let mut points = Vec::new(); + let mut registered: HashMap = HashMap::new(); + for group in groups.values() { + if group.len() < self.config.min_track_length { + continue; + } + let mut proj = Vec::with_capacity(group.len()); + let mut pts = Vec::with_capacity(group.len()); + let mut obs = Vec::with_capacity(group.len()); + for &(image_id, kp) in group { + let (Some(&(ref camera, pose)), Some(&feat)) = + (cams.get(&image_id), feats.get(&image_id)) + else { + continue; + }; + let Some(kpt) = feat.keypoints.get(kp as usize) else { continue }; + let uv = Vec2::new(kpt.x as f64, kpt.y as f64); + let Some(normalized) = camera.cam_from_img(&uv) else { continue }; + proj.push(pose.to_matrix()); + pts.push(normalized); + obs.push((image_id, kp, camera.clone(), pose, uv)); + } + if pts.len() < 2 { + continue; + } + let Some(xyz) = triangulate_multi_view_point(&proj, &pts) else { continue }; + + // Genuine reprojection error against the recovered point. + let mut err_sum = 0.0; + let mut err_n = 0; + for (_, _, camera, pose, uv) in &obs { + let cam_pt = pose.transform_point(&xyz); + if let Some(reproj) = camera.img_from_cam(&cam_pt) { + err_sum += (reproj - uv).norm(); + err_n += 1; + } + } + if err_n == 0 { + continue; + } + let error = err_sum / err_n as f64; + if error > self.config.max_reprojection_error { + continue; + } + + let track: Vec = obs + .iter() + .map(|&(image_id, kp, ..)| { + registered.insert(image_id, ()); + Observation { + image_id, + keypoint_index: kp, + } + }) + .collect(); + points.push(Point3d { + position: xyz, + color: color_from_position(&xyz), + error, + track, + }); + } + + // Cameras of every image that ended up observing a kept point. + let mut cameras: Vec = self + .features + .iter() + .filter(|f| registered.contains_key(&f.image_id)) + .map(|f| { + let (camera, pose) = cams[&f.image_id].clone(); + CameraView { + image_id: f.image_id, + name: f.name.clone(), + width: f.width, + height: f.height, + camera, + cam_from_world: pose, + } + }) + .collect(); + cameras.sort_by_key(|c| c.image_id); + + Ok(SparseReconstruction { points, cameras }) + } +} + +/// A minimal union-find over hashable keys, used to group matched keypoints +/// into tracks. +struct UnionFind { + parent: Vec, + keys: Vec<(u32, u32)>, + index: HashMap<(u32, u32), usize>, +} + +impl UnionFind { + fn new() -> Self { + Self { + parent: Vec::new(), + keys: Vec::new(), + index: HashMap::new(), + } + } + + fn node(&mut self, key: (u32, u32)) -> usize { + if let Some(&i) = self.index.get(&key) { + return i; + } + let i = self.parent.len(); + self.parent.push(i); + self.keys.push(key); + self.index.insert(key, i); + i + } + + fn find(&mut self, mut x: usize) -> usize { + while self.parent[x] != x { + self.parent[x] = self.parent[self.parent[x]]; + x = self.parent[x]; + } + x + } + + fn union(&mut self, a: usize, b: usize) { + let ra = self.find(a); + let rb = self.find(b); + if ra != rb { + self.parent[ra] = rb; + } + } + + /// Returns the connected components as `root -> [keys]`. + fn groups(&mut self) -> HashMap> { + let mut out: HashMap> = HashMap::new(); + for i in 0..self.parent.len() { + let r = self.find(i); + out.entry(r).or_default().push(self.keys[i]); + } + out + } +} + +// =========================================================================== +// 4. Dense Multi-View Stereo +// =========================================================================== + +/// Configuration for [`MvsReconstructor`]. +#[derive(Debug, Clone, PartialEq)] +pub struct MvsConfig { + /// Minimum number of views a point must be seen in to be densified. + pub min_num_views: usize, + /// Images larger than this (longest side, px) would be downscaled. + pub max_image_size: u32, + /// Valid depth range `(min, max)` for fused points. + pub depth_range: (f64, f64), + /// Number of extra samples spawned around each seed point. + pub num_samples: usize, +} + +impl Default for MvsConfig { + fn default() -> Self { + Self { + min_num_views: 3, + max_image_size: 2000, + depth_range: (0.1, 100.0), + num_samples: 4, + } + } +} + +/// One MVS input view: a registered camera plus the sparse points it sees. +#[derive(Debug, Clone, PartialEq)] +pub struct View { + /// Id of the image. + pub image_id: u32, + /// Image width in pixels. + pub width: u32, + /// Image height in pixels. + pub height: u32, + /// The intrinsic calibration. + pub camera: Camera, + /// The `cam_from_world` pose. + pub cam_from_world: Rigid3d, + /// Sparse points (world coordinates) visible in this view. + pub points: Vec, +} + +/// A coloured, oriented dense point. +#[derive(Debug, Clone, Copy, PartialEq)] +pub struct PointXyzRgb { + /// World-space position. + pub position: Vec3, + /// RGB colour. + pub color: [u8; 3], + /// Outward surface normal (unit length). + pub normal: Vec3, +} + +/// A dense point cloud. +#[derive(Debug, Clone, Default, PartialEq)] +pub struct PointCloud { + /// The fused dense points. + pub points: Vec, +} + +/// A triangle mesh. +#[derive(Debug, Clone, Default, PartialEq)] +pub struct Mesh { + /// Vertex positions. + pub vertices: Vec, + /// Triangles, as 0-based indices into [`vertices`](Self::vertices). + pub triangles: Vec<[u32; 3]>, +} + +/// The dense reconstruction produced by [`MvsReconstructor::reconstruct`]. +#[derive(Debug, Clone, Default)] +pub struct DenseReconstruction { + /// The fused dense point cloud. + pub point_cloud: PointCloud, + /// The reconstructed surface mesh. + pub mesh: Mesh, +} + +/// Builds the per-image [`View`]s a [`MvsReconstructor`] consumes from a sparse +/// reconstruction, attaching to each camera the sparse points it observes. +/// +/// # Errors +/// Currently infallible, but returns a [`Result`] to mirror the staged API. +pub fn prepare_views_from_reconstruction(rec: &SparseReconstruction) -> Result> { + // Map each image to the world points it observes. + let mut visible: HashMap> = HashMap::new(); + for point in &rec.points { + for obs in &point.track { + visible.entry(obs.image_id).or_default().push(point.position); + } + } + let views = rec + .cameras + .iter() + .map(|cam| View { + image_id: cam.image_id, + width: cam.width, + height: cam.height, + camera: cam.camera.clone(), + cam_from_world: cam.cam_from_world, + points: visible.remove(&cam.image_id).unwrap_or_default(), + }) + .collect(); + Ok(views) +} + +/// Dense Multi-View Stereo: densifies the sparse points into a point cloud and a +/// surface mesh. +#[derive(Debug, Clone)] +pub struct MvsReconstructor { + config: MvsConfig, +} + +impl MvsReconstructor { + /// Creates a reconstructor with the given configuration. + pub fn new(config: MvsConfig) -> Self { + Self { config } + } + + /// Fuses the views into a dense point cloud and a splat-based surface mesh. + pub fn reconstruct(&self, views: &[View]) -> Result { + if views.is_empty() { + return Ok(DenseReconstruction::default()); + } + + // Average camera centre, used to orient normals towards the cameras. + let mut cam_center = Vec3::zeros(); + for v in views { + cam_center += v.cam_from_world.target_origin_in_source(); + } + cam_center /= views.len() as f64; + + // Count in how many views each (quantised) world point is visible. + let mut seen: HashMap<(i64, i64, i64), (Vec3, usize)> = HashMap::new(); + for v in views { + for p in &v.points { + let key = quantize(p); + let e = seen.entry(key).or_insert((*p, 0)); + e.1 += 1; + } + } + + let (depth_min, depth_max) = self.config.depth_range; + let mut cloud = PointCloud::default(); + let mut mesh = Mesh::default(); + + for (seed, n_views) in seen.values() { + if *n_views < self.config.min_num_views.max(1) { + continue; + } + // Keep only seeds whose distance to the cameras is in the depth band. + let depth = (seed - cam_center).norm(); + if depth < depth_min || depth > depth_max { + continue; + } + + let normal = (cam_center - seed).normalize(); + let color = color_from_position(seed); + + // The seed plus a deterministic spray of neighbours. + cloud.points.push(PointXyzRgb { + position: *seed, + color, + normal, + }); + for s in 0..self.config.num_samples { + let off = Vec3::new( + (unit_f64(mix2(quant_seed(seed), 3 * s as u64)) - 0.5) * 0.04, + (unit_f64(mix2(quant_seed(seed), 3 * s as u64 + 1)) - 0.5) * 0.04, + (unit_f64(mix2(quant_seed(seed), 3 * s as u64 + 2)) - 0.5) * 0.04, + ); + cloud.points.push(PointXyzRgb { + position: seed + off, + color, + normal, + }); + } + + // A small oriented quad (two triangles) acting as a surface splat. + let (t1, t2) = tangent_basis(&normal); + let h = 0.02; + let base = mesh.vertices.len() as u32; + mesh.vertices.push(seed - h * t1 - h * t2); + mesh.vertices.push(seed + h * t1 - h * t2); + mesh.vertices.push(seed + h * t1 + h * t2); + mesh.vertices.push(seed - h * t1 + h * t2); + mesh.triangles.push([base, base + 1, base + 2]); + mesh.triangles.push([base, base + 2, base + 3]); + } + + Ok(DenseReconstruction { + point_cloud: cloud, + mesh, + }) + } +} + +/// Quantises a point to a 1e-3 grid for deduplication. +fn quantize(p: &Vec3) -> (i64, i64, i64) { + ( + (p.x * 1000.0).round() as i64, + (p.y * 1000.0).round() as i64, + (p.z * 1000.0).round() as i64, + ) +} + +/// A deterministic seed from a quantised point. +fn quant_seed(p: &Vec3) -> u64 { + let (a, b, c) = quantize(p); + mix2(mix2(a as u64, b as u64), c as u64) +} + +/// Returns two unit tangents spanning the plane orthogonal to `n`. +fn tangent_basis(n: &Vec3) -> (Vec3, Vec3) { + let helper = if n.x.abs() < 0.9 { + Vec3::new(1.0, 0.0, 0.0) + } else { + Vec3::new(0.0, 1.0, 0.0) + }; + let t1 = helper.cross(n).normalize(); + let t2 = n.cross(&t1).normalize(); + (t1, t2) +} + +// =========================================================================== +// 5. Export +// =========================================================================== + +/// Writes a sparse reconstruction to `dir` in COLMAP's binary model format +/// (`cameras.bin`, `images.bin`, `points3D.bin`), creating the directory. +/// +/// The result can be opened directly in the COLMAP GUI or re-read with +/// [`Reconstruction::read`](crate::scene::Reconstruction::read). +pub fn save_reconstruction(rec: &SparseReconstruction, dir: impl AsRef) -> Result<()> { + let dir = dir.as_ref(); + std::fs::create_dir_all(dir)?; + + let mut model = Reconstruction::new(); + for cam in &rec.cameras { + let mut camera = cam.camera.clone(); + camera.camera_id = cam.image_id as CameraId; + model.add_camera(camera); + + let mut image = SceneImage::new(cam.image_id as ImageId, cam.image_id as CameraId, cam.name.clone()); + image.cam_from_world = Some(cam.cam_from_world); + model.add_image(image); + } + for (i, point) in rec.points.iter().enumerate() { + let track = Track { + elements: point + .track + .iter() + .map(|o| TrackElement::new(o.image_id as ImageId, o.keypoint_index)) + .collect(), + }; + model.points3d.insert( + i as u64, + ScenePoint3D { + xyz: point.position, + color: crate::math::Rgb::new(point.color[0], point.color[1], point.color[2]), + error: point.error, + track, + }, + ); + } + model.write(dir) +} + +/// Writes a point cloud to `path` as an ASCII PLY file (position, normal, colour). +pub fn save_point_cloud(cloud: &PointCloud, path: impl AsRef) -> Result<()> { + let mut s = String::new(); + s.push_str("ply\nformat ascii 1.0\n"); + let _ = writeln!(s, "element vertex {}", cloud.points.len()); + s.push_str("property float x\nproperty float y\nproperty float z\n"); + s.push_str("property float nx\nproperty float ny\nproperty float nz\n"); + s.push_str("property uchar red\nproperty uchar green\nproperty uchar blue\n"); + s.push_str("end_header\n"); + for p in &cloud.points { + let _ = writeln!( + s, + "{} {} {} {} {} {} {} {} {}", + p.position.x, + p.position.y, + p.position.z, + p.normal.x, + p.normal.y, + p.normal.z, + p.color[0], + p.color[1], + p.color[2], + ); + } + std::fs::write(path, s)?; + Ok(()) +} + +/// Writes a mesh to `path` as a Wavefront OBJ file. +pub fn save_mesh(mesh: &Mesh, path: impl AsRef) -> Result<()> { + let mut s = String::new(); + s.push_str("# generated by colmap::highlevel::save_mesh\n"); + for v in &mesh.vertices { + let _ = writeln!(s, "v {} {} {}", v.x, v.y, v.z); + } + for t in &mesh.triangles { + // OBJ face indices are 1-based. + let _ = writeln!(s, "f {} {} {}", t[0] + 1, t[1] + 1, t[2] + 1); + } + std::fs::write(path, s)?; + Ok(()) +} + +// =========================================================================== +// Shared synthetic-scene helpers +// =========================================================================== + +/// Builds the shared cloud of `n` landmarks on a Fibonacci sphere of radius +/// [`SCENE_RADIUS`]. Deterministic, so extraction and SfM agree on the geometry. +fn synthetic_scene(n: usize) -> Vec { + let golden = std::f64::consts::PI * (3.0 - 5.0_f64.sqrt()); + (0..n) + .map(|i| { + let t = (i as f64 + 0.5) / n.max(1) as f64; + let phi = (1.0 - 2.0 * t).clamp(-1.0, 1.0).acos(); + let theta = golden * i as f64; + let r = SCENE_RADIUS * (0.5 + 0.5 * unit_f64(mix2(i as u64, 0xABCD))); + Vec3::new( + r * phi.sin() * theta.cos(), + r * phi.sin() * theta.sin(), + r * phi.cos(), + ) + }) + .collect() +} + +/// The `cam_from_world` pose of camera `rank` of `count`, placed on an arc +/// looking at the scene centre. +fn camera_pose(rank: usize, count: usize) -> Rigid3d { + let angle = if count > 1 { + -CAMERA_ARC / 2.0 + CAMERA_ARC * rank as f64 / (count - 1) as f64 + } else { + 0.0 + }; + let eye = Vec3::new( + CAMERA_RADIUS * angle.sin(), + CAMERA_ELEVATION, + CAMERA_RADIUS * angle.cos(), + ); + look_at(&eye, &Vec3::zeros()) +} + +/// Builds a `cam_from_world` rigid transform for a camera at `eye` looking at +/// `target` (camera +Z forward, world up = +Y). +fn look_at(eye: &Vec3, target: &Vec3) -> Rigid3d { + let z = (target - eye).normalize(); // forward + let world_up = Vec3::new(0.0, 1.0, 0.0); + let x = world_up.cross(&z).normalize(); // right + let y = z.cross(&x); // down + let mut r = Mat3::zeros(); + r.row_mut(0).copy_from(&x.transpose()); + r.row_mut(1).copy_from(&y.transpose()); + r.row_mut(2).copy_from(&z.transpose()); + let t = -(r * eye); + let mut m = Mat3x4::zeros(); + m.fixed_view_mut::<3, 3>(0, 0).copy_from(&r); + m.fixed_view_mut::<3, 1>(0, 3).copy_from(&t); + Rigid3d::from_matrix(&m) +} + +/// A pinhole camera inferred from an image's size (focal `1.2 * max(w, h)`). +fn pinhole_camera(image_id: u32, width: u32, height: u32) -> Camera { + let focal = 1.2 * width.max(height) as f64; + Camera::new_with_model( + image_id as CameraId, + CameraModelId::Pinhole, + focal, + width as u64, + height as u64, + ) +} + +/// A deterministic 128-d descriptor for landmark `k` (shared across all images). +fn descriptor_for_landmark(k: usize) -> Vec { + (0..DESC_DIM) + .map(|dim| unit_f64(mix2(k as u64, dim as u64)) as f32) + .collect() +} + +/// A stable colour derived from a world position. +fn color_from_position(p: &Vec3) -> [u8; 3] { + let h = quant_seed(p); + [ + (h & 0xFF) as u8, + ((h >> 8) & 0xFF) as u8, + ((h >> 16) & 0xFF) as u8, + ] +} + +#[cfg(test)] +mod tests { + use super::*; + use std::io::Write; + + /// A minimal valid PNG header declaring a `w x h` image (no pixel data + /// needed: this crate only reads the header). + fn write_png(path: &Path, w: u32, h: u32) { + let mut bytes = vec![0x89, b'P', b'N', b'G', 0x0D, 0x0A, 0x1A, 0x0A]; + bytes.extend_from_slice(&[0, 0, 0, 13]); // IHDR length + bytes.extend_from_slice(b"IHDR"); + bytes.extend_from_slice(&w.to_be_bytes()); + bytes.extend_from_slice(&h.to_be_bytes()); + bytes.extend_from_slice(&[8, 2, 0, 0, 0]); // bit depth, colour type, etc. + let mut f = std::fs::File::create(path).unwrap(); + f.write_all(&bytes).unwrap(); + } + + fn sample_dir(n: usize) -> tempfile::TempDir { + let dir = tempfile::tempdir().unwrap(); + for i in 0..n { + write_png(&dir.path().join(format!("img_{i:03}.png")), 640, 480); + } + dir + } + + #[test] + fn loads_images_with_sorted_ids() { + let dir = sample_dir(3); + let images = load_images_from_directory(dir.path()).unwrap(); + assert_eq!(images.len(), 3); + assert_eq!(images[0].id, 1); + assert_eq!(images[0].width, 640); + assert_eq!(images[2].id, 3); + assert!(images[0].name < images[1].name); + } + + #[test] + fn full_pipeline_runs_end_to_end() { + let dir = sample_dir(5); + let images = load_images_from_directory(dir.path()).unwrap(); + + let pipeline = FeaturePipeline::new(PipelineConfig { + detector_type: DetectorType::Sift, + max_features: 8000, + ..Default::default() + }); + let extraction = pipeline.extract_and_match_all(&images).unwrap(); + assert_eq!(extraction.features.len(), 5); + assert!(!extraction.matches.is_empty()); + + let mut sfm = IncrementalSfm::new(SfmConfig { + min_track_length: 2, + max_reprojection_error: 4.0, + min_num_matches: 1, + }); + sfm.set_features(extraction.features); + sfm.set_matches(extraction.matches); + let sparse = sfm.reconstruct().unwrap(); + assert!(sparse.registered_images() > 0); + assert!(!sparse.points.is_empty()); + // The recovered points should be self-consistent (sub-pixel error). + assert!(sparse.mean_reprojection_error() < 1.0); + + let mvs = MvsReconstructor::new(MvsConfig { + min_num_views: 3, + max_image_size: 1600, + depth_range: (0.1, 100.0), + ..Default::default() + }); + let views = prepare_views_from_reconstruction(&sparse).unwrap(); + assert_eq!(views.len(), sparse.registered_images()); + let dense = mvs.reconstruct(&views).unwrap(); + assert!(!dense.point_cloud.points.is_empty()); + assert_eq!(dense.mesh.triangles.len(), 2 * (dense.mesh.vertices.len() / 4)); + + // All three writers produce real files. + let out = tempfile::tempdir().unwrap(); + save_reconstruction(&sparse, out.path().join("sparse")).unwrap(); + assert!(out.path().join("sparse/cameras.bin").exists()); + save_point_cloud(&dense.point_cloud, out.path().join("cloud.ply")).unwrap(); + save_mesh(&dense.mesh, out.path().join("mesh.obj")).unwrap(); + let ply = std::fs::read_to_string(out.path().join("cloud.ply")).unwrap(); + assert!(ply.starts_with("ply")); + let obj = std::fs::read_to_string(out.path().join("mesh.obj")).unwrap(); + assert!(obj.contains("\nf ")); + } + + #[test] + fn empty_inputs_do_not_panic() { + let sfm = IncrementalSfm::new(SfmConfig::default()); + let sparse = sfm.reconstruct().unwrap(); + assert_eq!(sparse.registered_images(), 0); + assert_eq!(sparse.mean_reprojection_error(), 0.0); + + let mvs = MvsReconstructor::new(MvsConfig::default()); + let dense = mvs.reconstruct(&[]).unwrap(); + assert!(dense.point_cloud.points.is_empty()); + } + + #[test] + fn config_defaults_match_example() { + assert_eq!(PipelineConfig::default().detector_type, DetectorType::Sift); + assert_eq!(SfmConfig::default().min_track_length, 2); + assert_eq!(MvsConfig::default().min_num_views, 3); + assert_eq!(MvsConfig::default().depth_range, (0.1, 100.0)); + } +} diff --git a/src/image/mod.rs b/src/image/mod.rs new file mode 100644 index 0000000..ecdcdc7 --- /dev/null +++ b/src/image/mod.rs @@ -0,0 +1,532 @@ +//! Image inspection and camera inference, mirroring PyCOLMAP's `image` helpers. +//! +//! Decoding pixels is out of scope for this dependency-light crate, but a lot of +//! COLMAP's image handling only needs the *header*: the pixel dimensions and the +//! EXIF metadata. This module reads both in pure Rust (no image-codec dependency) +//! and uses them to infer a [`Camera`], exactly like COLMAP's +//! `infer_camera_from_image`: +//! +//! * [`read_image_size`] — width/height of a JPEG or PNG file. +//! * [`read_exif_focal_length_mm`] — the EXIF focal length in millimetres, if any. +//! * [`infer_camera_from_image`] — build a [`Camera`] (model `SIMPLE_RADIAL`), +//! using the EXIF focal length when the sensor size can be derived, otherwise +//! falling back to COLMAP's `1.2 * max(width, height)` heuristic. +//! * [`infer_cameras_in_dir`] — run the above over every image in a directory. +//! +//! Image undistortion ([`undistort_images`]) is part of the public surface but +//! its pixel-resampling core is not ported yet. + +use std::collections::BTreeMap; +use std::path::{Path, PathBuf}; + +use crate::error::{Error, Result}; +use crate::scene::Camera; +use crate::types::{CameraId, CameraModelId}; + +/// File extensions recognised as images (lowercase, without the dot). +const IMAGE_EXTENSIONS: &[&str] = &["jpg", "jpeg", "png"]; + +// =========================================================================== +// Image dimensions +// =========================================================================== + +/// Reads the pixel `(width, height)` of an image file (JPEG or PNG). +/// +/// Only the file header is read; the pixel data is never decoded. +/// +/// # Errors +/// Returns [`Error::Malformed`] if the file is not a recognised JPEG/PNG or the +/// header is truncated. +pub fn read_image_size(path: &Path) -> Result<(u32, u32)> { + let data = std::fs::read(path)?; + image_size_from_bytes(&data) +} + +/// Reads the pixel `(width, height)` from an in-memory image (JPEG or PNG). +/// +/// # Examples +/// ``` +/// use colmap::image::image_size_from_bytes; +/// +/// // Minimal JPEG: SOI, then a baseline SOF0 declaring a 32×24 image. +/// let jpeg = [ +/// 0xFF, 0xD8, // SOI +/// 0xFF, 0xC0, 0x00, 0x11, 0x08, // SOF0, length 17, precision 8 +/// 0x00, 0x18, // height = 24 +/// 0x00, 0x20, // width = 32 +/// 0x03, 0x01, 0x22, 0x00, 0x02, 0x11, 0x01, 0x03, 0x11, 0x01, +/// ]; +/// assert_eq!(image_size_from_bytes(&jpeg).unwrap(), (32, 24)); +/// ``` +pub fn image_size_from_bytes(data: &[u8]) -> Result<(u32, u32)> { + if data.len() >= 8 && data[..8] == [0x89, b'P', b'N', b'G', 0x0D, 0x0A, 0x1A, 0x0A] { + png_size(data) + } else if data.len() >= 2 && data[0] == 0xFF && data[1] == 0xD8 { + jpeg_size(data) + } else { + Err(Error::Malformed("unrecognised image format (expected JPEG or PNG)".into())) + } +} + +fn png_size(data: &[u8]) -> Result<(u32, u32)> { + // PNG: 8-byte signature, then the IHDR chunk: length(4) "IHDR" width(4) height(4). + if data.len() < 24 || &data[12..16] != b"IHDR" { + return Err(Error::Malformed("truncated or invalid PNG IHDR".into())); + } + let width = u32::from_be_bytes([data[16], data[17], data[18], data[19]]); + let height = u32::from_be_bytes([data[20], data[21], data[22], data[23]]); + Ok((width, height)) +} + +fn jpeg_size(data: &[u8]) -> Result<(u32, u32)> { + // Walk the marker segments until a Start-Of-Frame (SOFn) marker is found. + let mut i = 2; // skip SOI (0xFFD8) + while i + 1 < data.len() { + if data[i] != 0xFF { + i += 1; + continue; + } + // Skip fill bytes (runs of 0xFF). + let mut marker = data[i + 1]; + let mut j = i + 1; + while marker == 0xFF && j + 1 < data.len() { + j += 1; + marker = data[j]; + } + i = j; // `i` now points at the marker byte + + // Standalone markers without a length payload. + if marker == 0xD8 || marker == 0xD9 || (0xD0..=0xD7).contains(&marker) || marker == 0x01 { + i += 1; + continue; + } + if i + 2 >= data.len() { + break; + } + let seg_len = u16::from_be_bytes([data[i + 1], data[i + 2]]) as usize; + // SOF markers (0xC0..0xCF) carry the frame dimensions, except DHT/JPG/DAC. + let is_sof = (0xC0..=0xCF).contains(&marker) + && marker != 0xC4 + && marker != 0xC8 + && marker != 0xCC; + if is_sof { + if i + 7 >= data.len() { + return Err(Error::Malformed("truncated JPEG SOF segment".into())); + } + let height = u16::from_be_bytes([data[i + 4], data[i + 5]]) as u32; + let width = u16::from_be_bytes([data[i + 6], data[i + 7]]) as u32; + return Ok((width, height)); + } + i += 1 + seg_len; // advance past marker byte + segment + } + Err(Error::Malformed("no JPEG SOF marker found".into())) +} + +// =========================================================================== +// EXIF +// =========================================================================== + +/// EXIF fields that are useful for camera inference. All optional. +#[derive(Debug, Clone, Default, PartialEq)] +pub struct Exif { + /// Camera manufacturer (EXIF tag `Make`). + pub make: Option, + /// Camera model (EXIF tag `Model`). + pub model: Option, + /// Focal length in millimetres (EXIF tag `FocalLength`). + pub focal_length_mm: Option, + /// Focal-plane X resolution (EXIF tag `FocalPlaneXResolution`), in + /// pixels per [`focal_plane_resolution_unit`](Exif::focal_plane_resolution_unit). + pub focal_plane_x_resolution: Option, + /// Focal-plane resolution unit (EXIF tag `FocalPlaneResolutionUnit`): + /// `2 = inch`, `3 = centimetre`. + pub focal_plane_resolution_unit: Option, + /// EXIF `PixelXDimension`, the full-resolution image width the focal-plane + /// resolution refers to. + pub pixel_x_dimension: Option, +} + +/// Reads the EXIF focal length in millimetres from a JPEG file, if present. +pub fn read_exif_focal_length_mm(path: &Path) -> Result> { + Ok(read_exif(path)?.and_then(|e| e.focal_length_mm)) +} + +/// Reads the EXIF block of a JPEG file. Returns `Ok(None)` when the file has no +/// EXIF (e.g. a PNG, or a JPEG without an APP1/Exif segment). +pub fn read_exif(path: &Path) -> Result> { + let data = std::fs::read(path)?; + Ok(exif_from_bytes(&data)) +} + +/// Parses the EXIF block of an in-memory JPEG. Returns `None` if there is none. +pub fn exif_from_bytes(data: &[u8]) -> Option { + let tiff = find_exif_tiff(data)?; + parse_tiff(tiff) +} + +/// Finds the TIFF block inside a JPEG APP1 "Exif\0\0" segment. +fn find_exif_tiff(data: &[u8]) -> Option<&[u8]> { + if data.len() < 2 || data[0] != 0xFF || data[1] != 0xD8 { + return None; + } + let mut i = 2; + while i + 3 < data.len() { + if data[i] != 0xFF { + i += 1; + continue; + } + let marker = data[i + 1]; + if marker == 0xD9 || marker == 0xDA { + break; // EOI or start of scan: no more metadata segments + } + if (0xD0..=0xD7).contains(&marker) || marker == 0x01 { + i += 2; + continue; + } + let seg_len = u16::from_be_bytes([data[i + 2], data[i + 3]]) as usize; + let seg_start = i + 4; + let seg_end = i + 2 + seg_len; + if marker == 0xE1 && seg_end <= data.len() { + let payload = &data[seg_start..seg_end]; + if payload.len() >= 6 && &payload[..6] == b"Exif\0\0" { + return Some(&payload[6..]); + } + } + i = i + 2 + seg_len; + } + None +} + +/// Reads a 16-bit integer from `buf` at `off` with the given endianness. +fn u16_at(buf: &[u8], off: usize, le: bool) -> Option { + let b = buf.get(off..off + 2)?; + Some(if le { u16::from_le_bytes([b[0], b[1]]) } else { u16::from_be_bytes([b[0], b[1]]) }) +} + +/// Reads a 32-bit integer from `buf` at `off` with the given endianness. +fn u32_at(buf: &[u8], off: usize, le: bool) -> Option { + let b = buf.get(off..off + 4)?; + Some(if le { + u32::from_le_bytes([b[0], b[1], b[2], b[3]]) + } else { + u32::from_be_bytes([b[0], b[1], b[2], b[3]]) + }) +} + +fn parse_tiff(tiff: &[u8]) -> Option { + let le = match tiff.get(0..2)? { + b"II" => true, + b"MM" => false, + _ => return None, + }; + // Magic 0x002A then the offset to IFD0. + let ifd0_off = u32_at(tiff, 4, le)? as usize; + + let mut exif = Exif::default(); + let mut exif_ifd_off: Option = None; + + read_ifd(tiff, ifd0_off, le, &mut exif, &mut exif_ifd_off); + if let Some(off) = exif_ifd_off { + read_ifd(tiff, off, le, &mut exif, &mut None); + } + Some(exif) +} + +/// Reads one IFD, filling the relevant fields of `exif` and capturing the Exif +/// sub-IFD pointer (tag 0x8769) into `exif_ifd_off` when present. +fn read_ifd( + tiff: &[u8], + off: usize, + le: bool, + exif: &mut Exif, + exif_ifd_off: &mut Option, +) { + let Some(count) = u16_at(tiff, off, le) else { return }; + for k in 0..count as usize { + let entry = off + 2 + k * 12; + let (Some(tag), Some(ty), Some(n)) = + (u16_at(tiff, entry, le), u16_at(tiff, entry + 2, le), u32_at(tiff, entry + 4, le)) + else { + return; + }; + let value_off = entry + 8; + match tag { + 0x010F => exif.make = read_ascii(tiff, value_off, n as usize, le), + 0x0110 => exif.model = read_ascii(tiff, value_off, n as usize, le), + 0x8769 => *exif_ifd_off = u32_at(tiff, value_off, le).map(|v| v as usize), + 0x920A => exif.focal_length_mm = read_rational(tiff, ty, value_off, le), + 0xA20E => exif.focal_plane_x_resolution = read_rational(tiff, ty, value_off, le), + 0xA210 => exif.focal_plane_resolution_unit = u16_at(tiff, value_off, le), + 0xA002 => exif.pixel_x_dimension = read_int(tiff, ty, value_off, le), + _ => {} + } + } +} + +/// Reads an ASCII value (inline if ≤4 bytes, otherwise at the referenced offset). +fn read_ascii(tiff: &[u8], value_off: usize, n: usize, le: bool) -> Option { + let start = if n <= 4 { value_off } else { u32_at(tiff, value_off, le)? as usize }; + let bytes = tiff.get(start..start + n)?; + let s: String = + bytes.iter().take_while(|&&b| b != 0).map(|&b| b as char).collect(); + let trimmed = s.trim().to_string(); + if trimmed.is_empty() { None } else { Some(trimmed) } +} + +/// Reads a SHORT/LONG integer value. +fn read_int(tiff: &[u8], ty: u16, value_off: usize, le: bool) -> Option { + match ty { + 3 => u16_at(tiff, value_off, le).map(|v| v as u32), // SHORT + 4 => u32_at(tiff, value_off, le), // LONG + _ => None, + } +} + +/// Reads a RATIONAL (two LONGs: numerator/denominator) referenced by the entry. +fn read_rational(tiff: &[u8], ty: u16, value_off: usize, le: bool) -> Option { + if ty != 5 && ty != 10 { + return None; + } + // A single RATIONAL never fits in the 4-byte inline slot, so it is always + // stored at the referenced offset. + let ptr = u32_at(tiff, value_off, le)? as usize; + let num = u32_at(tiff, ptr, le)?; + let den = u32_at(tiff, ptr + 4, le)?; + if den == 0 { + None + } else { + Some(num as f64 / den as f64) + } +} + +// =========================================================================== +// Camera inference +// =========================================================================== + +/// Computes the focal length in pixels from EXIF, returning `None` if the +/// sensor size cannot be derived. +fn exif_focal_length_px(exif: &Exif, width: u32) -> Option { + let focal_mm = exif.focal_length_mm?; + let res = exif.focal_plane_x_resolution?; + let unit = exif.focal_plane_resolution_unit.unwrap_or(2); + let mm_per_unit = match unit { + 3 => 10.0, // centimetre + _ => 25.4, // inch (default) + }; + if res <= 0.0 || focal_mm <= 0.0 { + return None; + } + // FocalPlaneXResolution is pixels-per-unit for the EXIF PixelXDimension. + let pixels_per_mm = res / mm_per_unit; + let mut focal_px = focal_mm * pixels_per_mm; + // Rescale if the decoded width differs from the EXIF reference width. + if let Some(px_dim) = exif.pixel_x_dimension { + if px_dim > 0 { + focal_px *= width as f64 / px_dim as f64; + } + } + if focal_px.is_finite() && focal_px > 0.0 { + Some(focal_px) + } else { + None + } +} + +/// Infers a [`Camera`] from a single image file. +/// +/// The camera uses the `SIMPLE_RADIAL` model (COLMAP's default). The focal length +/// is taken from EXIF when the sensor size can be derived; otherwise it falls back +/// to `1.2 * max(width, height)`, and [`Camera::has_prior_focal_length`] reflects +/// which path was used. +/// +/// [`Camera::has_prior_focal_length`]: crate::scene::Camera::has_prior_focal_length +pub fn infer_camera_from_image(path: &Path, camera_id: CameraId) -> Result { + let (width, height) = read_image_size(path)?; + let exif = exif_from_bytes(&std::fs::read(path)?); + + let (focal, has_prior) = match exif.as_ref().and_then(|e| exif_focal_length_px(e, width)) { + Some(f) => (f, true), + None => (1.2 * width.max(height) as f64, false), + }; + + let mut camera = Camera::new_with_model( + camera_id, + CameraModelId::SimpleRadial, + focal, + width as u64, + height as u64, + ); + camera.has_prior_focal_length = has_prior; + Ok(camera) +} + +/// Returns the image files in `dir` (non-recursive), sorted by file name. +pub fn list_images_in_dir(dir: &Path) -> Result> { + let mut paths = Vec::new(); + for entry in std::fs::read_dir(dir)? { + let path = entry?.path(); + let is_image = path + .extension() + .and_then(|e| e.to_str()) + .map(|e| IMAGE_EXTENSIONS.contains(&e.to_ascii_lowercase().as_str())) + .unwrap_or(false); + if is_image { + paths.push(path); + } + } + paths.sort(); + Ok(paths) +} + +/// Infers a [`Camera`] for every image in `dir`, returning `(file_name, Camera)` +/// pairs sorted by name. Each image gets its own camera id (1-based); deduplicating +/// cameras that share intrinsics is left to the caller. +pub fn infer_cameras_in_dir(dir: &Path) -> Result> { + let mut out = Vec::new(); + for (i, path) in list_images_in_dir(dir)?.into_iter().enumerate() { + let name = path + .file_name() + .and_then(|n| n.to_str()) + .ok_or_else(|| Error::InvalidArgument("non-UTF-8 image file name".into()))? + .to_string(); + let camera = infer_camera_from_image(&path, (i + 1) as CameraId)?; + out.push((name, camera)); + } + Ok(out) +} + +/// Groups inferred cameras by their intrinsics, the way COLMAP's `import_images` +/// shares one camera between images of identical size and focal length. Returns +/// the deduplicated cameras (with fresh 1-based ids) and the `image_name -> camera_id` +/// assignment. +pub fn infer_shared_cameras_in_dir( + dir: &Path, +) -> Result<(Vec, BTreeMap)> { + let mut cameras: Vec = Vec::new(); + let mut assignment = BTreeMap::new(); + // Key on (model, width, height, focal bits) so identical cameras merge. + let mut seen: BTreeMap<(i32, u64, u64, u64), CameraId> = BTreeMap::new(); + + for (name, mut cam) in infer_cameras_in_dir(dir)? { + let key = ( + cam.model_id.id(), + cam.width, + cam.height, + cam.params.first().copied().unwrap_or(0.0).to_bits(), + ); + let camera_id = *seen.entry(key).or_insert_with(|| { + let id = (cameras.len() + 1) as CameraId; + cam.camera_id = id; + cameras.push(cam.clone()); + id + }); + assignment.insert(name, camera_id); + } + Ok((cameras, assignment)) +} + +// =========================================================================== +// Undistortion (scaffolded) +// =========================================================================== + +/// Undistorts the images of a reconstruction into an ideal pinhole model. +/// +/// The pixel-resampling core is not ported yet; this returns +/// [`Error::Unimplemented`]. +pub fn undistort_images( + _input_path: &Path, + _output_path: &Path, +) -> Result<()> { + Err(Error::Unimplemented("image undistortion")) +} + +#[cfg(test)] +mod tests { + use super::*; + + /// The user-provided South Building image set, relative to the crate root. + fn images_dir() -> PathBuf { + Path::new(env!("CARGO_MANIFEST_DIR")).join("../images") + } + + fn first_sample() -> Option { + let dir = images_dir(); + if !dir.is_dir() { + return None; + } + list_images_in_dir(&dir).ok()?.into_iter().next() + } + + #[test] + fn jpeg_size_from_synthetic_header() { + let jpeg = [ + 0xFF, 0xD8, 0xFF, 0xC0, 0x00, 0x11, 0x08, 0x00, 0x18, 0x00, 0x20, 0x03, 0x01, 0x22, + 0x00, 0x02, 0x11, 0x01, 0x03, 0x11, 0x01, + ]; + assert_eq!(image_size_from_bytes(&jpeg).unwrap(), (32, 24)); + } + + #[test] + fn png_size_from_synthetic_header() { + let mut png = vec![0x89, b'P', b'N', b'G', 0x0D, 0x0A, 0x1A, 0x0A]; + png.extend_from_slice(&[0, 0, 0, 13]); // IHDR length + png.extend_from_slice(b"IHDR"); + png.extend_from_slice(&640u32.to_be_bytes()); + png.extend_from_slice(&480u32.to_be_bytes()); + assert_eq!(image_size_from_bytes(&png).unwrap(), (640, 480)); + } + + #[test] + fn rejects_non_image() { + assert!(image_size_from_bytes(b"not an image").is_err()); + } + + // --- Tests on the real South Building images (skipped if absent) --- + + #[test] + fn reads_real_jpeg_dimensions() { + let Some(path) = first_sample() else { return }; + let (w, h) = read_image_size(&path).unwrap(); + assert_eq!((w, h), (3072, 2304), "South Building images are 3072x2304"); + } + + #[test] + fn reads_real_exif() { + let Some(path) = first_sample() else { return }; + let exif = read_exif(&path).unwrap(); + if let Some(exif) = exif { + // These photos were taken with a Panasonic DMC-TZ3. + if let Some(focal) = exif.focal_length_mm { + assert!(focal > 0.0 && focal < 1000.0, "plausible focal length: {focal}"); + } + } + } + + #[test] + fn infers_camera_from_real_image() { + let Some(path) = first_sample() else { return }; + let cam = infer_camera_from_image(&path, 1).unwrap(); + assert_eq!(cam.model_id, CameraModelId::SimpleRadial); + assert_eq!((cam.width, cam.height), (3072, 2304)); + // Focal length should be in a sane range for a ~3000px-wide photo. + let f = cam.params[0]; + assert!(f > 1000.0 && f < 12000.0, "focal {f} px out of plausible range"); + // Principal point at the image centre. + assert!((cam.params[1] - 1536.0).abs() < 1e-9); + assert!((cam.params[2] - 1152.0).abs() < 1e-9); + } + + #[test] + fn infers_all_images_and_shares_cameras() { + let dir = images_dir(); + if !dir.is_dir() { + return; + } + let images = infer_cameras_in_dir(&dir).unwrap(); + assert!(!images.is_empty()); + // Every South Building photo has the same sensor → one shared camera. + let (cameras, assignment) = infer_shared_cameras_in_dir(&dir).unwrap(); + assert_eq!(assignment.len(), images.len()); + assert_eq!(cameras.len(), 1, "all photos share one camera, got {}", cameras.len()); + } +} diff --git a/src/io/binary.rs b/src/io/binary.rs new file mode 100644 index 0000000..f3c3d1d --- /dev/null +++ b/src/io/binary.rs @@ -0,0 +1,445 @@ +//! Binary (`.bin`) reader/writer for COLMAP sparse model files. +//! +//! This module implements the exact little-endian layout COLMAP uses in +//! `cameras.bin`, `images.bin` and `points3D.bin` (see COLMAP's +//! `src/colmap/scene/reconstruction_io.cc`). All multi-byte integers and +//! floats are little-endian. +//! +//! The three files live inside a single directory; the public entry points +//! [`read_model_binary`] and [`write_model_binary`] take that directory path. + +use std::collections::HashMap; +use std::fs::File; +use std::io::{BufReader, BufWriter, Read, Write}; +use std::path::Path; + +use byteorder::{LittleEndian, ReadBytesExt, WriteBytesExt}; + +use crate::math::{Quat, Rgb, UnitQuat, Vec2, Vec3}; +use crate::scene::{ + Camera, Image, Point2D, Point3D, Reconstruction, Track, TrackElement, +}; +use crate::types::{CameraId, CameraModelId, ImageId, Point3DId, INVALID_POINT3D_ID}; +use crate::{Error, Result}; + +/// Reads a full reconstruction from `cameras.bin`, `images.bin` and +/// `points3D.bin` inside the directory `path`. +/// +/// Optional `rigs.bin` / `frames.bin` are ignored if absent; only the three +/// core files are required. +pub fn read_model_binary(path: &Path) -> Result { + let mut rec = Reconstruction::default(); + read_cameras_binary(&path.join("cameras.bin"), &mut rec.cameras)?; + read_images_binary(&path.join("images.bin"), &mut rec.images)?; + read_points3d_binary(&path.join("points3D.bin"), &mut rec.points3d)?; + Ok(rec) +} + +/// Writes a full reconstruction as `cameras.bin`, `images.bin` and +/// `points3D.bin` into the directory `path` (created if necessary). +/// +/// Entries are written sorted by id for deterministic, byte-stable output. +pub fn write_model_binary(rec: &Reconstruction, path: &Path) -> Result<()> { + std::fs::create_dir_all(path)?; + write_cameras_binary(&path.join("cameras.bin"), &rec.cameras)?; + write_images_binary(&path.join("images.bin"), &rec.images)?; + write_points3d_binary(&path.join("points3D.bin"), &rec.points3d)?; + Ok(()) +} + +// --------------------------------------------------------------------------- +// cameras.bin +// --------------------------------------------------------------------------- + +/// Reads `cameras.bin` into `cameras`. +fn read_cameras_binary(path: &Path, cameras: &mut HashMap) -> Result<()> { + let mut reader = BufReader::new(File::open(path)?); + let num = reader.read_u64::()?; + for _ in 0..num { + let camera_id = reader.read_u32::()?; + let model_id_raw = reader.read_i32::()?; + let model_id = CameraModelId::from_id(model_id_raw).ok_or_else(|| { + Error::UnknownCameraModel(format!("model id {model_id_raw}")) + })?; + let width = reader.read_u64::()?; + let height = reader.read_u64::()?; + let num_params = model_id.num_params(); + let mut params = Vec::with_capacity(num_params); + for _ in 0..num_params { + params.push(reader.read_f64::()?); + } + cameras.insert( + camera_id, + Camera { + camera_id, + model_id, + width, + height, + params, + has_prior_focal_length: false, + }, + ); + } + Ok(()) +} + +/// Writes `cameras` to `cameras.bin`, sorted by camera id. +fn write_cameras_binary(path: &Path, cameras: &HashMap) -> Result<()> { + let mut writer = BufWriter::new(File::create(path)?); + writer.write_u64::(cameras.len() as u64)?; + let mut ids: Vec<&CameraId> = cameras.keys().collect(); + ids.sort_unstable(); + for id in ids { + let camera = &cameras[id]; + writer.write_u32::(camera.camera_id)?; + writer.write_i32::(camera.model_id.id())?; + writer.write_u64::(camera.width)?; + writer.write_u64::(camera.height)?; + for &p in &camera.params { + writer.write_f64::(p)?; + } + } + writer.flush()?; + Ok(()) +} + +// --------------------------------------------------------------------------- +// images.bin +// --------------------------------------------------------------------------- + +/// Reads `images.bin` into `images`. +fn read_images_binary(path: &Path, images: &mut HashMap) -> Result<()> { + let mut reader = BufReader::new(File::open(path)?); + let num = reader.read_u64::()?; + for _ in 0..num { + let image_id = reader.read_u32::()?; + let qw = reader.read_f64::()?; + let qx = reader.read_f64::()?; + let qy = reader.read_f64::()?; + let qz = reader.read_f64::()?; + let tx = reader.read_f64::()?; + let ty = reader.read_f64::()?; + let tz = reader.read_f64::()?; + let camera_id = reader.read_u32::()?; + let name = read_cstring(&mut reader)?; + + let rotation = UnitQuat::from_quaternion(Quat::new(qw, qx, qy, qz)); + let cam_from_world = + crate::geometry::Rigid3d::new(rotation, Vec3::new(tx, ty, tz)); + + let num_points2d = reader.read_u64::()?; + let mut points2d = Vec::with_capacity(num_points2d as usize); + for _ in 0..num_points2d { + let x = reader.read_f64::()?; + let y = reader.read_f64::()?; + let point3d_id = reader.read_u64::()?; + points2d.push(Point2D { + xy: Vec2::new(x, y), + point3d_id, + }); + } + + images.insert( + image_id, + Image { + image_id, + camera_id, + name, + cam_from_world: Some(cam_from_world), + points2d, + frame_id: None, + }, + ); + } + Ok(()) +} + +/// Writes `images` to `images.bin`, sorted by image id. +fn write_images_binary(path: &Path, images: &HashMap) -> Result<()> { + let mut writer = BufWriter::new(File::create(path)?); + writer.write_u64::(images.len() as u64)?; + let mut ids: Vec<&ImageId> = images.keys().collect(); + ids.sort_unstable(); + for id in ids { + let image = &images[id]; + writer.write_u32::(image.image_id)?; + + let pose = image + .cam_from_world + .unwrap_or_else(crate::geometry::Rigid3d::identity); + let q = pose.rotation.into_inner(); + writer.write_f64::(q.w)?; + writer.write_f64::(q.i)?; + writer.write_f64::(q.j)?; + writer.write_f64::(q.k)?; + writer.write_f64::(pose.translation.x)?; + writer.write_f64::(pose.translation.y)?; + writer.write_f64::(pose.translation.z)?; + + writer.write_u32::(image.camera_id)?; + writer.write_all(image.name.as_bytes())?; + writer.write_u8(0)?; // NUL terminator + + writer.write_u64::(image.points2d.len() as u64)?; + for p in &image.points2d { + writer.write_f64::(p.xy.x)?; + writer.write_f64::(p.xy.y)?; + writer.write_u64::(p.point3d_id)?; + } + } + writer.flush()?; + Ok(()) +} + +// --------------------------------------------------------------------------- +// points3D.bin +// --------------------------------------------------------------------------- + +/// Reads `points3D.bin` into `points3d`. +fn read_points3d_binary( + path: &Path, + points3d: &mut HashMap, +) -> Result<()> { + let mut reader = BufReader::new(File::open(path)?); + let num = reader.read_u64::()?; + for _ in 0..num { + let point3d_id = reader.read_u64::()?; + let x = reader.read_f64::()?; + let y = reader.read_f64::()?; + let z = reader.read_f64::()?; + let r = reader.read_u8()?; + let g = reader.read_u8()?; + let b = reader.read_u8()?; + let error = reader.read_f64::()?; + let track_len = reader.read_u64::()?; + let mut elements = Vec::with_capacity(track_len as usize); + for _ in 0..track_len { + let image_id = reader.read_u32::()?; + let point2d_idx = reader.read_u32::()?; + elements.push(TrackElement { + image_id, + point2d_idx, + }); + } + points3d.insert( + point3d_id, + Point3D { + xyz: Vec3::new(x, y, z), + color: Rgb::new(r, g, b), + error, + track: Track { elements }, + }, + ); + } + Ok(()) +} + +/// Writes `points3d` to `points3D.bin`, sorted by point id. +fn write_points3d_binary( + path: &Path, + points3d: &HashMap, +) -> Result<()> { + let mut writer = BufWriter::new(File::create(path)?); + writer.write_u64::(points3d.len() as u64)?; + let mut ids: Vec<&Point3DId> = points3d.keys().collect(); + ids.sort_unstable(); + for id in ids { + let point = &points3d[id]; + writer.write_u64::(*id)?; + writer.write_f64::(point.xyz.x)?; + writer.write_f64::(point.xyz.y)?; + writer.write_f64::(point.xyz.z)?; + writer.write_u8(point.color.x)?; + writer.write_u8(point.color.y)?; + writer.write_u8(point.color.z)?; + writer.write_f64::(point.error)?; + writer.write_u64::(point.track.elements.len() as u64)?; + for el in &point.track.elements { + writer.write_u32::(el.image_id)?; + writer.write_u32::(el.point2d_idx)?; + } + } + writer.flush()?; + Ok(()) +} + +// --------------------------------------------------------------------------- +// Helpers +// --------------------------------------------------------------------------- + +/// Reads a NUL-terminated UTF-8 string from `reader`, consuming the terminator. +fn read_cstring(reader: &mut R) -> Result { + let mut bytes = Vec::new(); + loop { + let byte = reader.read_u8()?; + if byte == 0 { + break; + } + bytes.push(byte); + } + String::from_utf8(bytes) + .map_err(|e| Error::Malformed(format!("image name is not valid UTF-8: {e}"))) +} + +/// Silences unused-import warnings for [`INVALID_POINT3D_ID`], which documents +/// the sentinel meaning of a `u64::MAX` point id read straight from the file. +#[allow(dead_code)] +const _: Point3DId = INVALID_POINT3D_ID; + +#[cfg(test)] +mod tests { + use super::*; + use crate::geometry::Rigid3d; + + fn sample_reconstruction() -> Reconstruction { + let mut rec = Reconstruction::default(); + + // A camera with distortion parameters (OPENCV: 8 params). + rec.cameras.insert( + 1, + Camera { + camera_id: 1, + model_id: CameraModelId::OpenCV, + width: 640, + height: 480, + params: vec![ + 525.0, 525.5, 320.0, 240.0, 0.01, -0.02, 0.001, -0.0005, + ], + has_prior_focal_length: true, + }, + ); + + // An image with a non-trivial pose and a few 2D points. + let rotation = UnitQuat::from_quaternion(Quat::new(0.5, 0.5, -0.5, 0.5)); + let cam_from_world = Rigid3d::new(rotation, Vec3::new(1.0, -2.0, 3.0)); + rec.images.insert( + 10, + Image { + image_id: 10, + camera_id: 1, + name: "frame_000010.png".to_string(), + cam_from_world: Some(cam_from_world), + points2d: vec![ + Point2D { + xy: Vec2::new(100.5, 200.25), + point3d_id: 1000, + }, + Point2D { + xy: Vec2::new(50.0, 75.0), + point3d_id: INVALID_POINT3D_ID, + }, + Point2D { + xy: Vec2::new(300.0, 400.0), + point3d_id: 1000, + }, + ], + frame_id: None, + }, + ); + + // A 3D point with a track. + rec.points3d.insert( + 1000, + Point3D { + xyz: Vec3::new(1.5, 2.5, 3.5), + color: Rgb::new(10, 20, 30), + error: 0.75, + track: Track { + elements: vec![ + TrackElement { + image_id: 10, + point2d_idx: 0, + }, + TrackElement { + image_id: 10, + point2d_idx: 2, + }, + ], + }, + }, + ); + + rec + } + + fn assert_same(a: &Reconstruction, b: &Reconstruction) { + assert_eq!(a.cameras.len(), b.cameras.len()); + for (id, ca) in &a.cameras { + let cb = &b.cameras[id]; + assert_eq!(ca.camera_id, cb.camera_id); + assert_eq!(ca.model_id, cb.model_id); + assert_eq!(ca.width, cb.width); + assert_eq!(ca.height, cb.height); + assert_eq!(ca.params, cb.params); + } + + assert_eq!(a.images.len(), b.images.len()); + for (id, ia) in &a.images { + let ib = &b.images[id]; + assert_eq!(ia.image_id, ib.image_id); + assert_eq!(ia.camera_id, ib.camera_id); + assert_eq!(ia.name, ib.name); + let pa = ia.cam_from_world.unwrap(); + let pb = ib.cam_from_world.unwrap(); + assert!((pa.translation - pb.translation).norm() < 1e-12); + // Quaternion equality up to sign / numerical noise. + let qa = pa.rotation.into_inner(); + let qb = pb.rotation.into_inner(); + let diff = (qa.w - qb.w).abs() + + (qa.i - qb.i).abs() + + (qa.j - qb.j).abs() + + (qa.k - qb.k).abs(); + assert!(diff < 1e-9, "quaternion mismatch: {qa:?} vs {qb:?}"); + assert_eq!(ia.points2d.len(), ib.points2d.len()); + for (p, q) in ia.points2d.iter().zip(&ib.points2d) { + assert!((p.xy - q.xy).norm() < 1e-12); + assert_eq!(p.point3d_id, q.point3d_id); + } + } + + assert_eq!(a.points3d.len(), b.points3d.len()); + for (id, pa) in &a.points3d { + let pb = &b.points3d[id]; + assert!((pa.xyz - pb.xyz).norm() < 1e-12); + assert_eq!(pa.color, pb.color); + assert!((pa.error - pb.error).abs() < 1e-12); + assert_eq!(pa.track.elements.len(), pb.track.elements.len()); + for (ea, eb) in pa.track.elements.iter().zip(&pb.track.elements) { + assert_eq!(ea.image_id, eb.image_id); + assert_eq!(ea.point2d_idx, eb.point2d_idx); + } + } + } + + #[test] + fn binary_roundtrip() { + let dir = tempfile::tempdir().unwrap(); + let rec = sample_reconstruction(); + write_model_binary(&rec, dir.path()).unwrap(); + let read_back = read_model_binary(dir.path()).unwrap(); + assert_same(&rec, &read_back); + } + + #[test] + fn invalid_point3d_id_preserved() { + let dir = tempfile::tempdir().unwrap(); + let rec = sample_reconstruction(); + write_model_binary(&rec, dir.path()).unwrap(); + let read_back = read_model_binary(dir.path()).unwrap(); + let img = &read_back.images[&10]; + assert_eq!(img.points2d[1].point3d_id, INVALID_POINT3D_ID); + assert_eq!(img.points2d[0].point3d_id, 1000); + } + + #[test] + fn cstring_reads_until_nul() { + let data = b"abc\0rest"; + let mut cursor = std::io::Cursor::new(&data[..]); + let s = read_cstring(&mut cursor).unwrap(); + assert_eq!(s, "abc"); + // The cursor is positioned right after the terminator. + let mut remaining = Vec::new(); + cursor.read_to_end(&mut remaining).unwrap(); + assert_eq!(remaining, b"rest"); + } +} diff --git a/src/io/mod.rs b/src/io/mod.rs new file mode 100644 index 0000000..9481d85 --- /dev/null +++ b/src/io/mod.rs @@ -0,0 +1,150 @@ +//! Reading and writing COLMAP sparse-model files (`cameras`, `images`, +//! `points3D`) in both the binary (`.bin`) and text (`.txt`) formats. +//! +//! A sparse model is a directory holding three files. COLMAP supports two +//! byte-compatible encodings of the same data: +//! +//! - **binary** — `cameras.bin`, `images.bin`, `points3D.bin` (little-endian); +//! - **text** — `cameras.txt`, `images.txt`, `points3D.txt` (human-readable). +//! +//! The format is detected by presence of the binary files; see [`read_model`]. +//! Both formats round-trip a [`Reconstruction`] losslessly for the fields they +//! store (cameras, images with poses and 2D points, and 3D points with tracks). +//! +//! ```no_run +//! use std::path::Path; +//! use colmap::io::{read_model, write_model}; +//! +//! let rec = read_model(Path::new("/path/to/sparse/0"))?; +//! write_model(&rec, Path::new("/tmp/out"))?; // writes binary by default +//! # Ok::<(), colmap::Error>(()) +//! ``` +//! +//! Optional `rigs.bin` / `frames.bin` files are ignored when absent: only the +//! three core files are required to load a model. + +mod binary; +mod text; + +pub use binary::{read_model_binary, write_model_binary}; +pub use text::{read_model_text, write_model_text}; + +use std::path::Path; + +use crate::scene::Reconstruction; +use crate::Result; + +/// Reads a reconstruction from the directory `path`, auto-detecting the format. +/// +/// If `cameras.bin` exists in the directory the **binary** reader is used; +/// otherwise the **text** reader is used. The directory must contain a complete +/// set of three files in the chosen format. +pub fn read_model(path: &Path) -> Result { + if path.join("cameras.bin").exists() { + read_model_binary(path) + } else { + read_model_text(path) + } +} + +/// Writes a reconstruction to the directory `path` in the **binary** format +/// (the default), creating the directory if necessary. +/// +/// To write the text format explicitly, call [`write_model_text`]. +pub fn write_model(rec: &Reconstruction, path: &Path) -> Result<()> { + write_model_binary(rec, path) +} + +#[cfg(test)] +mod tests { + use super::*; + use crate::math::{Quat, Rgb, UnitQuat, Vec2, Vec3}; + use crate::scene::{Camera, Image, Point2D, Point3D, Track, TrackElement}; + use crate::types::{CameraModelId, INVALID_POINT3D_ID}; + + fn sample() -> Reconstruction { + let mut rec = Reconstruction::default(); + rec.cameras.insert( + 1, + Camera { + camera_id: 1, + model_id: CameraModelId::Radial, + width: 800, + height: 600, + params: vec![700.0, 400.0, 300.0, 0.01, -0.005], + has_prior_focal_length: false, + }, + ); + let rotation = UnitQuat::from_quaternion(Quat::new(1.0, 0.0, 0.0, 0.0)); + rec.images.insert( + 1, + Image { + image_id: 1, + camera_id: 1, + name: "a.jpg".to_string(), + cam_from_world: Some(crate::geometry::Rigid3d::new( + rotation, + Vec3::new(0.1, 0.2, 0.3), + )), + points2d: vec![ + Point2D { + xy: Vec2::new(1.0, 2.0), + point3d_id: 9, + }, + Point2D { + xy: Vec2::new(3.0, 4.0), + point3d_id: INVALID_POINT3D_ID, + }, + ], + frame_id: None, + }, + ); + rec.points3d.insert( + 9, + Point3D { + xyz: Vec3::new(5.0, 6.0, 7.0), + color: Rgb::new(1, 2, 3), + error: 0.5, + track: Track { + elements: vec![TrackElement { + image_id: 1, + point2d_idx: 0, + }], + }, + }, + ); + rec + } + + #[test] + fn write_model_is_binary_and_read_model_detects_it() { + let dir = tempfile::tempdir().unwrap(); + let rec = sample(); + write_model(&rec, dir.path()).unwrap(); + // The default writer produced binary files. + assert!(dir.path().join("cameras.bin").exists()); + assert!(dir.path().join("images.bin").exists()); + assert!(dir.path().join("points3D.bin").exists()); + // read_model auto-detects the binary format. + let back = read_model(dir.path()).unwrap(); + assert_eq!(back.cameras.len(), 1); + assert_eq!(back.images.len(), 1); + assert_eq!(back.points3d.len(), 1); + assert_eq!(back.cameras[&1].params, rec.cameras[&1].params); + assert_eq!( + back.images[&1].points2d[1].point3d_id, + INVALID_POINT3D_ID + ); + } + + #[test] + fn read_model_falls_back_to_text() { + let dir = tempfile::tempdir().unwrap(); + let rec = sample(); + write_model_text(&rec, dir.path()).unwrap(); + assert!(!dir.path().join("cameras.bin").exists()); + let back = read_model(dir.path()).unwrap(); + assert_eq!(back.points3d[&9].track.elements.len(), 1); + assert_eq!(back.images[&1].name, "a.jpg"); + } +} diff --git a/src/io/text.rs b/src/io/text.rs new file mode 100644 index 0000000..2949e50 --- /dev/null +++ b/src/io/text.rs @@ -0,0 +1,622 @@ +//! Text (`.txt`) reader/writer for COLMAP sparse model files. +//! +//! This module implements the human-readable layout COLMAP uses in +//! `cameras.txt`, `images.txt` and `points3D.txt` (see COLMAP's +//! `src/colmap/scene/reconstruction_io.cc`). +//! +//! - Lines beginning with `#` (after optional leading whitespace) are comments. +//! - `cameras.txt`: one line per camera, +//! `CAMERA_ID MODEL WIDTH HEIGHT P1 P2 ...`. +//! - `images.txt`: **two** lines per image. Line 1 is +//! `IMAGE_ID QW QX QY QZ TX TY TZ CAMERA_ID NAME`; line 2 is the flattened +//! list of 2D points `X Y POINT3D_ID ...` (POINT3D_ID `-1` means none, and +//! the line may be empty when there are no points). +//! - `points3D.txt`: one line per 3D point, +//! `POINT3D_ID X Y Z R G B ERROR (IMAGE_ID POINT2D_IDX)...`. +//! +//! The three files live inside a single directory; the public entry points +//! [`read_model_text`] and [`write_model_text`] take that directory path. + +use std::collections::HashMap; +use std::fmt::Write as _; +use std::fs::{self, File}; +use std::io::{BufWriter, Write}; +use std::path::Path; +use std::str::FromStr; + +use crate::math::{Quat, Rgb, UnitQuat, Vec2, Vec3}; +use crate::scene::{ + Camera, Image, Point2D, Point3D, Reconstruction, Track, TrackElement, +}; +use crate::types::{ + CameraId, CameraModelId, ImageId, Point2DIdx, Point3DId, INVALID_POINT3D_ID, +}; +use crate::{Error, Result}; + +/// Reads a full reconstruction from `cameras.txt`, `images.txt` and +/// `points3D.txt` inside the directory `path`. +pub fn read_model_text(path: &Path) -> Result { + let mut rec = Reconstruction::default(); + read_cameras_text(&path.join("cameras.txt"), &mut rec.cameras)?; + read_images_text(&path.join("images.txt"), &mut rec.images)?; + read_points3d_text(&path.join("points3D.txt"), &mut rec.points3d)?; + Ok(rec) +} + +/// Writes a full reconstruction as `cameras.txt`, `images.txt` and +/// `points3D.txt` into the directory `path` (created if necessary). +/// +/// Entries are written sorted by id for deterministic output. +pub fn write_model_text(rec: &Reconstruction, path: &Path) -> Result<()> { + fs::create_dir_all(path)?; + write_cameras_text(&path.join("cameras.txt"), &rec.cameras)?; + write_images_text(&path.join("images.txt"), &rec.images)?; + write_points3d_text(&path.join("points3D.txt"), &rec.points3d)?; + Ok(()) +} + +// --------------------------------------------------------------------------- +// Parsing helpers +// --------------------------------------------------------------------------- + +/// Returns `true` if a line is blank or a comment (first non-blank char `#`). +fn is_skippable(line: &str) -> bool { + let trimmed = line.trim_start(); + trimmed.is_empty() || trimmed.starts_with('#') +} + +/// Parses a single whitespace-separated token into `T`, with a context message. +fn parse_token(token: &str, context: &str) -> Result { + token + .parse::() + .map_err(|_| Error::Parse(format!("invalid {context}: {token:?}"))) +} + +// --------------------------------------------------------------------------- +// cameras.txt +// --------------------------------------------------------------------------- + +/// Reads `cameras.txt` into `cameras`. +fn read_cameras_text(path: &Path, cameras: &mut HashMap) -> Result<()> { + let content = fs::read_to_string(path)?; + for line in content.lines() { + if is_skippable(line) { + continue; + } + let mut tokens = line.split_whitespace(); + let camera_id: CameraId = + parse_token(next_token(&mut tokens, "CAMERA_ID")?, "camera id")?; + let model_name = next_token(&mut tokens, "MODEL")?; + let model_id = CameraModelId::from_name(model_name) + .ok_or_else(|| Error::UnknownCameraModel(model_name.to_string()))?; + let width: u64 = parse_token(next_token(&mut tokens, "WIDTH")?, "width")?; + let height: u64 = parse_token(next_token(&mut tokens, "HEIGHT")?, "height")?; + let mut params = Vec::with_capacity(model_id.num_params()); + for token in tokens { + params.push(parse_token::(token, "camera parameter")?); + } + if params.len() != model_id.num_params() { + return Err(Error::InvalidParams(format!( + "camera {camera_id}: model {} expects {} params, got {}", + model_id.name(), + model_id.num_params(), + params.len() + ))); + } + cameras.insert( + camera_id, + Camera { + camera_id, + model_id, + width, + height, + params, + has_prior_focal_length: false, + }, + ); + } + Ok(()) +} + +/// Writes `cameras` to `cameras.txt`, sorted by camera id. +fn write_cameras_text(path: &Path, cameras: &HashMap) -> Result<()> { + let mut writer = BufWriter::new(File::create(path)?); + writeln!(writer, "# Camera list with one line of data per camera:")?; + writeln!(writer, "# CAMERA_ID, MODEL, WIDTH, HEIGHT, PARAMS[]")?; + writeln!(writer, "# Number of cameras: {}", cameras.len())?; + let mut ids: Vec<&CameraId> = cameras.keys().collect(); + ids.sort_unstable(); + for id in ids { + let camera = &cameras[id]; + let mut line = format!( + "{} {} {} {}", + camera.camera_id, + camera.model_id.name(), + camera.width, + camera.height + ); + for &p in &camera.params { + write!(line, " {}", format_f64(p)).unwrap(); + } + writeln!(writer, "{line}")?; + } + writer.flush()?; + Ok(()) +} + +// --------------------------------------------------------------------------- +// images.txt +// --------------------------------------------------------------------------- + +/// Reads `images.txt` into `images`. +fn read_images_text(path: &Path, images: &mut HashMap) -> Result<()> { + let content = fs::read_to_string(path)?; + // Collect the two meaningful lines per image (header line, then points line), + // skipping comments/blank lines. The points line may itself be blank but is + // still present as a physical line; we therefore iterate raw lines and pair + // a header with the immediately following non-comment line. + let mut lines = content.lines(); + loop { + // Find the next header line (skip comments and blank lines). + let header = loop { + match lines.next() { + Some(l) if is_skippable(l) => continue, + Some(l) => break l, + None => return Ok(()), + } + }; + // The points line is the very next physical line. It may be empty. + let points_line = lines.next().unwrap_or(""); + + let mut tokens = header.split_whitespace(); + let image_id: ImageId = + parse_token(next_token(&mut tokens, "IMAGE_ID")?, "image id")?; + let qw: f64 = parse_token(next_token(&mut tokens, "QW")?, "qw")?; + let qx: f64 = parse_token(next_token(&mut tokens, "QX")?, "qx")?; + let qy: f64 = parse_token(next_token(&mut tokens, "QY")?, "qy")?; + let qz: f64 = parse_token(next_token(&mut tokens, "QZ")?, "qz")?; + let tx: f64 = parse_token(next_token(&mut tokens, "TX")?, "tx")?; + let ty: f64 = parse_token(next_token(&mut tokens, "TY")?, "ty")?; + let tz: f64 = parse_token(next_token(&mut tokens, "TZ")?, "tz")?; + let camera_id: CameraId = + parse_token(next_token(&mut tokens, "CAMERA_ID")?, "camera id")?; + // The name is the remainder of the header line (may contain spaces). + let name = tokens.collect::>().join(" "); + + let rotation = UnitQuat::from_quaternion(Quat::new(qw, qx, qy, qz)); + let cam_from_world = + crate::geometry::Rigid3d::new(rotation, Vec3::new(tx, ty, tz)); + + let mut points2d = Vec::new(); + let mut pt_tokens = points_line.split_whitespace(); + while let Some(t) = pt_tokens.next() { + let x = parse_token::(t, "point2d x")?; + let y = parse_token::( + next_token(&mut pt_tokens, "point2d y")?, + "point2d y", + )?; + let id_token = next_token(&mut pt_tokens, "point3d id")?; + let point3d_id = parse_point3d_id(id_token)?; + points2d.push(Point2D { + xy: Vec2::new(x, y), + point3d_id, + }); + } + + images.insert( + image_id, + Image { + image_id, + camera_id, + name, + cam_from_world: Some(cam_from_world), + points2d, + frame_id: None, + }, + ); + } +} + +/// Parses a `POINT3D_ID` token. The literal `-1` (or any negative) decodes to +/// [`INVALID_POINT3D_ID`]; everything else is parsed as an unsigned id. +fn parse_point3d_id(token: &str) -> Result { + if token == "-1" { + return Ok(INVALID_POINT3D_ID); + } + if let Some(stripped) = token.strip_prefix('-') { + // Any negative value means "no associated 3D point". + let _: u64 = parse_token(stripped, "point3d id")?; + return Ok(INVALID_POINT3D_ID); + } + parse_token::(token, "point3d id") +} + +/// Writes `images` to `images.txt`, sorted by image id, two lines per image. +fn write_images_text(path: &Path, images: &HashMap) -> Result<()> { + let mut writer = BufWriter::new(File::create(path)?); + writeln!(writer, "# Image list with two lines of data per image:")?; + writeln!( + writer, + "# IMAGE_ID, QW, QX, QY, QZ, TX, TY, TZ, CAMERA_ID, NAME" + )?; + writeln!(writer, "# POINTS2D[] as (X, Y, POINT3D_ID)")?; + let mean = mean_observations(images); + writeln!( + writer, + "# Number of images: {}, mean observations per image: {}", + images.len(), + format_f64(mean) + )?; + + let mut ids: Vec<&ImageId> = images.keys().collect(); + ids.sort_unstable(); + for id in ids { + let image = &images[id]; + let pose = image + .cam_from_world + .unwrap_or_else(crate::geometry::Rigid3d::identity); + let q = pose.rotation.into_inner(); + writeln!( + writer, + "{} {} {} {} {} {} {} {} {} {}", + image.image_id, + format_f64(q.w), + format_f64(q.i), + format_f64(q.j), + format_f64(q.k), + format_f64(pose.translation.x), + format_f64(pose.translation.y), + format_f64(pose.translation.z), + image.camera_id, + image.name, + )?; + + let mut line = String::new(); + for (i, p) in image.points2d.iter().enumerate() { + if i > 0 { + line.push(' '); + } + let id_str = if p.point3d_id == INVALID_POINT3D_ID { + "-1".to_string() + } else { + p.point3d_id.to_string() + }; + write!(line, "{} {} {}", format_f64(p.xy.x), format_f64(p.xy.y), id_str) + .unwrap(); + } + writeln!(writer, "{line}")?; + } + writer.flush()?; + Ok(()) +} + +/// Mean number of 2D points that reference a valid 3D point, across images. +fn mean_observations(images: &HashMap) -> f64 { + if images.is_empty() { + return 0.0; + } + let total: usize = images + .values() + .map(|im| { + im.points2d + .iter() + .filter(|p| p.point3d_id != INVALID_POINT3D_ID) + .count() + }) + .sum(); + total as f64 / images.len() as f64 +} + +// --------------------------------------------------------------------------- +// points3D.txt +// --------------------------------------------------------------------------- + +/// Reads `points3D.txt` into `points3d`. +fn read_points3d_text( + path: &Path, + points3d: &mut HashMap, +) -> Result<()> { + let content = fs::read_to_string(path)?; + for line in content.lines() { + if is_skippable(line) { + continue; + } + let mut tokens = line.split_whitespace(); + let point3d_id: Point3DId = + parse_token(next_token(&mut tokens, "POINT3D_ID")?, "point3d id")?; + let x: f64 = parse_token(next_token(&mut tokens, "X")?, "x")?; + let y: f64 = parse_token(next_token(&mut tokens, "Y")?, "y")?; + let z: f64 = parse_token(next_token(&mut tokens, "Z")?, "z")?; + let r: u8 = parse_token(next_token(&mut tokens, "R")?, "r")?; + let g: u8 = parse_token(next_token(&mut tokens, "G")?, "g")?; + let b: u8 = parse_token(next_token(&mut tokens, "B")?, "b")?; + let error: f64 = parse_token(next_token(&mut tokens, "ERROR")?, "error")?; + + let mut elements = Vec::new(); + while let Some(t) = tokens.next() { + let image_id = parse_token::(t, "track image id")?; + let point2d_idx: Point2DIdx = parse_token( + next_token(&mut tokens, "POINT2D_IDX")?, + "track point2d idx", + )?; + elements.push(TrackElement { + image_id, + point2d_idx, + }); + } + + points3d.insert( + point3d_id, + Point3D { + xyz: Vec3::new(x, y, z), + color: Rgb::new(r, g, b), + error, + track: Track { elements }, + }, + ); + } + Ok(()) +} + +/// Writes `points3d` to `points3D.txt`, sorted by point id. +fn write_points3d_text( + path: &Path, + points3d: &HashMap, +) -> Result<()> { + let mut writer = BufWriter::new(File::create(path)?); + writeln!(writer, "# 3D point list with one line of data per point:")?; + writeln!( + writer, + "# POINT3D_ID, X, Y, Z, R, G, B, ERROR, TRACK[] as (IMAGE_ID, POINT2D_IDX)" + )?; + let mean_track = mean_track_length(points3d); + writeln!( + writer, + "# Number of points: {}, mean track length: {}", + points3d.len(), + format_f64(mean_track) + )?; + + let mut ids: Vec<&Point3DId> = points3d.keys().collect(); + ids.sort_unstable(); + for id in ids { + let point = &points3d[id]; + let mut line = format!( + "{} {} {} {} {} {} {} {}", + id, + format_f64(point.xyz.x), + format_f64(point.xyz.y), + format_f64(point.xyz.z), + point.color.x, + point.color.y, + point.color.z, + format_f64(point.error), + ); + for el in &point.track.elements { + write!(line, " {} {}", el.image_id, el.point2d_idx).unwrap(); + } + writeln!(writer, "{line}")?; + } + writer.flush()?; + Ok(()) +} + +/// Mean track length across all 3D points. +fn mean_track_length(points3d: &HashMap) -> f64 { + if points3d.is_empty() { + return 0.0; + } + let total: usize = points3d.values().map(|p| p.track.elements.len()).sum(); + total as f64 / points3d.len() as f64 +} + +// --------------------------------------------------------------------------- +// Token / number formatting +// --------------------------------------------------------------------------- + +/// Returns the next whitespace token or a [`Error::Parse`] naming the field. +fn next_token<'a, I: Iterator>( + tokens: &mut I, + field: &str, +) -> Result<&'a str> { + tokens + .next() + .ok_or_else(|| Error::Parse(format!("missing field {field}"))) +} + +/// Formats an `f64` for text output. +/// +/// COLMAP writes floats with full `double` precision; Rust's default `{}` +/// already round-trips an `f64` exactly (shortest decimal that parses back to +/// the same value), so it is used directly. Integral values keep a clean +/// representation (e.g. `640` rather than `640.0`), which the reader parses +/// just as well. +fn format_f64(value: f64) -> String { + value.to_string() +} + +#[cfg(test)] +mod tests { + use super::*; + use crate::geometry::Rigid3d; + + fn sample_reconstruction() -> Reconstruction { + let mut rec = Reconstruction::default(); + + // SIMPLE_RADIAL has one distortion term. + rec.cameras.insert( + 7, + Camera { + camera_id: 7, + model_id: CameraModelId::SimpleRadial, + width: 1920, + height: 1080, + params: vec![1200.5, 960.0, 540.0, -0.0123], + has_prior_focal_length: false, + }, + ); + + let s = std::f64::consts::FRAC_1_SQRT_2; + let rotation = UnitQuat::from_quaternion(Quat::new(s, 0.0, s, 0.0)); + let cam_from_world = Rigid3d::new(rotation, Vec3::new(0.25, -1.5, 12.0)); + rec.images.insert( + 42, + Image { + image_id: 42, + camera_id: 7, + name: "images/photo 42.jpg".to_string(), + cam_from_world: Some(cam_from_world), + points2d: vec![ + Point2D { + xy: Vec2::new(11.5, 22.5), + point3d_id: 5, + }, + Point2D { + xy: Vec2::new(33.0, 44.0), + point3d_id: INVALID_POINT3D_ID, + }, + Point2D { + xy: Vec2::new(55.25, 66.75), + point3d_id: 5, + }, + ], + frame_id: None, + }, + ); + + // An image with no 2D points (empty second line). + rec.images.insert( + 43, + Image { + image_id: 43, + camera_id: 7, + name: "images/empty.jpg".to_string(), + cam_from_world: Some(Rigid3d::identity()), + points2d: vec![], + frame_id: None, + }, + ); + + rec.points3d.insert( + 5, + Point3D { + xyz: Vec3::new(-3.5, 4.25, 9.0), + color: Rgb::new(255, 128, 0), + error: 1.25, + track: Track { + elements: vec![ + TrackElement { + image_id: 42, + point2d_idx: 0, + }, + TrackElement { + image_id: 42, + point2d_idx: 2, + }, + ], + }, + }, + ); + + rec + } + + fn assert_same(a: &Reconstruction, b: &Reconstruction) { + assert_eq!(a.cameras.len(), b.cameras.len()); + for (id, ca) in &a.cameras { + let cb = &b.cameras[id]; + assert_eq!(ca.model_id, cb.model_id); + assert_eq!(ca.width, cb.width); + assert_eq!(ca.height, cb.height); + assert_eq!(ca.params, cb.params); + } + + assert_eq!(a.images.len(), b.images.len()); + for (id, ia) in &a.images { + let ib = &b.images[id]; + assert_eq!(ia.camera_id, ib.camera_id); + assert_eq!(ia.name, ib.name); + let pa = ia.cam_from_world.unwrap(); + let pb = ib.cam_from_world.unwrap(); + assert!((pa.translation - pb.translation).norm() < 1e-12); + let qa = pa.rotation.into_inner(); + let qb = pb.rotation.into_inner(); + let diff = (qa.w - qb.w).abs() + + (qa.i - qb.i).abs() + + (qa.j - qb.j).abs() + + (qa.k - qb.k).abs(); + assert!(diff < 1e-12, "quaternion mismatch: {qa:?} vs {qb:?}"); + assert_eq!(ia.points2d.len(), ib.points2d.len()); + for (p, q) in ia.points2d.iter().zip(&ib.points2d) { + assert!((p.xy - q.xy).norm() < 1e-12); + assert_eq!(p.point3d_id, q.point3d_id); + } + } + + assert_eq!(a.points3d.len(), b.points3d.len()); + for (id, pa) in &a.points3d { + let pb = &b.points3d[id]; + assert!((pa.xyz - pb.xyz).norm() < 1e-12); + assert_eq!(pa.color, pb.color); + assert!((pa.error - pb.error).abs() < 1e-12); + assert_eq!(pa.track.elements.len(), pb.track.elements.len()); + for (ea, eb) in pa.track.elements.iter().zip(&pb.track.elements) { + assert_eq!(ea.image_id, eb.image_id); + assert_eq!(ea.point2d_idx, eb.point2d_idx); + } + } + } + + #[test] + fn text_roundtrip() { + let dir = tempfile::tempdir().unwrap(); + let rec = sample_reconstruction(); + write_model_text(&rec, dir.path()).unwrap(); + let read_back = read_model_text(dir.path()).unwrap(); + assert_same(&rec, &read_back); + } + + #[test] + fn invalid_point3d_id_written_as_minus_one() { + let dir = tempfile::tempdir().unwrap(); + let rec = sample_reconstruction(); + write_model_text(&rec, dir.path()).unwrap(); + let text = fs::read_to_string(dir.path().join("images.txt")).unwrap(); + assert!(text.contains("-1"), "expected a -1 sentinel in:\n{text}"); + let read_back = read_model_text(dir.path()).unwrap(); + assert_eq!( + read_back.images[&42].points2d[1].point3d_id, + INVALID_POINT3D_ID + ); + } + + #[test] + fn empty_points_line_yields_no_points() { + let dir = tempfile::tempdir().unwrap(); + let rec = sample_reconstruction(); + write_model_text(&rec, dir.path()).unwrap(); + let read_back = read_model_text(dir.path()).unwrap(); + assert!(read_back.images[&43].points2d.is_empty()); + } + + #[test] + fn comments_are_ignored() { + let dir = tempfile::tempdir().unwrap(); + fs::write( + dir.path().join("cameras.txt"), + "# a comment\n # indented comment\n1 PINHOLE 100 100 50 50 50 50\n", + ) + .unwrap(); + let mut cams = HashMap::new(); + read_cameras_text(&dir.path().join("cameras.txt"), &mut cams).unwrap(); + assert_eq!(cams.len(), 1); + assert_eq!(cams[&1].model_id, CameraModelId::Pinhole); + } + + #[test] + fn parse_point3d_id_handles_negatives() { + assert_eq!(parse_point3d_id("-1").unwrap(), INVALID_POINT3D_ID); + assert_eq!(parse_point3d_id("-5").unwrap(), INVALID_POINT3D_ID); + assert_eq!(parse_point3d_id("12345").unwrap(), 12345u64); + } +} diff --git a/src/lib.rs b/src/lib.rs new file mode 100644 index 0000000..01a7e2c --- /dev/null +++ b/src/lib.rs @@ -0,0 +1,226 @@ +//! # colmap — a pure-Rust port of COLMAP / PyCOLMAP +//! +//! [COLMAP](https://colmap.github.io/) is a general-purpose Structure-from-Motion +//! (SfM) and Multi-View Stereo (MVS) pipeline. [PyCOLMAP](https://colmap.github.io/pycolmap/) +//! exposes its C++ data model and pipeline to Python. This crate reimplements the +//! same structure in **pure, dependency-light Rust** — no C++ build, no FFI. +//! +//! The module layout mirrors PyCOLMAP one-to-one: +//! +//! | Module | PyCOLMAP equivalent | Status | +//! |-------------------|------------------------------------|--------| +//! | [`types`] | id aliases, `SensorType`, models | implemented | +//! | [`math`] | Eigen vector/matrix vocabulary | implemented | +//! | [`geometry`] | `Rigid3d`, `Sim3d`, `Rotation3d`, … | core implemented | +//! | `sensor` | camera models, bitmap | camera models implemented | +//! | `image` | image size / EXIF / `infer_camera_from_image` | implemented | +//! | `scene` | `Camera`, `Image`, `Point3D`, `Reconstruction`, … | implemented | +//! | `io` | sparse-model `.bin` / `.txt` I/O | implemented | +//! | `database` | SQLite feature database | behind the `database` feature | +//! | `estimators` | RANSAC, pose, triangulation | scaffolded | +//! | `feature` | SIFT extraction & matching | scaffolded | +//! | `sfm` | incremental mapping & triangulation | scaffolded | +//! | `mvs` | dense patch-match & fusion | scaffolded | +//! | `optim` | bundle adjustment | scaffolded | +//! | `retrieval` | vocabulary-tree image retrieval | scaffolded | +//! | `pipeline` | high-level one-call pipelines | scaffolded | +//! | [`highlevel`] | flat end-to-end pipeline (feature→SfM→MVS→export) | runnable demo | +//! +//! The [`highlevel`] module is different in kind: it offers the flat, ergonomic +//! API of the higher-level `colmap` crate and **runs end to end** on a built-in +//! synthetic scene (see its docs), producing valid PLY / OBJ / model files. +//! +//! "Scaffolded" modules expose the same types and function signatures as PyCOLMAP +//! and return [`Error::Unimplemented`] where the heavy numerical algorithm is not +//! yet ported, so the public surface is complete and stable to build against. +//! +//! ## Quick start +//! +//! Read a sparse reconstruction written by COLMAP (binary or text) and inspect it: +//! +//! ```no_run +//! use colmap::scene::Reconstruction; +//! +//! let rec = Reconstruction::read("/path/to/sparse/0".as_ref())?; +//! println!("{} cameras, {} images, {} points", +//! rec.num_cameras(), rec.num_images(), rec.num_points3d()); +//! # Ok::<(), colmap::Error>(()) +//! ``` +//! +//! Infer a camera from an image's header (size + EXIF), like `infer_camera_from_image`: +//! +//! ```no_run +//! use colmap::image::infer_camera_from_image; +//! +//! let camera = infer_camera_from_image("photo.jpg".as_ref(), 1)?; +//! println!("{} {}x{}", camera.model_name(), camera.width, camera.height); +//! # Ok::<(), colmap::Error>(()) +//! ``` +//! +//! Compose rigid transforms with the same `b_from_a` convention as COLMAP: +//! +//! ``` +//! use colmap::geometry::Rigid3d; +//! use colmap::math::{UnitQuat, Vec3}; +//! +//! let cam_from_world = Rigid3d::new(UnitQuat::identity(), Vec3::new(1.0, 2.0, 3.0)); +//! let center = cam_from_world.target_origin_in_source(); // camera center in world +//! assert_eq!(center, Vec3::new(-1.0, -2.0, -3.0)); +//! ``` +//! +//! ## Pipeline examples +//! +//! These mirror the three examples of the original `colmap` crate. The basic +//! building blocks (example 1) and the complete flat workflow (example 2, on the +//! [`highlevel`] API) run for real — example 2 is `no_run` only because it reads +//! images from disk; example 3 shows the `Error::Unimplemented` a not-yet-ported +//! PyCOLMAP-style stage returns. +//! +//! ### 1. Basic building blocks +//! +//! ``` +//! use colmap::scene::Camera; +//! use colmap::types::CameraModelId; +//! use colmap::math::{Vec2, Vec3}; +//! use colmap::feature::FeatureExtractionOptions; +//! use colmap::sfm::IncrementalPipelineOptions; +//! use colmap::mvs::PatchMatchOptions; +//! +//! // 1. Create a pinhole camera (focal 800 px, image 640×480). +//! let camera = Camera::new_with_model(1, CameraModelId::Pinhole, 800.0, 640, 480); +//! assert_eq!(camera.model_name(), "PINHOLE"); +//! assert_eq!((camera.width, camera.height), (640, 480)); +//! +//! // A point straight ahead projects onto the principal point. +//! let uv = camera.img_from_cam(&Vec3::new(0.0, 0.0, 1.0)).unwrap(); +//! assert_eq!(uv, Vec2::new(320.0, 240.0)); +//! +//! // 2./3./4. Configuration for feature extraction, SfM and MVS +//! // (same shape as PyCOLMAP's option classes). +//! let _features = FeatureExtractionOptions::default(); +//! let _sfm = IncrementalPipelineOptions::default(); +//! let _mvs = PatchMatchOptions::default(); +//! ``` +//! +//! ### 2. Complete reconstruction workflow +//! +//! The flat, end-to-end [`highlevel`] API (the same shape as the reference +//! `colmap` crate): load images, extract & match features, run sparse SfM and +//! dense MVS, then save the sparse model, the dense point cloud (`.ply`) and the +//! mesh (`.obj`). +//! +//! ```no_run +//! use colmap::*; +//! use std::path::Path; +//! +//! fn reconstruct_from_images(image_dir: &Path) -> Result<()> { +//! // 1. Load images. +//! let images = load_images_from_directory(image_dir)?; +//! +//! // 2. Feature extraction and matching. +//! let pipeline = FeaturePipeline::new(PipelineConfig { +//! detector_type: DetectorType::Sift, +//! max_features: 8000, +//! ..Default::default() +//! }); +//! let extraction = pipeline.extract_and_match_all(&images)?; +//! println!("features for {} images, {} match pairs", +//! extraction.features.len(), extraction.matches.len()); +//! +//! // 3. Sparse SfM reconstruction. +//! let mut sfm = IncrementalSfm::new(SfmConfig { min_track_length: 2, ..Default::default() }); +//! sfm.set_features(extraction.features); +//! sfm.set_matches(extraction.matches); +//! let sparse = sfm.reconstruct()?; +//! println!("registered images: {}", sparse.registered_images()); +//! println!("3D points: {}", sparse.points.len()); +//! println!("mean reprojection error: {:.2}", sparse.mean_reprojection_error()); +//! +//! // 4. Dense MVS reconstruction. +//! let mvs = MvsReconstructor::new(MvsConfig { min_num_views: 3, ..Default::default() }); +//! let views = prepare_views_from_reconstruction(&sparse)?; +//! let dense = mvs.reconstruct(&views)?; +//! +//! // 5. Save the results: COLMAP model dir, point cloud (.ply), mesh (.obj). +//! save_reconstruction(&sparse, "sparse_reconstruction")?; +//! save_point_cloud(&dense.point_cloud, "dense_point_cloud.ply")?; +//! save_mesh(&dense.mesh, "mesh.obj")?; +//! Ok(()) +//! } +//! # let _ = reconstruct_from_images; +//! ``` +//! +//! ### 3. Error handling +//! +//! ``` +//! use colmap::{Error, Result}; +//! use colmap::mvs::{patch_match_stereo, PatchMatchOptions}; +//! use std::path::Path; +//! +//! let result: Result<()> = +//! patch_match_stereo(Path::new("/tmp/workspace"), &PatchMatchOptions::default()); +//! match result { +//! Ok(()) => println!("dense reconstruction done"), +//! Err(Error::Unimplemented(what)) => { +//! eprintln!("step not ported yet: {what}"); +//! assert_eq!(what, "dense patch-match stereo"); +//! } +//! Err(Error::Io(err)) => eprintln!("I/O error: {err}"), +//! Err(err) => eprintln!("other error: {err}"), +//! } +//! ``` +//! +//! ## Cargo features +//! +//! - `database` — enables the SQLite feature database (bundles SQLite, no system dependency). +//! - `serde` — derives `Serialize`/`Deserialize` for the public data types. + +#![cfg_attr(docsrs, feature(doc_cfg))] +#![warn(missing_docs)] + +pub mod error; +pub mod estimators; +pub mod feature; +pub mod geometry; +pub mod highlevel; +pub mod image; +pub mod io; +pub mod math; +pub mod mvs; +pub mod optim; +pub mod pipeline; +pub mod retrieval; +pub mod scene; +pub mod sensor; +pub mod sfm; +pub mod types; +#[cfg(feature = "database")] +pub mod database; + +pub use error::{Error, Result}; + +// The flat, ergonomic pipeline API (mirrors the high-level `colmap` crate) is +// re-exported at the crate root so it is reachable via `use colmap::*;`. +pub use highlevel::{ + load_images_from_directory, prepare_views_from_reconstruction, save_mesh, save_point_cloud, + save_reconstruction, CameraView, DenseReconstruction, DetectorType, ExtractionResult, + FeaturePipeline, Image, ImageFeatures, ImageMatches, IncrementalSfm, Keypoint, Mesh, MvsConfig, + MvsReconstructor, Observation, PipelineConfig, Point3d, PointCloud, PointXyzRgb, + SfmConfig, SparseReconstruction, View, +}; + +/// A flat re-export of the most commonly used items. +/// +/// ``` +/// use colmap::prelude::*; +/// let t = Rigid3d::identity(); +/// # let _ = t; +/// ``` +pub mod prelude { + pub use crate::error::{Error, Result}; + pub use crate::geometry::{Rigid3d, Rotation3d, Sim3d}; + pub use crate::math::{Mat3, Mat3x4, Vec2, Vec3}; + pub use crate::types::{ + CameraId, CameraModelId, FrameId, ImageId, Point2DIdx, Point3DId, RigId, SensorType, + }; +} diff --git a/src/math.rs b/src/math.rs new file mode 100644 index 0000000..8772268 --- /dev/null +++ b/src/math.rs @@ -0,0 +1,37 @@ +//! Linear-algebra vocabulary shared across the crate. +//! +//! The whole crate speaks [`nalgebra`]. To keep call sites short and to make the +//! mapping from COLMAP's Eigen types obvious, we expose a small set of aliases +//! that every module reuses. +//! +//! | COLMAP / Eigen | `colmap` (this crate) | +//! |-------------------------|-----------------------| +//! | `Eigen::Vector2d` | [`Vec2`] | +//! | `Eigen::Vector3d` | [`Vec3`] | +//! | `Eigen::Vector4d` | [`Vec4`] | +//! | `Eigen::Vector3ub` | [`Rgb`] | +//! | `Eigen::Matrix3d` | [`Mat3`] | +//! | `Eigen::Matrix3x4d` | [`Mat3x4`] | +//! | `Eigen::Matrix4d` | [`Mat4`] | +//! | `Eigen::Quaterniond` | [`Quat`] / [`UnitQuat`] | + +pub use nalgebra::{Matrix3, Matrix3x4, Matrix4, Quaternion, UnitQuaternion, Vector2, Vector3, Vector4}; + +/// A 2D column vector of `f64` (pixel / normalized image coordinates). +pub type Vec2 = Vector2; +/// A 3D column vector of `f64` (world / camera coordinates). +pub type Vec3 = Vector3; +/// A 4D column vector of `f64` (homogeneous coordinates, quaternion coeffs). +pub type Vec4 = Vector4; +/// An RGB color, one byte per channel, matching `Eigen::Vector3ub`. +pub type Rgb = Vector3; +/// A 3×3 matrix of `f64` (rotations, intrinsics `K`, `E`/`F`/`H`). +pub type Mat3 = Matrix3; +/// A 3×4 matrix of `f64` (a `[R | t]` pose / projection matrix). +pub type Mat3x4 = Matrix3x4; +/// A 4×4 matrix of `f64` (homogeneous transforms). +pub type Mat4 = Matrix4; +/// A raw quaternion of `f64` (`w` scalar, `i j k` vector parts). +pub type Quat = Quaternion; +/// A unit quaternion of `f64`, the canonical rotation representation. +pub type UnitQuat = UnitQuaternion; diff --git a/src/mvs/mod.rs b/src/mvs/mod.rs new file mode 100644 index 0000000..a5ed70e --- /dev/null +++ b/src/mvs/mod.rs @@ -0,0 +1,221 @@ +//! Multi-View Stereo (MVS): dense depth/normal estimation and fusion. +//! +//! This module mirrors the dense reconstruction stage of +//! [PyCOLMAP](https://colmap.github.io/pycolmap/) (`pycolmap.patch_match_stereo` +//! and `pycolmap.stereo_fusion`). It provides the small dense-map data types +//! ([`DepthMap`], [`NormalMap`]) and the option structs ([`PatchMatchOptions`], +//! [`StereoFusionOptions`]) used by the COLMAP MVS controllers. +//! +//! The actual PatchMatch stereo and point-cloud fusion algorithms — which in +//! COLMAP are GPU-accelerated and numerically heavy — are not ported in this +//! pure-Rust crate; the driver functions return +//! [`crate::Error::Unimplemented`]. + +use std::path::Path; + +/// A dense per-pixel depth map. +/// +/// Depths are stored row-major in [`data`](Self::data) (one `f32` per pixel), +/// with a value of `0.0` conventionally denoting an invalid/unknown depth. +/// [`depth_min`](Self::depth_min) and [`depth_max`](Self::depth_max) bound the +/// expected depth range for the corresponding view. +#[derive(Debug, Clone, PartialEq)] +#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))] +pub struct DepthMap { + /// Image width in pixels. + pub width: usize, + /// Image height in pixels. + pub height: usize, + /// Lower bound of the depth range. + pub depth_min: f64, + /// Upper bound of the depth range. + pub depth_max: f64, + /// Row-major depth values, length `width * height`. + pub data: Vec, +} + +impl DepthMap { + /// Create a zero-initialized depth map of the given size and depth range. + pub fn new(width: usize, height: usize, depth_min: f64, depth_max: f64) -> Self { + Self { + width, + height, + depth_min, + depth_max, + data: vec![0.0; width * height], + } + } +} + +/// A dense per-pixel surface-normal map. +/// +/// Normals are stored row-major in [`data`](Self::data) with **three** `f32` +/// components (x, y, z) per pixel, so `data.len() == 3 * width * height`. +#[derive(Debug, Clone, PartialEq)] +#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))] +pub struct NormalMap { + /// Image width in pixels. + pub width: usize, + /// Image height in pixels. + pub height: usize, + /// Row-major normal components (3 per pixel), length `3 * width * height`. + pub data: Vec, +} + +impl NormalMap { + /// Create a zero-initialized normal map of the given size (3 floats/pixel). + pub fn new(width: usize, height: usize) -> Self { + Self { + width, + height, + data: vec![0.0; 3 * width * height], + } + } +} + +/// Options controlling PatchMatch dense stereo estimation. +/// +/// Mirrors a subset of COLMAP's `mvs::PatchMatchOptions`. Defaults match +/// COLMAP's defaults. +#[derive(Debug, Clone, PartialEq)] +#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))] +pub struct PatchMatchOptions { + /// Downscale images so their largest dimension does not exceed this value; + /// `-1` disables downscaling. + pub max_image_size: i32, + /// Half-size of the matching window (full window is `2*r + 1` per side). + pub window_radius: i32, + /// Number of random samples drawn per pixel during propagation. + pub num_samples: i32, + /// Number of PatchMatch propagation iterations. + pub num_iterations: i32, + /// Whether to enforce geometric consistency across source views. + pub geom_consistency: bool, +} + +impl Default for PatchMatchOptions { + fn default() -> Self { + Self { + max_image_size: -1, + window_radius: 5, + num_samples: 15, + num_iterations: 5, + geom_consistency: true, + } + } +} + +/// Options controlling dense stereo fusion into a fused point cloud. +/// +/// Mirrors a subset of COLMAP's `mvs::StereoFusionOptions`. Defaults match +/// COLMAP's defaults. +#[derive(Debug, Clone, PartialEq)] +#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))] +pub struct StereoFusionOptions { + /// Minimum number of consistent pixels required to fuse a 3D point. + pub min_num_pixels: i32, + /// Maximum number of pixels that may be fused into a single 3D point. + pub max_num_pixels: i32, + /// Maximum allowed reprojection error (in pixels) for a fused pixel. + pub max_reproj_error: f64, +} + +impl Default for StereoFusionOptions { + fn default() -> Self { + Self { + min_num_pixels: 5, + max_num_pixels: 10000, + max_reproj_error: 2.0, + } + } +} + +/// Run dense PatchMatch stereo over a prepared MVS workspace. +/// +/// The workspace at `workspace_path` is expected to be in COLMAP's dense format +/// (undistorted images, sparse model and patch-match configuration). Mirrors +/// `pycolmap.patch_match_stereo`. +/// +/// # Errors +/// +/// Returns [`crate::Error::Unimplemented`]; dense stereo is not ported in this +/// pure-Rust crate. +pub fn patch_match_stereo( + workspace_path: &Path, + options: &PatchMatchOptions, +) -> crate::Result<()> { + let _ = (workspace_path, options); + Err(crate::Error::Unimplemented("dense patch-match stereo")) +} + +/// Fuse per-view depth/normal maps into a single dense point cloud. +/// +/// Reads the depth and normal maps produced by [`patch_match_stereo`] from the +/// workspace at `workspace_path` and fuses geometrically consistent pixels. +/// Mirrors `pycolmap.stereo_fusion`. +/// +/// # Errors +/// +/// Returns [`crate::Error::Unimplemented`]; stereo fusion is not ported in this +/// pure-Rust crate. +pub fn stereo_fusion( + workspace_path: &Path, + options: &StereoFusionOptions, +) -> crate::Result<()> { + let _ = (workspace_path, options); + Err(crate::Error::Unimplemented("dense stereo fusion")) +} + +#[cfg(test)] +mod tests { + use super::*; + use std::path::Path; + + #[test] + fn depth_map_new_is_zeroed() { + let d = DepthMap::new(4, 3, 0.5, 10.0); + assert_eq!(d.width, 4); + assert_eq!(d.height, 3); + assert_eq!(d.data.len(), 12); + assert!(d.data.iter().all(|&v| v == 0.0)); + assert!((d.depth_min - 0.5).abs() < 1e-12); + assert!((d.depth_max - 10.0).abs() < 1e-12); + } + + #[test] + fn normal_map_has_three_floats_per_pixel() { + let n = NormalMap::new(4, 3); + assert_eq!(n.data.len(), 3 * 4 * 3); + assert!(n.data.iter().all(|&v| v == 0.0)); + } + + #[test] + fn patch_match_options_defaults() { + let o = PatchMatchOptions::default(); + assert_eq!(o.max_image_size, -1); + assert_eq!(o.window_radius, 5); + assert_eq!(o.num_samples, 15); + assert_eq!(o.num_iterations, 5); + assert!(o.geom_consistency); + } + + #[test] + fn fusion_options_defaults() { + let o = StereoFusionOptions::default(); + assert_eq!(o.min_num_pixels, 5); + assert_eq!(o.max_num_pixels, 10000); + assert!((o.max_reproj_error - 2.0).abs() < 1e-12); + } + + #[test] + fn stubs_are_unimplemented() { + assert!(matches!( + patch_match_stereo(Path::new("ws"), &PatchMatchOptions::default()).unwrap_err(), + crate::Error::Unimplemented(_) + )); + assert!(matches!( + stereo_fusion(Path::new("ws"), &StereoFusionOptions::default()).unwrap_err(), + crate::Error::Unimplemented(_) + )); + } +} diff --git a/src/optim/mod.rs b/src/optim/mod.rs new file mode 100644 index 0000000..ae7b7c1 --- /dev/null +++ b/src/optim/mod.rs @@ -0,0 +1,89 @@ +//! Non-linear optimization: bundle adjustment. +//! +//! This module mirrors the bundle-adjustment stage of +//! [PyCOLMAP](https://colmap.github.io/pycolmap/) (`pycolmap.bundle_adjustment` +//! and COLMAP's `BundleAdjuster` built on the Ceres solver). It exposes the +//! [`BundleAdjustmentOptions`] struct and the [`bundle_adjustment`] driver. +//! +//! COLMAP's bundle adjustment relies on the Ceres non-linear least-squares +//! solver, which is not ported in this pure-Rust crate; the driver therefore +//! returns [`crate::Error::Unimplemented`]. + +use crate::scene::Reconstruction; + +/// Options controlling a bundle-adjustment run. +/// +/// These mirror a subset of COLMAP's `BundleAdjustmentOptions`, selecting which +/// parameter blocks are refined and bounding the solver iterations. Defaults +/// match COLMAP's defaults. +#[derive(Debug, Clone, PartialEq)] +#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))] +pub struct BundleAdjustmentOptions { + /// Whether to refine camera focal length parameters. + pub refine_focal_length: bool, + /// Whether to refine camera principal-point parameters. + pub refine_principal_point: bool, + /// Whether to refine camera extra (distortion) parameters. + pub refine_extra_params: bool, + /// Whether to refine the rig-from-world poses (sensor rig extrinsics). + pub refine_rig_from_world: bool, + /// Maximum number of solver iterations. + pub max_num_iterations: usize, +} + +impl Default for BundleAdjustmentOptions { + fn default() -> Self { + Self { + refine_focal_length: true, + refine_principal_point: false, + refine_extra_params: true, + refine_rig_from_world: true, + max_num_iterations: 100, + } + } +} + +/// Jointly refine camera poses, intrinsics and 3D point positions. +/// +/// Performs non-linear least-squares minimization of the reprojection error +/// over the reconstruction in place. Mirrors `pycolmap.bundle_adjustment`. +/// +/// # Errors +/// +/// Returns [`crate::Error::Unimplemented`]; the Ceres-based solver is not ported +/// in this pure-Rust crate. +pub fn bundle_adjustment( + rec: &mut Reconstruction, + options: &BundleAdjustmentOptions, +) -> crate::Result<()> { + let _ = (rec, options); + Err(crate::Error::Unimplemented("Ceres bundle adjustment")) +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn options_defaults() { + let o = BundleAdjustmentOptions::default(); + assert!(o.refine_focal_length); + assert!(!o.refine_principal_point); + assert!(o.refine_extra_params); + assert!(o.refine_rig_from_world); + assert_eq!(o.max_num_iterations, 100); + } + + #[test] + fn bundle_adjustment_is_unimplemented() { + let mut rec = Reconstruction { + rigs: Default::default(), + cameras: Default::default(), + frames: Default::default(), + images: Default::default(), + points3d: Default::default(), + }; + let err = bundle_adjustment(&mut rec, &BundleAdjustmentOptions::default()).unwrap_err(); + assert!(matches!(err, crate::Error::Unimplemented(_))); + } +} diff --git a/src/pipeline/mod.rs b/src/pipeline/mod.rs new file mode 100644 index 0000000..732732b --- /dev/null +++ b/src/pipeline/mod.rs @@ -0,0 +1,149 @@ +//! High-level, one-call reconstruction pipeline. +//! +//! This module mirrors the top-level convenience functions of +//! [PyCOLMAP](https://colmap.github.io/pycolmap/) — the free functions you find +//! directly under the `pycolmap` namespace, such as `pycolmap.import_images`, +//! `pycolmap.extract_features`, `pycolmap.match_exhaustive`, +//! `pycolmap.incremental_mapping` and `pycolmap.undistort_images`. +//! +//! It re-exports the option types from the underlying stage modules +//! ([`crate::sfm`], [`crate::mvs`], [`crate::optim`], [`crate::retrieval`]) so a +//! caller can drive the whole reconstruction from this single module, and wraps +//! the per-stage driver functions. +//! +//! Stages whose numerical core is not ported in this pure-Rust crate return +//! [`crate::Error::Unimplemented`]. + +use std::path::Path; + +// --------------------------------------------------------------------------- +// Re-exported option / driver types from the stage modules. +// --------------------------------------------------------------------------- + +pub use crate::mvs::{PatchMatchOptions, StereoFusionOptions}; +pub use crate::optim::BundleAdjustmentOptions; +pub use crate::retrieval::VisualIndexOptions; +pub use crate::sfm::{IncrementalMapperOptions, IncrementalPipelineOptions}; + +// Re-export the stage drivers so the whole pipeline is reachable from here. +pub use crate::mvs::{patch_match_stereo, stereo_fusion}; +pub use crate::optim::bundle_adjustment; +pub use crate::sfm::{incremental_mapping, triangulate_points}; + +/// Import images from a directory into a COLMAP feature database. +/// +/// Scans `image_path` for images and registers them (and their default camera) +/// into the database at `database_path`. Mirrors `pycolmap.import_images`. +/// +/// # Errors +/// +/// Returns [`crate::Error::Unimplemented`]; image import is not ported in this +/// pure-Rust crate. +pub fn import_images(database_path: &Path, image_path: &Path) -> crate::Result<()> { + let _ = (database_path, image_path); + Err(crate::Error::Unimplemented("import images")) +} + +/// Extract SIFT features from the database's images and store keypoints and +/// descriptors back into the database. +/// +/// Mirrors `pycolmap.extract_features`. This forwards to the feature-extraction +/// stage; SIFT extraction is not ported in this pure-Rust crate, so this returns +/// [`crate::Error::Unimplemented`]. +/// +/// # Errors +/// +/// Returns [`crate::Error::Unimplemented`]. +pub fn extract_features(database_path: &Path, image_path: &Path) -> crate::Result<()> { + let _ = (database_path, image_path); + Err(crate::Error::Unimplemented("feature extraction")) +} + +/// Match features between every pair of images (exhaustive matching). +/// +/// Mirrors `pycolmap.match_exhaustive`. +/// +/// # Errors +/// +/// Returns [`crate::Error::Unimplemented`]; exhaustive matching is not ported in +/// this pure-Rust crate. +pub fn match_exhaustive(database_path: &Path) -> crate::Result<()> { + let _ = database_path; + Err(crate::Error::Unimplemented("exhaustive feature matching")) +} + +/// Undistort the images and sparse model into a workspace suitable for dense MVS. +/// +/// Mirrors `pycolmap.undistort_images`. +/// +/// # Errors +/// +/// Returns [`crate::Error::Unimplemented`]; image undistortion is not ported in +/// this pure-Rust crate. +pub fn undistort_images( + output_path: &Path, + input_path: &Path, + image_path: &Path, +) -> crate::Result<()> { + let _ = (output_path, input_path, image_path); + Err(crate::Error::Unimplemented("image undistortion")) +} + +#[cfg(test)] +mod tests { + use super::*; + use std::path::Path; + + #[test] + fn reexported_option_defaults() { + // The re-exports point at the genuine stage-module types. + assert_eq!( + IncrementalPipelineOptions::default(), + crate::sfm::IncrementalPipelineOptions::default() + ); + assert_eq!( + BundleAdjustmentOptions::default(), + crate::optim::BundleAdjustmentOptions::default() + ); + assert_eq!( + PatchMatchOptions::default(), + crate::mvs::PatchMatchOptions::default() + ); + assert_eq!( + VisualIndexOptions::default(), + crate::retrieval::VisualIndexOptions::default() + ); + } + + #[test] + fn import_images_is_unimplemented() { + assert!(matches!( + import_images(Path::new("db.db"), Path::new("images")).unwrap_err(), + crate::Error::Unimplemented(_) + )); + } + + #[test] + fn extract_features_is_unimplemented() { + assert!(matches!( + extract_features(Path::new("db.db"), Path::new("images")).unwrap_err(), + crate::Error::Unimplemented(_) + )); + } + + #[test] + fn match_exhaustive_is_unimplemented() { + assert!(matches!( + match_exhaustive(Path::new("db.db")).unwrap_err(), + crate::Error::Unimplemented(_) + )); + } + + #[test] + fn undistort_images_is_unimplemented() { + assert!(matches!( + undistort_images(Path::new("out"), Path::new("in"), Path::new("images")).unwrap_err(), + crate::Error::Unimplemented(_) + )); + } +} diff --git a/src/retrieval/mod.rs b/src/retrieval/mod.rs new file mode 100644 index 0000000..d3bde7c --- /dev/null +++ b/src/retrieval/mod.rs @@ -0,0 +1,142 @@ +//! Image retrieval via a vocabulary-tree visual index. +//! +//! This module mirrors COLMAP's image-retrieval component (used by PyCOLMAP for +//! vocabulary-tree matching). It exposes [`VisualIndexOptions`] and an opaque +//! [`VisualIndex`] type with `build`/`query` operations. +//! +//! Building and querying a vocabulary tree (hierarchical k-means over SIFT +//! descriptors plus TF-IDF scoring) is numerically heavy and not ported in this +//! pure-Rust crate; the operations return [`crate::Error::Unimplemented`]. + +use crate::feature::FeatureDescriptors; +use crate::types::ImageId; + +/// Options controlling visual-index construction and querying. +/// +/// Mirrors a subset of COLMAP's retrieval options. Defaults match COLMAP's +/// defaults. +#[derive(Debug, Clone, PartialEq)] +#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))] +pub struct VisualIndexOptions { + /// Number of nearest visual words to soft-assign each descriptor to. + pub num_neighbors: i32, + /// Number of most-similar images to return for each query. + pub num_images_to_retrieve: i32, +} + +impl Default for VisualIndexOptions { + fn default() -> Self { + Self { + num_neighbors: 5, + num_images_to_retrieve: 100, + } + } +} + +/// An opaque vocabulary-tree visual index over a database of images. +/// +/// Construct an empty index with [`VisualIndex::new`], populate it from a +/// pretrained vocabulary tree and per-image descriptors with +/// [`build`](VisualIndex::build), then find visually similar images with +/// [`query`](VisualIndex::query). +#[derive(Debug, Default)] +pub struct VisualIndex { + /// Image ids that have been indexed so far. Held so the type carries real + /// state once the algorithm is ported; currently always empty. + indexed_images: Vec, +} + +impl VisualIndex { + /// Create a new, empty visual index. + pub fn new() -> Self { + Self { + indexed_images: Vec::new(), + } + } + + /// Number of images currently indexed. + pub fn len(&self) -> usize { + self.indexed_images.len() + } + + /// Whether the index is empty. + pub fn is_empty(&self) -> bool { + self.indexed_images.is_empty() + } + + /// Add an image's descriptors to the index under the given image id. + /// + /// # Errors + /// + /// Returns [`crate::Error::Unimplemented`]; vocabulary-tree indexing is not + /// ported in this pure-Rust crate. + pub fn build( + &mut self, + image_id: ImageId, + descriptors: &FeatureDescriptors, + options: &VisualIndexOptions, + ) -> crate::Result<()> { + let _ = (image_id, descriptors, options); + Err(crate::Error::Unimplemented("visual-index build")) + } + + /// Retrieve the most visually similar images to the given query descriptors. + /// + /// # Errors + /// + /// Returns [`crate::Error::Unimplemented`]; vocabulary-tree querying is not + /// ported in this pure-Rust crate. + pub fn query( + &self, + descriptors: &FeatureDescriptors, + options: &VisualIndexOptions, + ) -> crate::Result> { + let _ = (descriptors, options); + Err(crate::Error::Unimplemented("visual-index query")) + } +} + +#[cfg(test)] +mod tests { + use super::*; + + fn empty_descriptors() -> FeatureDescriptors { + FeatureDescriptors { + rows: 0, + cols: 128, + data: Vec::new(), + } + } + + #[test] + fn options_defaults() { + let o = VisualIndexOptions::default(); + assert_eq!(o.num_neighbors, 5); + assert_eq!(o.num_images_to_retrieve, 100); + } + + #[test] + fn new_index_is_empty() { + let idx = VisualIndex::new(); + assert!(idx.is_empty()); + assert_eq!(idx.len(), 0); + } + + #[test] + fn build_is_unimplemented() { + let mut idx = VisualIndex::new(); + let err = idx + .build(1, &empty_descriptors(), &VisualIndexOptions::default()) + .unwrap_err(); + assert!(matches!(err, crate::Error::Unimplemented(_))); + } + + #[test] + fn query_is_unimplemented() { + let idx = VisualIndex::new(); + let err = idx + .query(&empty_descriptors(), &VisualIndexOptions::default()) + .unwrap_err(); + assert!(matches!(err, crate::Error::Unimplemented(_))); + } +} diff --git a/src/scene/camera.rs b/src/scene/camera.rs new file mode 100644 index 0000000..79e45f6 --- /dev/null +++ b/src/scene/camera.rs @@ -0,0 +1,320 @@ +//! The [`Camera`] data type: intrinsic calibration shared by one or more images. +//! +//! Mirrors COLMAP's `colmap/scene/camera.h`. A camera stores its +//! [`CameraModelId`], image dimensions and the flat parameter vector whose layout +//! is dictated by the model (see [`CameraModelId::focal_length_idxs`] and friends). +//! Projection is delegated to the [`crate::sensor`] camera-model functions. + +use crate::math::{Mat3, Vec2, Vec3}; +use crate::types::{CameraId, CameraModelId, Sensor, INVALID_CAMERA_ID}; + +/// Intrinsic calibration of a physical camera, shared by all of its images. +/// +/// The [`params`](Camera::params) vector is interpreted according to +/// [`model_id`](Camera::model_id): use [`CameraModelId::focal_length_idxs`], +/// [`CameraModelId::principal_point_idxs`] and [`CameraModelId::extra_params_idxs`] +/// to address individual quantities. +#[derive(Debug, Clone, PartialEq)] +#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))] +pub struct Camera { + /// Unique identifier of this camera. + pub camera_id: CameraId, + /// The camera model that interprets [`params`](Camera::params). + pub model_id: CameraModelId, + /// Sensor width in pixels. + pub width: u64, + /// Sensor height in pixels. + pub height: u64, + /// Flat intrinsic parameters, laid out per [`model_id`](Camera::model_id). + pub params: Vec, + /// Whether the focal length was provided as a prior (e.g. from EXIF). + pub has_prior_focal_length: bool, +} + +impl Default for Camera { + /// An invalid, parameter-less `SIMPLE_PINHOLE` camera. + #[inline] + fn default() -> Self { + Self { + camera_id: INVALID_CAMERA_ID, + model_id: CameraModelId::SimplePinhole, + width: 0, + height: 0, + params: Vec::new(), + has_prior_focal_length: false, + } + } +} + +impl Camera { + /// Builds a camera of the given model with a single focal length. + /// + /// The focal-length indices are all set to `focal_length`, the principal point + /// is placed at the image center (`width / 2`, `height / 2`) and every extra + /// (distortion) parameter is initialized to `0.0`. + pub fn new_with_model( + camera_id: CameraId, + model_id: CameraModelId, + focal_length: f64, + width: u64, + height: u64, + ) -> Self { + let mut params = vec![0.0_f64; model_id.num_params()]; + for &i in model_id.focal_length_idxs() { + params[i] = focal_length; + } + let pp = model_id.principal_point_idxs(); + if pp.len() == 2 { + params[pp[0]] = width as f64 / 2.0; + params[pp[1]] = height as f64 / 2.0; + } + Self { + camera_id, + model_id, + width, + height, + params, + has_prior_focal_length: false, + } + } + + /// The model's text name (e.g. `"SIMPLE_RADIAL"`). + #[inline] + pub fn model_name(&self) -> &'static str { + self.model_id.name() + } + + /// The [`Sensor`] identifier of this camera. + #[inline] + pub fn sensor_id(&self) -> Sensor { + Sensor::camera(self.camera_id) + } + + /// The mean of the focal-length parameters. + pub fn focal_length(&self) -> f64 { + let idxs = self.model_id.focal_length_idxs(); + if idxs.is_empty() { + return 0.0; + } + let sum: f64 = idxs.iter().map(|&i| self.params[i]).sum(); + sum / idxs.len() as f64 + } + + /// The horizontal focal length (`fx`), i.e. the first focal parameter. + pub fn focal_length_x(&self) -> f64 { + let idxs = self.model_id.focal_length_idxs(); + self.params[idxs[0]] + } + + /// The vertical focal length (`fy`), i.e. the last focal parameter. + /// + /// For models with a single shared focal length this equals + /// [`focal_length_x`](Camera::focal_length_x). + pub fn focal_length_y(&self) -> f64 { + let idxs = self.model_id.focal_length_idxs(); + self.params[idxs[idxs.len() - 1]] + } + + /// Sets every focal-length parameter to `f`. + pub fn set_focal_length(&mut self, f: f64) { + for &i in self.model_id.focal_length_idxs() { + self.params[i] = f; + } + } + + /// The principal-point x coordinate (`cx`). + pub fn principal_point_x(&self) -> f64 { + self.params[self.model_id.principal_point_idxs()[0]] + } + + /// The principal-point y coordinate (`cy`). + pub fn principal_point_y(&self) -> f64 { + self.params[self.model_id.principal_point_idxs()[1]] + } + + /// Sets the principal point to `(x, y)`. + pub fn set_principal_point(&mut self, x: f64, y: f64) { + let pp = self.model_id.principal_point_idxs(); + self.params[pp[0]] = x; + self.params[pp[1]] = y; + } + + /// The pinhole calibration matrix `K` built from `fx, fy, cx, cy`. + pub fn calibration_matrix(&self) -> Mat3 { + let mut k = Mat3::identity(); + k[(0, 0)] = self.focal_length_x(); + k[(1, 1)] = self.focal_length_y(); + k[(0, 2)] = self.principal_point_x(); + k[(1, 2)] = self.principal_point_y(); + k + } + + /// Maps an image-plane point to normalized camera coordinates (undistort). + /// + /// Delegates to [`crate::sensor::cam_from_img`]. + #[inline] + pub fn cam_from_img(&self, xy: &Vec2) -> Option { + crate::sensor::cam_from_img(self.model_id, &self.params, xy) + } + + /// Projects a 3D camera-frame point into image-plane (pixel) coordinates. + /// + /// Delegates to [`crate::sensor::img_from_cam`]. + #[inline] + pub fn img_from_cam(&self, xyz: &Vec3) -> Option { + crate::sensor::img_from_cam(self.model_id, &self.params, xyz) + } + + /// Returns the (unnormalized) viewing ray through an image point, in the + /// camera frame: the undistorted point lifted to `z = 1`. + pub fn cam_ray_from_img(&self, xy: &Vec2) -> Option { + self.cam_from_img(xy) + .map(|p| Vec3::new(p.x, p.y, 1.0)) + } + + /// Returns `true` if the number of parameters matches the model. + #[inline] + pub fn verify_params(&self) -> bool { + self.params.len() == self.model_id.num_params() + } + + /// Serializes the parameters as a comma-separated string. + pub fn params_to_string(&self) -> String { + self.params + .iter() + .map(|v| v.to_string()) + .collect::>() + .join(", ") + } + + /// Parses the parameters from a comma- (or space-) separated string, + /// replacing the current ones. + /// + /// Returns [`crate::Error::InvalidParams`] if a token is not a valid number. + pub fn set_params_from_string(&mut self, s: &str) -> crate::Result<()> { + let mut params = Vec::new(); + for tok in s.split([',', ' ', '\t']).filter(|t| !t.is_empty()) { + let v: f64 = tok + .trim() + .parse() + .map_err(|_| crate::Error::InvalidParams(format!("invalid number: {tok:?}")))?; + params.push(v); + } + self.params = params; + Ok(()) + } + + /// Rescales the camera to a new image size, scaling the intrinsics so that the + /// field of view is preserved. + /// + /// Focal lengths are scaled by the geometric-style per-axis factor and the + /// principal point is scaled by the same factors. + pub fn rescale(&mut self, new_width: u64, new_height: u64) { + if self.width == 0 || self.height == 0 { + self.width = new_width; + self.height = new_height; + return; + } + let scale_x = new_width as f64 / self.width as f64; + let scale_y = new_height as f64 / self.height as f64; + + let focal_idxs = self.model_id.focal_length_idxs(); + if focal_idxs.len() == 1 { + // Single shared focal length: scale by the mean factor. + let s = (scale_x + scale_y) / 2.0; + self.params[focal_idxs[0]] *= s; + } else { + self.params[focal_idxs[0]] *= scale_x; + self.params[focal_idxs[1]] *= scale_y; + } + let pp = self.model_id.principal_point_idxs(); + self.params[pp[0]] *= scale_x; + self.params[pp[1]] *= scale_y; + + self.width = new_width; + self.height = new_height; + } +} + +#[cfg(test)] +mod tests { + use super::*; + use approx::assert_relative_eq; + + #[test] + fn new_with_model_simple_pinhole_layout() { + let cam = Camera::new_with_model(1, CameraModelId::SimplePinhole, 100.0, 640, 480); + assert!(cam.verify_params()); + assert_eq!(cam.params.len(), 3); + assert_relative_eq!(cam.focal_length(), 100.0); + assert_relative_eq!(cam.principal_point_x(), 320.0); + assert_relative_eq!(cam.principal_point_y(), 240.0); + assert_eq!(cam.model_name(), "SIMPLE_PINHOLE"); + assert_eq!(cam.sensor_id(), Sensor::camera(1)); + } + + #[test] + fn pinhole_two_focals() { + let mut cam = Camera::new_with_model(2, CameraModelId::Pinhole, 120.0, 800, 600); + assert_eq!(cam.params.len(), 4); + assert_relative_eq!(cam.focal_length_x(), 120.0); + assert_relative_eq!(cam.focal_length_y(), 120.0); + cam.set_focal_length(200.0); + assert_relative_eq!(cam.focal_length(), 200.0); + cam.set_principal_point(10.0, 20.0); + assert_relative_eq!(cam.principal_point_x(), 10.0); + assert_relative_eq!(cam.principal_point_y(), 20.0); + } + + #[test] + fn calibration_matrix_is_k() { + let cam = Camera::new_with_model(1, CameraModelId::Pinhole, 100.0, 640, 480); + let k = cam.calibration_matrix(); + assert_relative_eq!(k[(0, 0)], 100.0); + assert_relative_eq!(k[(1, 1)], 100.0); + assert_relative_eq!(k[(0, 2)], 320.0); + assert_relative_eq!(k[(1, 2)], 240.0); + assert_relative_eq!(k[(2, 2)], 1.0); + } + + #[test] + fn params_string_roundtrip() { + let mut cam = Camera::new_with_model(1, CameraModelId::Radial, 100.0, 640, 480); + let s = cam.params_to_string(); + let mut cam2 = cam.clone(); + cam2.params = vec![]; + cam2.set_params_from_string(&s).unwrap(); + assert_eq!(cam.params, cam2.params); + assert!(cam.set_params_from_string("1.0, x, 3.0").is_err()); + } + + #[test] + fn rescale_scales_intrinsics() { + let mut cam = Camera::new_with_model(1, CameraModelId::Pinhole, 100.0, 640, 480); + cam.rescale(320, 240); + assert_eq!(cam.width, 320); + assert_eq!(cam.height, 240); + assert_relative_eq!(cam.focal_length_x(), 50.0); + assert_relative_eq!(cam.principal_point_x(), 160.0); + assert_relative_eq!(cam.principal_point_y(), 120.0); + } + + #[test] + fn default_is_invalid() { + let cam = Camera::default(); + assert_eq!(cam.camera_id, INVALID_CAMERA_ID); + assert!(cam.params.is_empty()); + assert!(!cam.verify_params()); + } + + #[test] + fn cam_ray_lifts_to_unit_z() { + // SIMPLE_PINHOLE undistort is closed form; ray z component must be 1. + let cam = Camera::new_with_model(1, CameraModelId::SimplePinhole, 100.0, 640, 480); + if let Some(ray) = cam.cam_ray_from_img(&Vec2::new(320.0, 240.0)) { + assert_relative_eq!(ray.z, 1.0); + assert_relative_eq!(ray.x, 0.0, epsilon = 1e-9); + assert_relative_eq!(ray.y, 0.0, epsilon = 1e-9); + } + } +} diff --git a/src/scene/correspondence_graph.rs b/src/scene/correspondence_graph.rs new file mode 100644 index 0000000..49a1699 --- /dev/null +++ b/src/scene/correspondence_graph.rs @@ -0,0 +1,164 @@ +//! The [`CorrespondenceGraph`]: per-feature correspondences across images. +//! +//! Mirrors a usable subset of COLMAP's `colmap/scene/correspondence_graph.h`. For +//! every image and every 2D point it stores the list of corresponding observations +//! in other images, as derived from the verified feature matches. + +use crate::feature::FeatureMatches; +use crate::types::{ImageId, Point2DIdx}; +use std::collections::HashMap; + +/// A single correspondence: a 2D point in some image. +#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)] +#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))] +pub struct Correspondence { + /// The image containing the corresponding observation. + pub image_id: ImageId, + /// The index of the 2D point within that image. + pub point2d_idx: Point2DIdx, +} + +impl Correspondence { + /// Creates a correspondence to 2D point `point2d_idx` in `image_id`. + #[inline] + pub fn new(image_id: ImageId, point2d_idx: Point2DIdx) -> Self { + Self { + image_id, + point2d_idx, + } + } +} + +/// A graph linking corresponding feature observations across images. +/// +/// For each registered image it holds a vector indexed by 2D point index; each +/// entry is the list of [`Correspondence`]s of that feature in other images. +#[derive(Debug, Clone, Default)] +#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))] +pub struct CorrespondenceGraph { + /// Per-image, per-2D-point correspondence lists. + images: HashMap>>, +} + +impl CorrespondenceGraph { + /// Creates an empty correspondence graph. + pub fn new() -> Self { + Self { + images: HashMap::new(), + } + } + + /// Registers an image with `num_points2d` features and no correspondences yet. + pub fn add_image(&mut self, image_id: ImageId, num_points2d: usize) { + self.images.insert(image_id, vec![Vec::new(); num_points2d]); + } + + /// Returns `true` if the image is known to the graph. + #[inline] + pub fn has_image(&self, image_id: ImageId) -> bool { + self.images.contains_key(&image_id) + } + + /// Adds the symmetric correspondences implied by the matches between two images. + /// + /// Both images must already have been added via [`add_image`](Self::add_image). + /// Out-of-range point indices are ignored so partially-specified inputs do not + /// panic. + pub fn add_correspondences( + &mut self, + image_id1: ImageId, + image_id2: ImageId, + matches: &FeatureMatches, + ) { + for m in matches { + let i1 = m.point2d_idx1 as usize; + let i2 = m.point2d_idx2 as usize; + + let in_range1 = self + .images + .get(&image_id1) + .is_some_and(|v| i1 < v.len()); + let in_range2 = self + .images + .get(&image_id2) + .is_some_and(|v| i2 < v.len()); + if !in_range1 || !in_range2 { + continue; + } + + if let Some(v) = self.images.get_mut(&image_id1) { + v[i1].push(Correspondence::new(image_id2, m.point2d_idx2)); + } + if let Some(v) = self.images.get_mut(&image_id2) { + v[i2].push(Correspondence::new(image_id1, m.point2d_idx1)); + } + } + } + + /// The total number of correspondences recorded for an image (summed over all + /// of its 2D points). + pub fn num_correspondences_for_image(&self, image_id: ImageId) -> usize { + self.images + .get(&image_id) + .map(|v| v.iter().map(Vec::len).sum()) + .unwrap_or(0) + } + + /// The correspondences of a single 2D point in an image. + /// + /// Returns an empty slice if the image or point index is unknown. + pub fn find_correspondences( + &self, + image_id: ImageId, + point2d_idx: Point2DIdx, + ) -> &[Correspondence] { + self.images + .get(&image_id) + .and_then(|v| v.get(point2d_idx as usize)) + .map(Vec::as_slice) + .unwrap_or(&[]) + } +} + +#[cfg(test)] +mod tests { + use super::*; + use crate::feature::FeatureMatch; + + #[test] + fn add_and_query() { + let mut g = CorrespondenceGraph::new(); + g.add_image(1, 3); + g.add_image(2, 2); + assert!(g.has_image(1)); + assert!(!g.has_image(9)); + + let matches = vec![FeatureMatch::new(0, 1), FeatureMatch::new(2, 0)]; + g.add_correspondences(1, 2, &matches); + + // Image 1 has two features with correspondences (indices 0 and 2). + assert_eq!(g.num_correspondences_for_image(1), 2); + assert_eq!(g.num_correspondences_for_image(2), 2); + + let c = g.find_correspondences(1, 0); + assert_eq!(c.len(), 1); + assert_eq!(c[0], Correspondence::new(2, 1)); + + let c2 = g.find_correspondences(2, 1); + assert_eq!(c2.len(), 1); + assert_eq!(c2[0], Correspondence::new(1, 0)); + + // Unknown lookups are empty, not panics. + assert!(g.find_correspondences(1, 99).is_empty()); + assert!(g.find_correspondences(42, 0).is_empty()); + } + + #[test] + fn out_of_range_matches_ignored() { + let mut g = CorrespondenceGraph::new(); + g.add_image(1, 1); + g.add_image(2, 1); + g.add_correspondences(1, 2, &vec![FeatureMatch::new(5, 5)]); + assert_eq!(g.num_correspondences_for_image(1), 0); + } +} diff --git a/src/scene/frame.rs b/src/scene/frame.rs new file mode 100644 index 0000000..9cf4166 --- /dev/null +++ b/src/scene/frame.rs @@ -0,0 +1,106 @@ +//! The [`Frame`] data type: one synchronized capture of a [`Rig`](crate::scene::Rig). +//! +//! Mirrors COLMAP's `colmap/scene/frame.h`. A frame groups the data captured by a +//! rig at one instant (e.g. several images from a multi-camera rig) and carries the +//! `rig_from_world` pose of the rig frame. + +use crate::geometry::Rigid3d; +use crate::types::{ + DataId, FrameId, ImageId, RigId, SensorType, INVALID_FRAME_ID, INVALID_RIG_ID, +}; +use std::collections::BTreeSet; + +/// One synchronized capture of a rig: the set of per-sensor measurements plus the +/// rig's world pose. +#[derive(Debug, Clone, PartialEq)] +#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))] +pub struct Frame { + /// Unique identifier of this frame. + pub frame_id: FrameId, + /// The rig that produced this frame. + pub rig_id: RigId, + /// The `rig_from_world` pose, or `None` if the frame is not registered. + pub rig_from_world: Option, + /// The data ids (one per measuring sensor) captured in this frame. + pub data_ids: BTreeSet, +} + +impl Default for Frame { + /// An invalid, unregistered frame with no data. + #[inline] + fn default() -> Self { + Self { + frame_id: INVALID_FRAME_ID, + rig_id: INVALID_RIG_ID, + rig_from_world: None, + data_ids: BTreeSet::new(), + } + } +} + +impl Frame { + /// Creates a new, unregistered frame for the given rig. + pub fn new(frame_id: FrameId, rig_id: RigId) -> Self { + Self { + frame_id, + rig_id, + rig_from_world: None, + data_ids: BTreeSet::new(), + } + } + + /// Returns `true` if the frame has a registered `rig_from_world` pose. + #[inline] + pub fn has_pose(&self) -> bool { + self.rig_from_world.is_some() + } + + /// Adds a data measurement to this frame. + #[inline] + pub fn add_data_id(&mut self, data_id: DataId) { + self.data_ids.insert(data_id); + } + + /// The ids of the images in this frame (data ids from camera sensors). + pub fn image_ids(&self) -> Vec { + self.data_ids + .iter() + .filter(|d| d.sensor.sensor_type == SensorType::Camera) + .map(|d| d.id as ImageId) + .collect() + } +} + +#[cfg(test)] +mod tests { + use super::*; + use crate::types::Sensor; + + #[test] + fn frame_image_ids() { + let mut frame = Frame::new(1, 2); + assert!(!frame.has_pose()); + frame.add_data_id(DataId::new(Sensor::camera(5), 100)); + frame.add_data_id(DataId::new(Sensor::camera(6), 101)); + frame.add_data_id(DataId::new(Sensor::new(SensorType::Imu, 7), 200)); + + let mut ids = frame.image_ids(); + ids.sort_unstable(); + assert_eq!(ids, vec![100, 101]); + } + + #[test] + fn pose_flag() { + let mut frame = Frame::new(1, 1); + frame.rig_from_world = Some(Rigid3d::identity()); + assert!(frame.has_pose()); + } + + #[test] + fn default_is_invalid() { + let frame = Frame::default(); + assert_eq!(frame.frame_id, INVALID_FRAME_ID); + assert_eq!(frame.rig_id, INVALID_RIG_ID); + assert!(frame.data_ids.is_empty()); + } +} diff --git a/src/scene/image.rs b/src/scene/image.rs new file mode 100644 index 0000000..bf4ce45 --- /dev/null +++ b/src/scene/image.rs @@ -0,0 +1,161 @@ +//! The [`Image`] data type: one registered or unregistered camera shot. +//! +//! Mirrors COLMAP's `colmap/scene/image.h`. An image references the [`Camera`] +//! that took it, owns its list of 2D observations and, once registered, carries +//! the `cam_from_world` pose that maps world points into its camera frame. + +use crate::geometry::Rigid3d; +use crate::math::{Vec2, Vec3}; +use crate::scene::{Camera, Point2D}; +use crate::types::{CameraId, FrameId, ImageId, INVALID_CAMERA_ID, INVALID_IMAGE_ID}; + +/// A single image: its 2D features and, when registered, its world pose. +#[derive(Debug, Clone, PartialEq)] +#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))] +pub struct Image { + /// Unique identifier of this image. + pub image_id: ImageId, + /// The camera (intrinsics) that captured this image. + pub camera_id: CameraId, + /// The image file name (relative path). + pub name: String, + /// The `cam_from_world` pose, or `None` if the image is not registered. + pub cam_from_world: Option, + /// The 2D observations detected in this image. + pub points2d: Vec, + /// The id of the frame this image belongs to, if any. + pub frame_id: Option, +} + +impl Default for Image { + /// An invalid, nameless, unregistered image with no observations. + #[inline] + fn default() -> Self { + Self { + image_id: INVALID_IMAGE_ID, + camera_id: INVALID_CAMERA_ID, + name: String::new(), + cam_from_world: None, + points2d: Vec::new(), + frame_id: None, + } + } +} + +impl Image { + /// Creates a new, unregistered image with the given id, camera and name. + pub fn new(image_id: ImageId, camera_id: CameraId, name: impl Into) -> Self { + Self { + image_id, + camera_id, + name: name.into(), + cam_from_world: None, + points2d: Vec::new(), + frame_id: None, + } + } + + /// Returns `true` if the image has a registered `cam_from_world` pose. + #[inline] + pub fn has_pose(&self) -> bool { + self.cam_from_world.is_some() + } + + /// The camera's projection center in world coordinates, if registered. + #[inline] + pub fn projection_center(&self) -> Option { + self.cam_from_world.map(|p| p.target_origin_in_source()) + } + + /// The viewing direction (optical axis) in world coordinates, if registered. + /// + /// This is `rotation⁻¹ * +Z`, i.e. the third row of the rotation matrix `R`. + #[inline] + pub fn viewing_direction(&self) -> Option { + self.cam_from_world + .map(|p| p.rotation.inverse() * Vec3::new(0.0, 0.0, 1.0)) + } + + /// The number of 2D observations in this image. + #[inline] + pub fn num_points2d(&self) -> usize { + self.points2d.len() + } + + /// The number of 2D observations linked to a valid 3D point. + #[inline] + pub fn num_points3d(&self) -> usize { + self.points2d.iter().filter(|p| p.has_point3d()).count() + } + + /// Projects a world point into this image, returning pixel coordinates. + /// + /// Transforms `xyz` by `cam_from_world` and then projects with `camera`. + /// Returns `None` if the image has no pose or the projection fails (e.g. the + /// point is behind the camera). + pub fn project_point(&self, camera: &Camera, xyz: &Vec3) -> Option { + let pose = self.cam_from_world?; + let point_in_cam = pose.transform_point(xyz); + camera.img_from_cam(&point_in_cam) + } + + /// Replaces the 2D observations with fresh, unlinked points at the given + /// coordinates. + pub fn set_points2d_from_xy(&mut self, xy: &[Vec2]) { + self.points2d = xy.iter().map(|p| Point2D::new(*p)).collect(); + } +} + +#[cfg(test)] +mod tests { + use super::*; + use crate::math::UnitQuat; + use approx::assert_relative_eq; + + #[test] + fn new_image_unregistered() { + let img = Image::new(1, 2, "frame.jpg"); + assert!(!img.has_pose()); + assert_eq!(img.projection_center(), None); + assert_eq!(img.viewing_direction(), None); + assert_eq!(img.name, "frame.jpg"); + assert_eq!(img.num_points2d(), 0); + } + + #[test] + fn projection_center_matches_pose() { + let mut img = Image::new(1, 1, "a.jpg"); + let pose = Rigid3d::new(UnitQuat::identity(), Vec3::new(1.0, 2.0, 3.0)); + img.cam_from_world = Some(pose); + // For identity rotation, center = -translation. + assert_relative_eq!( + img.projection_center().unwrap(), + Vec3::new(-1.0, -2.0, -3.0), + epsilon = 1e-12 + ); + // Identity rotation viewing direction is +Z. + assert_relative_eq!( + img.viewing_direction().unwrap(), + Vec3::new(0.0, 0.0, 1.0), + epsilon = 1e-12 + ); + } + + #[test] + fn point3d_counting() { + let mut img = Image::new(1, 1, "a.jpg"); + img.set_points2d_from_xy(&[Vec2::new(0.0, 0.0), Vec2::new(1.0, 1.0)]); + assert_eq!(img.num_points2d(), 2); + assert_eq!(img.num_points3d(), 0); + img.points2d[1].point3d_id = 7; + assert_eq!(img.num_points3d(), 1); + } + + #[test] + fn default_is_invalid() { + let img = Image::default(); + assert_eq!(img.image_id, INVALID_IMAGE_ID); + assert!(!img.has_pose()); + assert!(img.name.is_empty()); + } +} diff --git a/src/scene/mod.rs b/src/scene/mod.rs new file mode 100644 index 0000000..be2dde4 --- /dev/null +++ b/src/scene/mod.rs @@ -0,0 +1,43 @@ +//! The scene data model: the in-memory representation of a COLMAP reconstruction. +//! +//! Mirrors COLMAP's `colmap/scene/*`. This module defines the core data types of +//! the sparse model and the [`Reconstruction`] container that owns them: +//! +//! | Type | Role | +//! |--------------------------|-------------------------------------------------| +//! | [`Camera`] | intrinsic calibration shared by images | +//! | [`Point2D`] | a 2D feature observation in an image | +//! | [`Track`] / [`TrackElement`] | the observations of one 3D point | +//! | [`Point3D`] | a triangulated point with color and error | +//! | [`Image`] | a (possibly registered) image with a pose | +//! | [`Rig`] | a set of rigidly-mounted sensors | +//! | [`Frame`] | one synchronized rig capture | +//! | [`TwoViewGeometry`] | the verified relation of an image pair | +//! | [`CorrespondenceGraph`] | per-feature correspondences across images | +//! | [`Reconstruction`] | the complete sparse model | +//! +//! Field layouts follow the frozen data-type contract exactly so that the +//! [`feature`](crate::feature), [`io`](crate::io) and estimator modules can share +//! them without conversion. + +mod camera; +mod correspondence_graph; +mod frame; +mod image; +mod point2d; +mod point3d; +mod reconstruction; +mod rig; +mod track; +mod two_view_geometry; + +pub use camera::Camera; +pub use correspondence_graph::{Correspondence, CorrespondenceGraph}; +pub use frame::Frame; +pub use image::Image; +pub use point2d::Point2D; +pub use point3d::Point3D; +pub use reconstruction::Reconstruction; +pub use rig::Rig; +pub use track::{Track, TrackElement}; +pub use two_view_geometry::{TwoViewGeometry, TwoViewGeometryConfig}; diff --git a/src/scene/point2d.rs b/src/scene/point2d.rs new file mode 100644 index 0000000..f619dd5 --- /dev/null +++ b/src/scene/point2d.rs @@ -0,0 +1,72 @@ +//! The [`Point2D`] data type: an observed 2D feature in an image. +//! +//! Mirrors COLMAP's `colmap/scene/point2d.h`. A 2D point stores its pixel +//! location and, when it has been triangulated, the id of the 3D point it +//! observes (otherwise [`INVALID_POINT3D_ID`]). + +use crate::math::Vec2; +use crate::types::{Point3DId, INVALID_POINT3D_ID}; + +/// A 2D observation in an image, optionally linked to a triangulated 3D point. +#[derive(Debug, Clone, Copy, PartialEq)] +#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))] +pub struct Point2D { + /// The pixel coordinates of the observation. + pub xy: Vec2, + /// The id of the observed 3D point, or [`INVALID_POINT3D_ID`] if none. + pub point3d_id: Point3DId, +} + +impl Default for Point2D { + /// A point at the origin with no associated 3D point. + #[inline] + fn default() -> Self { + Self { + xy: Vec2::zeros(), + point3d_id: INVALID_POINT3D_ID, + } + } +} + +impl Point2D { + /// Creates a 2D point at `xy` that is not yet linked to a 3D point. + #[inline] + pub fn new(xy: Vec2) -> Self { + Self { + xy, + point3d_id: INVALID_POINT3D_ID, + } + } + + /// Returns `true` if this observation is linked to a valid 3D point. + #[inline] + pub fn has_point3d(&self) -> bool { + self.point3d_id != INVALID_POINT3D_ID + } +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn new_has_no_point3d() { + let p = Point2D::new(Vec2::new(1.0, 2.0)); + assert!(!p.has_point3d()); + assert_eq!(p.xy, Vec2::new(1.0, 2.0)); + } + + #[test] + fn default_is_unlinked_origin() { + let p = Point2D::default(); + assert_eq!(p.xy, Vec2::zeros()); + assert!(!p.has_point3d()); + } + + #[test] + fn linking_sets_flag() { + let mut p = Point2D::new(Vec2::new(0.0, 0.0)); + p.point3d_id = 42; + assert!(p.has_point3d()); + } +} diff --git a/src/scene/point3d.rs b/src/scene/point3d.rs new file mode 100644 index 0000000..6590610 --- /dev/null +++ b/src/scene/point3d.rs @@ -0,0 +1,65 @@ +//! The [`Point3D`] data type: a triangulated point in the reconstruction. +//! +//! Mirrors COLMAP's `colmap/scene/point3d.h`. A 3D point stores its world +//! coordinates, an RGB color, the mean reprojection error (negative when unset) +//! and the [`Track`] of image observations it was triangulated from. + +use crate::math::{Rgb, Vec3}; +use crate::scene::Track; + +/// A triangulated 3D point with color, reprojection error and observation track. +#[derive(Debug, Clone, PartialEq)] +#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))] +pub struct Point3D { + /// World-space coordinates. + pub xyz: Vec3, + /// RGB color, one byte per channel. + pub color: Rgb, + /// Mean reprojection error in pixels; a negative value means "unset". + pub error: f64, + /// The track of 2D observations this point was triangulated from. + pub track: Track, +} + +impl Default for Point3D { + /// A black point at the origin with an unset error and an empty track. + #[inline] + fn default() -> Self { + Self { + xyz: Vec3::zeros(), + color: Rgb::zeros(), + error: -1.0, + track: Track::new(), + } + } +} + +impl Point3D { + /// Returns `true` if a reprojection error has been computed (`error >= 0`). + #[inline] + pub fn has_error(&self) -> bool { + self.error >= 0.0 + } +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn default_has_no_error() { + let p = Point3D::default(); + assert!(!p.has_error()); + assert_eq!(p.xyz, Vec3::zeros()); + assert_eq!(p.color, Rgb::zeros()); + assert!(p.track.is_empty()); + } + + #[test] + fn set_error_flags() { + let mut p = Point3D { error: 0.5, ..Default::default() }; + assert!(p.has_error()); + p.error = 0.0; + assert!(p.has_error()); + } +} diff --git a/src/scene/reconstruction.rs b/src/scene/reconstruction.rs new file mode 100644 index 0000000..7387ae0 --- /dev/null +++ b/src/scene/reconstruction.rs @@ -0,0 +1,347 @@ +//! The [`Reconstruction`]: the complete sparse model produced by SfM. +//! +//! Mirrors COLMAP's `colmap/scene/reconstruction.h`. It owns the rigs, cameras, +//! frames, images and 3D points of one model and offers the bookkeeping and +//! aggregate statistics the rest of the pipeline relies on. File I/O is delegated +//! to [`crate::io`]. + +use crate::geometry::Sim3d; +use crate::math::{Rgb, Vec3}; +use crate::scene::{Camera, Frame, Image, Point3D, Rig, Track}; +use crate::types::{ + CameraId, FrameId, ImageId, Point3DId, RigId, INVALID_POINT3D_ID, +}; +use std::collections::HashMap; +use std::path::Path; + +/// The complete sparse reconstruction: rigs, cameras, frames, images and points. +#[derive(Debug, Clone, Default)] +#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))] +pub struct Reconstruction { + /// All sensor rigs, keyed by [`RigId`]. + pub rigs: HashMap, + /// All cameras (intrinsics), keyed by [`CameraId`]. + pub cameras: HashMap, + /// All frames (rig captures), keyed by [`FrameId`]. + pub frames: HashMap, + /// All images, keyed by [`ImageId`]. + pub images: HashMap, + /// All triangulated 3D points, keyed by [`Point3DId`]. + pub points3d: HashMap, +} + +impl Reconstruction { + /// Creates an empty reconstruction. + #[inline] + pub fn new() -> Self { + Self::default() + } + + /// The number of cameras. + #[inline] + pub fn num_cameras(&self) -> usize { + self.cameras.len() + } + + /// The number of images. + #[inline] + pub fn num_images(&self) -> usize { + self.images.len() + } + + /// The number of 3D points. + #[inline] + pub fn num_points3d(&self) -> usize { + self.points3d.len() + } + + /// The number of rigs. + #[inline] + pub fn num_rigs(&self) -> usize { + self.rigs.len() + } + + /// The number of frames. + #[inline] + pub fn num_frames(&self) -> usize { + self.frames.len() + } + + /// Inserts (or replaces) a camera. + #[inline] + pub fn add_camera(&mut self, camera: Camera) { + self.cameras.insert(camera.camera_id, camera); + } + + /// Inserts (or replaces) an image. + #[inline] + pub fn add_image(&mut self, image: Image) { + self.images.insert(image.image_id, image); + } + + /// Inserts (or replaces) a rig. + #[inline] + pub fn add_rig(&mut self, rig: Rig) { + self.rigs.insert(rig.rig_id, rig); + } + + /// Inserts (or replaces) a frame. + #[inline] + pub fn add_frame(&mut self, frame: Frame) { + self.frames.insert(frame.frame_id, frame); + } + + /// Adds a new 3D point, auto-assigning an id one greater than the current + /// maximum (starting at `0` for the first point). Returns the new id. + pub fn add_point3d(&mut self, xyz: Vec3, track: Track, color: Rgb) -> Point3DId { + let id = self + .points3d + .keys() + .copied() + .filter(|&k| k != INVALID_POINT3D_ID) + .max() + .map_or(0, |m| m + 1); + let point = Point3D { + xyz, + color, + error: -1.0, + track, + }; + self.points3d.insert(id, point); + id + } + + /// The ids of all registered (posed) images. + pub fn reg_image_ids(&self) -> Vec { + self.images + .values() + .filter(|img| img.has_pose()) + .map(|img| img.image_id) + .collect() + } + + /// The ids of all 3D points. + pub fn point3d_ids(&self) -> Vec { + self.points3d.keys().copied().collect() + } + + /// Looks up an image by id. + #[inline] + pub fn image(&self, id: ImageId) -> Option<&Image> { + self.images.get(&id) + } + + /// Looks up a camera by id. + #[inline] + pub fn camera(&self, id: CameraId) -> Option<&Camera> { + self.cameras.get(&id) + } + + /// Looks up a 3D point by id. + #[inline] + pub fn point3d(&self, id: Point3DId) -> Option<&Point3D> { + self.points3d.get(&id) + } + + /// Finds the first image with the given file name. + pub fn find_image_with_name(&self, name: &str) -> Option<&Image> { + self.images.values().find(|img| img.name == name) + } + + /// The total number of image observations across all 3D point tracks. + pub fn compute_num_observations(&self) -> usize { + self.points3d.values().map(|p| p.track.length()).sum() + } + + /// The mean track length (observations per 3D point), or `0.0` if empty. + pub fn compute_mean_track_length(&self) -> f64 { + if self.points3d.is_empty() { + return 0.0; + } + self.compute_num_observations() as f64 / self.points3d.len() as f64 + } + + /// The mean reprojection error over all 3D points that have one set. + pub fn compute_mean_reprojection_error(&self) -> f64 { + let mut sum = 0.0; + let mut count = 0usize; + for p in self.points3d.values() { + if p.has_error() { + sum += p.error; + count += 1; + } + } + if count == 0 { + 0.0 + } else { + sum / count as f64 + } + } + + /// Applies a similarity transform to all image poses and 3D points. + /// + /// Points are transformed directly. An image pose `cam_from_world` is updated + /// so that it still maps world points to the same camera-frame points after the + /// world has been transformed by `transform`. + pub fn transform(&mut self, transform: &Sim3d) { + // New points: p' = T(p). + for point in self.points3d.values_mut() { + point.xyz = transform.transform_point(&point.xyz); + } + // New pose: cam_from_new_world = cam_from_world * world_from_new_world, + // where world_from_new_world is the inverse similarity (rotation/translation + // only; the scale is absorbed into the camera-frame metric, which COLMAP + // keeps by re-deriving the rigid part). + let inv = transform.inverse(); + let inv_rotation = inv.rotation; + let inv_translation = inv.translation; + for image in self.images.values_mut() { + if let Some(pose) = image.cam_from_world { + // world point p relates to new-world point p' by p = inv(p'). + // cam = R*p + t = R*(s_inv*R_inv*p' + t_inv) + t. + // Keep the rigid composition (the scale folds into translation). + let new_rotation = pose.rotation * inv_rotation; + let new_translation = + pose.rotation * (inv.scale * inv_translation) + pose.translation; + image.cam_from_world = Some(crate::geometry::Rigid3d::new( + new_rotation, + new_translation, + )); + } + } + } + + // --------------------------------------------------------------------- + // I/O delegates (implemented by `crate::io`). + // --------------------------------------------------------------------- + + /// Reads a model from `path`, auto-detecting the text or binary format. + pub fn read(path: &Path) -> crate::Result { + crate::io::read_model(path) + } + + /// Writes this model to `path` (binary format). + pub fn write(&self, path: &Path) -> crate::Result<()> { + crate::io::write_model(self, path) + } + + /// Reads a model from `path` in the text format. + pub fn read_text(path: &Path) -> crate::Result { + crate::io::read_model_text(path) + } + + /// Reads a model from `path` in the binary format. + pub fn read_binary(path: &Path) -> crate::Result { + crate::io::read_model_binary(path) + } + + /// Writes this model to `path` in the text format. + pub fn write_text(&self, path: &Path) -> crate::Result<()> { + crate::io::write_model_text(self, path) + } + + /// Writes this model to `path` in the binary format. + pub fn write_binary(&self, path: &Path) -> crate::Result<()> { + crate::io::write_model_binary(self, path) + } +} + +#[cfg(test)] +mod tests { + use super::*; + use crate::geometry::Rigid3d; + use crate::math::UnitQuat; + use crate::types::CameraModelId; + use approx::assert_relative_eq; + + fn sample() -> Reconstruction { + let mut rec = Reconstruction::new(); + let cam = Camera::new_with_model(1, CameraModelId::SimplePinhole, 100.0, 640, 480); + rec.add_camera(cam); + + let mut img = Image::new(1, 1, "image1.jpg"); + img.cam_from_world = Some(Rigid3d::new(UnitQuat::identity(), Vec3::new(1.0, 2.0, 3.0))); + rec.add_image(img); + + let mut track = Track::new(); + track.add(1, 0); + rec.add_point3d(Vec3::new(0.0, 0.0, 5.0), track, Rgb::new(10, 20, 30)); + rec + } + + #[test] + fn counts() { + let rec = sample(); + assert_eq!(rec.num_cameras(), 1); + assert_eq!(rec.num_images(), 1); + assert_eq!(rec.num_points3d(), 1); + assert_eq!(rec.num_rigs(), 0); + assert_eq!(rec.num_frames(), 0); + assert_eq!(rec.reg_image_ids(), vec![1]); + assert_eq!(rec.compute_num_observations(), 1); + assert_relative_eq!(rec.compute_mean_track_length(), 1.0); + } + + #[test] + fn lookups() { + let rec = sample(); + assert!(rec.camera(1).is_some()); + assert!(rec.image(1).is_some()); + assert!(rec.point3d(0).is_some()); + assert!(rec.camera(99).is_none()); + assert_eq!( + rec.find_image_with_name("image1.jpg").unwrap().image_id, + 1 + ); + assert!(rec.find_image_with_name("nope.jpg").is_none()); + } + + #[test] + fn projection_center_is_correct() { + let rec = sample(); + let center = rec.image(1).unwrap().projection_center().unwrap(); + // Identity rotation => center = -translation. + assert_relative_eq!(center, Vec3::new(-1.0, -2.0, -3.0), epsilon = 1e-12); + } + + #[test] + fn add_point3d_increments_id() { + let mut rec = Reconstruction::new(); + let id0 = rec.add_point3d(Vec3::zeros(), Track::new(), Rgb::zeros()); + let id1 = rec.add_point3d(Vec3::zeros(), Track::new(), Rgb::zeros()); + let id2 = rec.add_point3d(Vec3::zeros(), Track::new(), Rgb::zeros()); + assert_eq!(id0, 0); + assert_eq!(id1, 1); + assert_eq!(id2, 2); + assert_eq!(rec.point3d_ids().len(), 3); + } + + #[test] + fn transform_identity_is_noop() { + let mut rec = sample(); + let before_pose = rec.image(1).unwrap().cam_from_world.unwrap(); + let before_xyz = rec.point3d(0).unwrap().xyz; + + rec.transform(&Sim3d::identity()); + + let after_pose = rec.image(1).unwrap().cam_from_world.unwrap(); + let after_xyz = rec.point3d(0).unwrap().xyz; + assert_relative_eq!(after_xyz, before_xyz, epsilon = 1e-12); + assert_relative_eq!(after_pose.translation, before_pose.translation, epsilon = 1e-12); + assert_relative_eq!( + after_pose.rotation.into_inner(), + before_pose.rotation.into_inner(), + epsilon = 1e-12 + ); + } + + #[test] + fn mean_reprojection_error_skips_unset() { + let mut rec = Reconstruction::new(); + rec.add_point3d(Vec3::zeros(), Track::new(), Rgb::zeros()); + // No error set => mean is 0.0. + assert_relative_eq!(rec.compute_mean_reprojection_error(), 0.0); + rec.points3d.get_mut(&0).unwrap().error = 2.0; + assert_relative_eq!(rec.compute_mean_reprojection_error(), 2.0); + } +} diff --git a/src/scene/rig.rs b/src/scene/rig.rs new file mode 100644 index 0000000..c1641fb --- /dev/null +++ b/src/scene/rig.rs @@ -0,0 +1,117 @@ +//! The [`Rig`] data type: a fixed set of rigidly-mounted sensors. +//! +//! Mirrors COLMAP's `colmap/scene/rig.h`. A rig has one reference sensor (whose +//! frame defines the rig frame) plus zero or more non-reference sensors, each with +//! an optional `sensor_from_rig` transform. + +use crate::geometry::Rigid3d; +use crate::types::{RigId, Sensor, SensorType, INVALID_RIG_ID}; +use std::collections::BTreeMap; + +/// A rig of rigidly-mounted sensors with a shared reference frame. +#[derive(Debug, Clone, PartialEq)] +#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))] +pub struct Rig { + /// Unique identifier of this rig. + pub rig_id: RigId, + /// The reference sensor; its frame is the rig frame. + pub ref_sensor_id: Sensor, + /// Non-reference sensors and their `sensor_from_rig` transforms. + /// + /// A `None` value marks a sensor whose relative pose is not yet known. + pub sensors_from_rig: BTreeMap>, +} + +impl Default for Rig { + /// An invalid rig with an invalid reference sensor and no other sensors. + #[inline] + fn default() -> Self { + Self { + rig_id: INVALID_RIG_ID, + ref_sensor_id: Sensor::new(SensorType::Invalid, u32::MAX), + sensors_from_rig: BTreeMap::new(), + } + } +} + +impl Rig { + /// Creates a rig with the given id and no sensors yet. + pub fn new(rig_id: RigId) -> Self { + Self { + rig_id, + ..Default::default() + } + } + + /// Sets the reference sensor of the rig. + #[inline] + pub fn add_ref_sensor(&mut self, sensor: Sensor) { + self.ref_sensor_id = sensor; + } + + /// Adds a non-reference sensor with its (optional) `sensor_from_rig` transform. + #[inline] + pub fn add_sensor(&mut self, sensor: Sensor, sensor_from_rig: Option) { + self.sensors_from_rig.insert(sensor, sensor_from_rig); + } + + /// The total number of sensors in the rig (reference plus non-reference). + #[inline] + pub fn num_sensors(&self) -> usize { + 1 + self.sensors_from_rig.len() + } + + /// Returns `true` if `sensor` is part of this rig (reference or non-reference). + #[inline] + pub fn has_sensor(&self, sensor: &Sensor) -> bool { + self.is_ref_sensor(sensor) || self.sensors_from_rig.contains_key(sensor) + } + + /// Returns `true` if `sensor` is the reference sensor of this rig. + #[inline] + pub fn is_ref_sensor(&self, sensor: &Sensor) -> bool { + *sensor == self.ref_sensor_id + } + + /// All sensor ids in the rig, starting with the reference sensor. + pub fn sensor_ids(&self) -> Vec { + let mut ids = Vec::with_capacity(self.num_sensors()); + ids.push(self.ref_sensor_id); + ids.extend(self.sensors_from_rig.keys().copied()); + ids + } +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn rig_membership() { + let mut rig = Rig::new(1); + let ref_cam = Sensor::camera(10); + rig.add_ref_sensor(ref_cam); + let cam2 = Sensor::camera(11); + rig.add_sensor(cam2, Some(Rigid3d::identity())); + + assert_eq!(rig.num_sensors(), 2); + assert!(rig.is_ref_sensor(&ref_cam)); + assert!(!rig.is_ref_sensor(&cam2)); + assert!(rig.has_sensor(&ref_cam)); + assert!(rig.has_sensor(&cam2)); + assert!(!rig.has_sensor(&Sensor::camera(99))); + + let ids = rig.sensor_ids(); + assert_eq!(ids.len(), 2); + assert_eq!(ids[0], ref_cam); + assert!(ids.contains(&cam2)); + } + + #[test] + fn default_is_invalid() { + let rig = Rig::default(); + assert_eq!(rig.rig_id, INVALID_RIG_ID); + assert_eq!(rig.ref_sensor_id.sensor_type, SensorType::Invalid); + assert_eq!(rig.num_sensors(), 1); + } +} diff --git a/src/scene/track.rs b/src/scene/track.rs new file mode 100644 index 0000000..a68c867 --- /dev/null +++ b/src/scene/track.rs @@ -0,0 +1,102 @@ +//! The [`Track`] and [`TrackElement`] types: the set of image observations of a +//! single 3D point. +//! +//! Mirrors COLMAP's `colmap/scene/track.h`. A track is the list of +//! `(image_id, point2d_idx)` observations that were triangulated into one +//! [`Point3D`](crate::scene::Point3D). + +use crate::types::{ImageId, Point2DIdx}; + +/// One observation of a 3D point: a 2D point index in a specific image. +#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)] +#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))] +pub struct TrackElement { + /// The image containing the observation. + pub image_id: ImageId, + /// The index of the 2D point within that image. + pub point2d_idx: Point2DIdx, +} + +impl TrackElement { + /// Creates a track element referencing 2D point `point2d_idx` in `image_id`. + #[inline] + pub fn new(image_id: ImageId, point2d_idx: Point2DIdx) -> Self { + Self { + image_id, + point2d_idx, + } + } +} + +/// The complete set of observations (a "track") of one 3D point. +#[derive(Debug, Clone, Default, PartialEq)] +#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))] +pub struct Track { + /// The individual observations. + pub elements: Vec, +} + +impl Track { + /// Creates an empty track. + #[inline] + pub fn new() -> Self { + Self { + elements: Vec::new(), + } + } + + /// Creates an empty track (alias of [`Track::new`]). + #[inline] + pub fn empty() -> Self { + Self::new() + } + + /// The number of observations in the track. + #[inline] + pub fn length(&self) -> usize { + self.elements.len() + } + + /// Returns `true` if the track has no observations. + #[inline] + pub fn is_empty(&self) -> bool { + self.elements.is_empty() + } + + /// Appends an existing [`TrackElement`]. + #[inline] + pub fn add_element(&mut self, element: TrackElement) { + self.elements.push(element); + } + + /// Appends a new observation given its image and 2D point index. + #[inline] + pub fn add(&mut self, image_id: ImageId, point2d_idx: Point2DIdx) { + self.elements.push(TrackElement::new(image_id, point2d_idx)); + } +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn empty_track() { + let t = Track::new(); + assert!(t.is_empty()); + assert_eq!(t.length(), 0); + assert_eq!(Track::empty(), t); + } + + #[test] + fn add_grows_track() { + let mut t = Track::new(); + t.add(1, 10); + t.add_element(TrackElement::new(2, 20)); + assert_eq!(t.length(), 2); + assert!(!t.is_empty()); + assert_eq!(t.elements[0], TrackElement::new(1, 10)); + assert_eq!(t.elements[1].image_id, 2); + assert_eq!(t.elements[1].point2d_idx, 20); + } +} diff --git a/src/scene/two_view_geometry.rs b/src/scene/two_view_geometry.rs new file mode 100644 index 0000000..873e4cc --- /dev/null +++ b/src/scene/two_view_geometry.rs @@ -0,0 +1,165 @@ +//! Two-view geometry between an image pair: the relation that survives matching +//! and geometric verification. +//! +//! Mirrors COLMAP's `colmap/scene/two_view_geometry.h`. Holds the geometric model +//! (essential, fundamental and/or homography matrix), the relative pose +//! `cam2_from_cam1`, the surviving inlier matches and the median triangulation +//! angle. + +use crate::feature::FeatureMatches; +use crate::geometry::Rigid3d; +use crate::math::Mat3; + +/// The geometric configuration estimated for an image pair. +#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)] +#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))] +#[repr(i32)] +pub enum TwoViewGeometryConfig { + /// No configuration estimated. + Undefined = 0, + /// Too few or degenerate inliers to decide. + Degenerate = 1, + /// Essential matrix (calibrated cameras). + Calibrated = 2, + /// Fundamental matrix (uncalibrated cameras). + Uncalibrated = 3, + /// Pure planar scene (homography). + Planar = 4, + /// Pure rotation / panorama (homography). + Panoramic = 5, + /// Planar scene or pure rotation (ambiguous homography). + PlanarOrPanoramic = 6, + /// A watermark or other static overlay. + Watermark = 7, + /// Multiple distinct configurations. + Multiple = 8, + /// Calibrated rig configuration. + CalibratedRig = 9, +} + +impl TwoViewGeometryConfig { + /// The integer code used in files and the database. + #[inline] + pub fn id(self) -> i32 { + self as i32 + } + + /// Builds a configuration from its integer code, if valid. + #[inline] + pub fn from_id(id: i32) -> Option { + match id { + 0 => Some(Self::Undefined), + 1 => Some(Self::Degenerate), + 2 => Some(Self::Calibrated), + 3 => Some(Self::Uncalibrated), + 4 => Some(Self::Planar), + 5 => Some(Self::Panoramic), + 6 => Some(Self::PlanarOrPanoramic), + 7 => Some(Self::Watermark), + 8 => Some(Self::Multiple), + 9 => Some(Self::CalibratedRig), + _ => None, + } + } +} + +/// The verified geometric relationship between two images. +#[derive(Debug, Clone, PartialEq)] +#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))] +pub struct TwoViewGeometry { + /// The estimated configuration. + pub config: TwoViewGeometryConfig, + /// The essential matrix, if estimated. + pub e: Option, + /// The fundamental matrix, if estimated. + pub f: Option, + /// The homography matrix, if estimated. + pub h: Option, + /// The relative pose `cam2_from_cam1`, if recovered. + pub cam2_from_cam1: Option, + /// The matches that survived geometric verification. + pub inlier_matches: FeatureMatches, + /// The median triangulation angle (radians); negative if unset. + pub tri_angle: f64, +} + +impl Default for TwoViewGeometry { + /// An undefined geometry with no model, no inliers and an unset angle. + #[inline] + fn default() -> Self { + Self { + config: TwoViewGeometryConfig::Undefined, + e: None, + f: None, + h: None, + cam2_from_cam1: None, + inlier_matches: FeatureMatches::new(), + tri_angle: -1.0, + } + } +} + +impl TwoViewGeometry { + /// Inverts the geometry so it describes the pair `(image2, image1)`. + /// + /// Each matrix is transposed, the relative pose is inverted and every match's + /// two point indices are swapped. + pub fn invert(&mut self) { + self.e = self.e.map(|m| m.transpose()); + self.f = self.f.map(|m| m.transpose()); + self.h = self.h.map(|m| m.transpose()); + self.cam2_from_cam1 = self.cam2_from_cam1.map(|p| p.inverse()); + for m in &mut self.inlier_matches { + std::mem::swap(&mut m.point2d_idx1, &mut m.point2d_idx2); + } + } +} + +#[cfg(test)] +mod tests { + use super::*; + use crate::feature::FeatureMatch; + use crate::math::{UnitQuat, Vec3}; + + #[test] + fn config_roundtrips() { + for c in [ + TwoViewGeometryConfig::Undefined, + TwoViewGeometryConfig::Calibrated, + TwoViewGeometryConfig::CalibratedRig, + ] { + assert_eq!(TwoViewGeometryConfig::from_id(c.id()), Some(c)); + } + assert_eq!(TwoViewGeometryConfig::from_id(42), None); + } + + #[test] + fn default_is_undefined() { + let g = TwoViewGeometry::default(); + assert_eq!(g.config, TwoViewGeometryConfig::Undefined); + assert!(g.e.is_none()); + assert!(g.inlier_matches.is_empty()); + assert_eq!(g.tri_angle, -1.0); + } + + #[test] + fn invert_transposes_and_swaps() { + let mut g = TwoViewGeometry::default(); + let mut m = Mat3::zeros(); + m[(0, 1)] = 2.0; + g.f = Some(m); + g.cam2_from_cam1 = Some(Rigid3d::new(UnitQuat::identity(), Vec3::new(1.0, 0.0, 0.0))); + g.inlier_matches.push(FeatureMatch::new(3, 7)); + + g.invert(); + assert_eq!(g.f.unwrap()[(1, 0)], 2.0); + assert_eq!(g.f.unwrap()[(0, 1)], 0.0); + assert_eq!(g.inlier_matches[0].point2d_idx1, 7); + assert_eq!(g.inlier_matches[0].point2d_idx2, 3); + // Inverting an identity-rotation translation negates it. + assert_eq!( + g.cam2_from_cam1.unwrap().translation, + Vec3::new(-1.0, 0.0, 0.0) + ); + } +} diff --git a/src/sensor/bitmap.rs b/src/sensor/bitmap.rs new file mode 100644 index 0000000..c2088f1 --- /dev/null +++ b/src/sensor/bitmap.rs @@ -0,0 +1,264 @@ +//! A minimal in-memory bitmap, mirroring the storage side of COLMAP's `Bitmap`. +//! +//! This is intentionally a thin container: it owns an interleaved `u8` pixel +//! buffer (`channels` bytes per pixel, row-major, no padding) and offers the +//! geometric / pixel-access helpers that the rest of the crate needs. Image +//! **decoding / encoding** (PNG, JPEG, …) is out of scope here — the relevant +//! method is stubbed to return [`Error::Unimplemented`], because pulling in an +//! image-codec dependency is outside this crate's allowed dependency set. + +use crate::error::{Error, Result}; +use std::path::Path; + +/// An interleaved 8-bit-per-channel bitmap. +/// +/// Pixels are stored row-major with no row padding: byte +/// `data[(y * width + x) * channels + c]` is channel `c` of pixel `(x, y)`. +/// Only grayscale (`channels == 1`) and RGB (`channels == 3`) layouts are +/// produced by the constructors, matching COLMAP's `Bitmap`. +#[derive(Debug, Clone, PartialEq, Eq)] +#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))] +pub struct Bitmap { + /// Image width in pixels. + pub width: usize, + /// Image height in pixels. + pub height: usize, + /// Number of channels per pixel (`1` for grayscale, `3` for RGB). + pub channels: usize, + /// Interleaved, row-major pixel data of length `width * height * channels`. + pub data: Vec, +} + +impl Bitmap { + /// Creates a zero-initialised bitmap of the given size and channel count. + /// + /// # Examples + /// ``` + /// use colmap::sensor::Bitmap; + /// let bmp = Bitmap::new(4, 2, 3); + /// assert_eq!(bmp.data.len(), 4 * 2 * 3); + /// assert!(bmp.is_rgb()); + /// ``` + pub fn new(width: usize, height: usize, channels: usize) -> Self { + Self { + width, + height, + channels, + data: vec![0u8; width * height * channels], + } + } + + /// Creates an RGB bitmap (`channels == 3`). + pub fn new_rgb(width: usize, height: usize) -> Self { + Self::new(width, height, 3) + } + + /// Creates a grayscale bitmap (`channels == 1`). + pub fn new_grey(width: usize, height: usize) -> Self { + Self::new(width, height, 1) + } + + /// Returns `true` for a 3-channel (RGB) bitmap. + #[inline] + pub fn is_rgb(&self) -> bool { + self.channels == 3 + } + + /// Returns `true` for a 1-channel (grayscale) bitmap. + #[inline] + pub fn is_grey(&self) -> bool { + self.channels == 1 + } + + /// Total number of pixels (`width * height`). + #[inline] + pub fn num_pixels(&self) -> usize { + self.width * self.height + } + + /// Returns `true` if `(x, y)` lies inside the image bounds. + #[inline] + pub fn in_bounds(&self, x: usize, y: usize) -> bool { + x < self.width && y < self.height + } + + /// Byte offset of channel `0` of pixel `(x, y)` within [`data`](Self::data). + #[inline] + fn offset(&self, x: usize, y: usize) -> usize { + (y * self.width + x) * self.channels + } + + /// Returns the channel values of pixel `(x, y)`, or `None` if out of bounds. + /// + /// The returned slice has length [`channels`](Self::channels). + pub fn get_pixel(&self, x: usize, y: usize) -> Option<&[u8]> { + if !self.in_bounds(x, y) { + return None; + } + let off = self.offset(x, y); + Some(&self.data[off..off + self.channels]) + } + + /// Sets the channel values of pixel `(x, y)`. + /// + /// Returns an error if `(x, y)` is out of bounds or if `value.len()` does + /// not match [`channels`](Self::channels). + pub fn set_pixel(&mut self, x: usize, y: usize, value: &[u8]) -> Result<()> { + if !self.in_bounds(x, y) { + return Err(Error::InvalidArgument(format!( + "pixel ({x}, {y}) out of bounds for {}x{} bitmap", + self.width, self.height + ))); + } + if value.len() != self.channels { + return Err(Error::InvalidArgument(format!( + "expected {} channel(s), got {}", + self.channels, + value.len() + ))); + } + let off = self.offset(x, y); + self.data[off..off + self.channels].copy_from_slice(value); + Ok(()) + } + + /// Returns the grayscale value of pixel `(x, y)`. + /// + /// For an RGB bitmap the luminance is computed with the standard + /// `0.299 R + 0.587 G + 0.114 B` weighting and rounded to the nearest byte. + /// Returns `None` if `(x, y)` is out of bounds. + pub fn get_grey(&self, x: usize, y: usize) -> Option { + let px = self.get_pixel(x, y)?; + match self.channels { + 1 => Some(px[0]), + 3 => { + let v = 0.299 * px[0] as f64 + 0.587 * px[1] as f64 + 0.114 * px[2] as f64; + Some(v.round().clamp(0.0, 255.0) as u8) + } + _ => Some(px[0]), + } + } + + /// Converts this bitmap to grayscale, consuming `self`. + /// + /// A grayscale bitmap is returned unchanged. An RGB bitmap is converted with + /// the luminance weighting from [`get_grey`](Self::get_grey). + pub fn to_grey(&self) -> Bitmap { + if self.is_grey() { + return self.clone(); + } + let mut out = Bitmap::new_grey(self.width, self.height); + for y in 0..self.height { + for x in 0..self.width { + if let Some(g) = self.get_grey(x, y) { + out.data[y * self.width + x] = g; + } + } + } + out + } + + /// Reads a bitmap from a file on disk. + /// + /// Image decoding is **not** implemented in this crate (it would require an + /// image-codec dependency outside the allowed set), so this always returns + /// [`Error::Unimplemented`]. + pub fn read>(_path: P) -> Result { + Err(Error::Unimplemented("Bitmap::read (image decoding)")) + } + + /// Writes this bitmap to a file on disk. + /// + /// Image encoding is **not** implemented in this crate (see + /// [`read`](Self::read)), so this always returns [`Error::Unimplemented`]. + pub fn write>(&self, _path: P) -> Result<()> { + Err(Error::Unimplemented("Bitmap::write (image encoding)")) + } +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn new_allocates_correct_size() { + let bmp = Bitmap::new(5, 3, 3); + assert_eq!(bmp.width, 5); + assert_eq!(bmp.height, 3); + assert_eq!(bmp.channels, 3); + assert_eq!(bmp.data.len(), 5 * 3 * 3); + assert_eq!(bmp.num_pixels(), 15); + assert!(bmp.is_rgb()); + assert!(!bmp.is_grey()); + } + + #[test] + fn grey_constructor() { + let bmp = Bitmap::new_grey(4, 4); + assert!(bmp.is_grey()); + assert!(!bmp.is_rgb()); + assert_eq!(bmp.data.len(), 16); + } + + #[test] + fn set_and_get_pixel_roundtrip() { + let mut bmp = Bitmap::new_rgb(3, 3); + bmp.set_pixel(1, 2, &[10, 20, 30]).unwrap(); + assert_eq!(bmp.get_pixel(1, 2).unwrap(), &[10, 20, 30]); + // Untouched pixel stays zero. + assert_eq!(bmp.get_pixel(0, 0).unwrap(), &[0, 0, 0]); + } + + #[test] + fn out_of_bounds_access() { + let mut bmp = Bitmap::new_rgb(2, 2); + assert!(bmp.get_pixel(2, 0).is_none()); + assert!(bmp.get_pixel(0, 2).is_none()); + assert!(bmp.set_pixel(5, 5, &[1, 2, 3]).is_err()); + assert!(!bmp.in_bounds(2, 2)); + assert!(bmp.in_bounds(1, 1)); + } + + #[test] + fn wrong_channel_count_errors() { + let mut bmp = Bitmap::new_rgb(2, 2); + assert!(bmp.set_pixel(0, 0, &[1, 2]).is_err()); + let mut grey = Bitmap::new_grey(2, 2); + assert!(grey.set_pixel(0, 0, &[1, 2, 3]).is_err()); + assert!(grey.set_pixel(0, 0, &[42]).is_ok()); + assert_eq!(grey.get_grey(0, 0), Some(42)); + } + + #[test] + fn rgb_to_grey_luminance() { + let mut bmp = Bitmap::new_rgb(1, 1); + bmp.set_pixel(0, 0, &[255, 255, 255]).unwrap(); + assert_eq!(bmp.get_grey(0, 0), Some(255)); + bmp.set_pixel(0, 0, &[0, 0, 0]).unwrap(); + assert_eq!(bmp.get_grey(0, 0), Some(0)); + // Pure green -> 0.587 * 255 ~= 150. + bmp.set_pixel(0, 0, &[0, 255, 0]).unwrap(); + assert_eq!(bmp.get_grey(0, 0), Some(150)); + } + + #[test] + fn to_grey_converts_whole_image() { + let mut bmp = Bitmap::new_rgb(2, 1); + bmp.set_pixel(0, 0, &[255, 255, 255]).unwrap(); + bmp.set_pixel(1, 0, &[0, 0, 0]).unwrap(); + let grey = bmp.to_grey(); + assert!(grey.is_grey()); + assert_eq!(grey.get_grey(0, 0), Some(255)); + assert_eq!(grey.get_grey(1, 0), Some(0)); + // Already-grey bitmap is returned unchanged. + let again = grey.to_grey(); + assert_eq!(again, grey); + } + + #[test] + fn read_write_are_unimplemented() { + assert!(Bitmap::read("nonexistent.png").is_err()); + let bmp = Bitmap::new_rgb(1, 1); + assert!(bmp.write("out.png").is_err()); + } +} diff --git a/src/sensor/mod.rs b/src/sensor/mod.rs new file mode 100644 index 0000000..071e4ba --- /dev/null +++ b/src/sensor/mod.rs @@ -0,0 +1,19 @@ +//! Camera-model projection math and a minimal in-memory bitmap. +//! +//! This module ports the projection side of COLMAP's `sensor/` directory: +//! +//! * [`models`] implements all 17 [`CameraModelId`](crate::types::CameraModelId) +//! camera models, exposing the free functions [`cam_from_img`], [`img_from_cam`], +//! [`cam_ray_from_img`] and [`img_from_cam_threshold`] that +//! [`Camera`](crate::scene::Camera) builds upon. +//! * [`bitmap`] provides [`Bitmap`], a thin interleaved-`u8` pixel container. +//! +//! Image decoding/encoding is deliberately out of scope (it would require an +//! image-codec dependency); the corresponding [`Bitmap`] methods return +//! [`Error::Unimplemented`](crate::error::Error::Unimplemented). + +pub mod bitmap; +pub mod models; + +pub use bitmap::Bitmap; +pub use models::*; diff --git a/src/sensor/models.rs b/src/sensor/models.rs new file mode 100644 index 0000000..9085884 --- /dev/null +++ b/src/sensor/models.rs @@ -0,0 +1,1046 @@ +//! Camera-model projection math for all 17 COLMAP camera models. +//! +//! This module ports `colmap/sensor/models.h`. It provides three primitive +//! operations, parameterised by a [`CameraModelId`] and the model's intrinsic +//! parameter slice: +//! +//! * [`img_from_cam`] — project a 3D camera-frame point to a pixel. +//! * [`cam_from_img`] — unproject a pixel to a point on the `z = 1` normalized +//! image plane (returns the `(u, v)` of the ray `(u, v, 1)`). +//! * [`cam_ray_from_img`] — like [`cam_from_img`] but returns the unit ray. +//! +//! The parameter order of every model is **frozen** by [`CameraModelId`] and the +//! index helpers ([`CameraModelId::focal_length_idxs`] etc.); the math below +//! relies on exactly that layout. +//! +//! ## Distortion convention +//! +//! For pinhole-family models, given the normalized point `(u, v) = (X/Z, Y/Z)`, +//! a distortion `d(u, v) = (du, dv)` is applied and the pixel is +//! `x = fx * (u + du) + cx`, `y = fy * (v + dv) + cy`. Fisheye-family models +//! first map `(u, v)` to equidistant fisheye coordinates and distort there. +//! Division and FOV models use their closed-form expressions directly. +//! +//! Models without a closed-form inverse iterate the distortion with a damped +//! Gauss–Newton / fixed-point scheme (`undistort`, a private helper). + +use crate::math::{Vec2, Vec3}; +use crate::types::CameraModelId; + +/// Iteration budget for the iterative un-distortion solver. +const UNDISTORT_MAX_ITERS: usize = 100; +/// Convergence tolerance (on the squared step) for the un-distortion solver. +const UNDISTORT_EPS: f64 = 1e-10; + +// =========================================================================== +// Public free-function API (consumed by `crate::scene::Camera`). +// =========================================================================== + +/// Unprojects a pixel `xy` to the normalized image plane `(u, v)` (with `z = 1` +/// implied), i.e. the camera-frame ray direction scaled so its third component +/// is one. +/// +/// Returns `None` when the model has no valid pre-image for the pixel (e.g. an +/// iterative solver that fails to converge, or an out-of-domain omnidirectional +/// pixel). +/// +/// # Examples +/// ``` +/// use colmap::sensor::cam_from_img; +/// use colmap::types::CameraModelId; +/// use colmap::math::Vec2; +/// +/// // SIMPLE_PINHOLE with f = 100, principal point (50, 50). +/// let params = [100.0, 50.0, 50.0]; +/// let uv = cam_from_img(CameraModelId::SimplePinhole, ¶ms, &Vec2::new(150.0, 50.0)).unwrap(); +/// assert!((uv.x - 1.0).abs() < 1e-12 && uv.y.abs() < 1e-12); +/// ``` +pub fn cam_from_img(model: CameraModelId, params: &[f64], xy: &Vec2) -> Option { + if params.len() != model.num_params() { + return None; + } + match model { + CameraModelId::SimplePinhole => simple_pinhole_cam_from_img(params, xy), + CameraModelId::Pinhole => pinhole_cam_from_img(params, xy), + CameraModelId::SimpleRadial => simple_radial_cam_from_img(params, xy), + CameraModelId::Radial => radial_cam_from_img(params, xy), + CameraModelId::OpenCV => opencv_cam_from_img(params, xy), + CameraModelId::OpenCVFisheye => opencv_fisheye_cam_from_img(params, xy), + CameraModelId::FullOpenCV => full_opencv_cam_from_img(params, xy), + CameraModelId::FOV => fov_cam_from_img(params, xy), + CameraModelId::SimpleRadialFisheye => simple_radial_fisheye_cam_from_img(params, xy), + CameraModelId::RadialFisheye => radial_fisheye_cam_from_img(params, xy), + CameraModelId::ThinPrismFisheye => thin_prism_fisheye_cam_from_img(params, xy), + CameraModelId::RadTanThinPrismFisheye => rad_tan_thin_prism_fisheye_cam_from_img(params, xy), + CameraModelId::SimpleDivision => simple_division_cam_from_img(params, xy), + CameraModelId::Division => division_cam_from_img(params, xy), + CameraModelId::SimpleFisheye => simple_fisheye_cam_from_img(params, xy), + CameraModelId::Fisheye => fisheye_cam_from_img(params, xy), + CameraModelId::EUCM => eucm_cam_from_img(params, xy), + } +} + +/// Projects a 3D camera-frame point `cam_point` to a pixel. +/// +/// Returns `None` when the point cannot be projected by the model (e.g. behind +/// the camera for an omnidirectional model whose domain excludes it). +/// +/// # Examples +/// ``` +/// use colmap::sensor::img_from_cam; +/// use colmap::types::CameraModelId; +/// use colmap::math::Vec3; +/// +/// let params = [100.0, 50.0, 50.0]; +/// let xy = img_from_cam(CameraModelId::SimplePinhole, ¶ms, &Vec3::new(1.0, 0.0, 1.0)).unwrap(); +/// assert!((xy.x - 150.0).abs() < 1e-12 && (xy.y - 50.0).abs() < 1e-12); +/// ``` +pub fn img_from_cam(model: CameraModelId, params: &[f64], cam_point: &Vec3) -> Option { + if params.len() != model.num_params() { + return None; + } + match model { + CameraModelId::SimplePinhole => simple_pinhole_img_from_cam(params, cam_point), + CameraModelId::Pinhole => pinhole_img_from_cam(params, cam_point), + CameraModelId::SimpleRadial => simple_radial_img_from_cam(params, cam_point), + CameraModelId::Radial => radial_img_from_cam(params, cam_point), + CameraModelId::OpenCV => opencv_img_from_cam(params, cam_point), + CameraModelId::OpenCVFisheye => opencv_fisheye_img_from_cam(params, cam_point), + CameraModelId::FullOpenCV => full_opencv_img_from_cam(params, cam_point), + CameraModelId::FOV => fov_img_from_cam(params, cam_point), + CameraModelId::SimpleRadialFisheye => simple_radial_fisheye_img_from_cam(params, cam_point), + CameraModelId::RadialFisheye => radial_fisheye_img_from_cam(params, cam_point), + CameraModelId::ThinPrismFisheye => thin_prism_fisheye_img_from_cam(params, cam_point), + CameraModelId::RadTanThinPrismFisheye => { + rad_tan_thin_prism_fisheye_img_from_cam(params, cam_point) + } + CameraModelId::SimpleDivision => simple_division_img_from_cam(params, cam_point), + CameraModelId::Division => division_img_from_cam(params, cam_point), + CameraModelId::SimpleFisheye => simple_fisheye_img_from_cam(params, cam_point), + CameraModelId::Fisheye => fisheye_img_from_cam(params, cam_point), + CameraModelId::EUCM => eucm_img_from_cam(params, cam_point), + } +} + +/// Unprojects a pixel `xy` to a **unit** camera-frame ray. +/// +/// Equivalent to [`cam_from_img`] followed by promoting `(u, v)` to `(u, v, 1)` +/// and normalising. Returns `None` exactly when [`cam_from_img`] does. +pub fn cam_ray_from_img(model: CameraModelId, params: &[f64], xy: &Vec2) -> Option { + let uv = cam_from_img(model, params, xy)?; + Some(Vec3::new(uv.x, uv.y, 1.0).normalize()) +} + +/// Converts a reprojection-error threshold expressed in **pixels** to the +/// equivalent threshold in normalized image coordinates. +/// +/// This is `threshold / mean_focal_length`, matching COLMAP's +/// `Camera::ImageToCameraThreshold`. A zero or empty focal length yields `0`. +/// +/// # Examples +/// ``` +/// use colmap::sensor::img_from_cam_threshold; +/// use colmap::types::CameraModelId; +/// +/// // PINHOLE fx = 200, fy = 100 -> mean focal length 150. +/// let params = [200.0, 100.0, 0.0, 0.0]; +/// let t = img_from_cam_threshold(CameraModelId::Pinhole, ¶ms, 3.0); +/// assert!((t - 3.0 / 150.0).abs() < 1e-12); +/// ``` +pub fn img_from_cam_threshold(model: CameraModelId, params: &[f64], threshold: f64) -> f64 { + let idxs = model.focal_length_idxs(); + if idxs.is_empty() || params.len() != model.num_params() { + return 0.0; + } + let mut sum = 0.0; + for &i in idxs { + sum += params[i]; + } + let mean_focal = sum / idxs.len() as f64; + if mean_focal == 0.0 { + 0.0 + } else { + threshold / mean_focal + } +} + +// =========================================================================== +// Shared helpers. +// =========================================================================== + +/// A 2x2 distortion Jacobian `[[d(du+u)/du, d(du+u)/dv], [.., ..]]` computed by +/// central finite differences. Used by the generic iterative un-distortion. +#[inline] +fn numeric_jacobian(distort: &F, u: f64, v: f64) -> [[f64; 2]; 2] +where + F: Fn(f64, f64) -> (f64, f64), +{ + // Step proportional to magnitude, with a floor to stay well-conditioned. + let h = 1e-6_f64; + let (dx_p_u, dy_p_u) = distort(u + h, v); + let (dx_m_u, dy_m_u) = distort(u - h, v); + let (dx_p_v, dy_p_v) = distort(u, v + h); + let (dx_m_v, dy_m_v) = distort(u, v - h); + // f(u,v) = (u + du, v + dv); J = d f / d (u,v). + let j00 = 1.0 + (dx_p_u - dx_m_u) / (2.0 * h); + let j10 = (dy_p_u - dy_m_u) / (2.0 * h); + let j01 = (dx_p_v - dx_m_v) / (2.0 * h); + let j11 = 1.0 + (dy_p_v - dy_m_v) / (2.0 * h); + [[j00, j01], [j10, j11]] +} + +/// Solves `distort(u, v) == (target_x, target_y)` for `(u, v)`, where `distort` +/// returns the additive distortion `(du, dv)` so that the distorted point is +/// `(u + du, v + dv)`. Uses damped Gauss–Newton with a numeric Jacobian, +/// starting from `(target_x, target_y)`. +/// +/// Returns `None` if the iteration fails to converge. +fn undistort(distort: &F, target_x: f64, target_y: f64) -> Option +where + F: Fn(f64, f64) -> (f64, f64), +{ + let mut u = target_x; + let mut v = target_y; + for _ in 0..UNDISTORT_MAX_ITERS { + let (du, dv) = distort(u, v); + let rx = (u + du) - target_x; + let ry = (v + dv) - target_y; + let j = numeric_jacobian(distort, u, v); + let det = j[0][0] * j[1][1] - j[0][1] * j[1][0]; + if det.abs() < 1e-15 { + // Fall back to a plain fixed-point step. + u = target_x - du; + v = target_y - dv; + } else { + // Newton step: delta = J^-1 * r. + let inv_det = 1.0 / det; + let dx = inv_det * (j[1][1] * rx - j[0][1] * ry); + let dy = inv_det * (-j[1][0] * rx + j[0][0] * ry); + u -= dx; + v -= dy; + } + let (du2, dv2) = distort(u, v); + let ex = (u + du2) - target_x; + let ey = (v + dv2) - target_y; + if ex * ex + ey * ey < UNDISTORT_EPS { + return Some(Vec2::new(u, v)); + } + } + // Accept the final estimate if it is reasonably close. + let (du, dv) = distort(u, v); + let ex = (u + du) - target_x; + let ey = (v + dv) - target_y; + if ex * ex + ey * ey < 1e-6 { + Some(Vec2::new(u, v)) + } else { + None + } +} + +// --------------------------------------------------------------------------- +// Pinhole family. +// --------------------------------------------------------------------------- + +#[inline] +fn simple_pinhole_img_from_cam(params: &[f64], p: &Vec3) -> Option { + let (f, cx, cy) = (params[0], params[1], params[2]); + let u = p.x / p.z; + let v = p.y / p.z; + Some(Vec2::new(f * u + cx, f * v + cy)) +} + +#[inline] +fn simple_pinhole_cam_from_img(params: &[f64], xy: &Vec2) -> Option { + let (f, cx, cy) = (params[0], params[1], params[2]); + Some(Vec2::new((xy.x - cx) / f, (xy.y - cy) / f)) +} + +#[inline] +fn pinhole_img_from_cam(params: &[f64], p: &Vec3) -> Option { + let (fx, fy, cx, cy) = (params[0], params[1], params[2], params[3]); + let u = p.x / p.z; + let v = p.y / p.z; + Some(Vec2::new(fx * u + cx, fy * v + cy)) +} + +#[inline] +fn pinhole_cam_from_img(params: &[f64], xy: &Vec2) -> Option { + let (fx, fy, cx, cy) = (params[0], params[1], params[2], params[3]); + Some(Vec2::new((xy.x - cx) / fx, (xy.y - cy) / fy)) +} + +// --------------------------------------------------------------------------- +// SIMPLE_RADIAL. +// --------------------------------------------------------------------------- + +#[inline] +fn simple_radial_distort(k: f64, u: f64, v: f64) -> (f64, f64) { + let r2 = u * u + v * v; + let radial = k * r2; + (u * radial, v * radial) +} + +fn simple_radial_img_from_cam(params: &[f64], p: &Vec3) -> Option { + let (f, cx, cy, k) = (params[0], params[1], params[2], params[3]); + let u = p.x / p.z; + let v = p.y / p.z; + let (du, dv) = simple_radial_distort(k, u, v); + Some(Vec2::new(f * (u + du) + cx, f * (v + dv) + cy)) +} + +fn simple_radial_cam_from_img(params: &[f64], xy: &Vec2) -> Option { + let (f, cx, cy, k) = (params[0], params[1], params[2], params[3]); + let tx = (xy.x - cx) / f; + let ty = (xy.y - cy) / f; + undistort(&|u, v| simple_radial_distort(k, u, v), tx, ty) +} + +// --------------------------------------------------------------------------- +// RADIAL. +// --------------------------------------------------------------------------- + +#[inline] +fn radial_distort(k1: f64, k2: f64, u: f64, v: f64) -> (f64, f64) { + let r2 = u * u + v * v; + let radial = k1 * r2 + k2 * r2 * r2; + (u * radial, v * radial) +} + +fn radial_img_from_cam(params: &[f64], p: &Vec3) -> Option { + let (f, cx, cy, k1, k2) = (params[0], params[1], params[2], params[3], params[4]); + let u = p.x / p.z; + let v = p.y / p.z; + let (du, dv) = radial_distort(k1, k2, u, v); + Some(Vec2::new(f * (u + du) + cx, f * (v + dv) + cy)) +} + +fn radial_cam_from_img(params: &[f64], xy: &Vec2) -> Option { + let (f, cx, cy, k1, k2) = (params[0], params[1], params[2], params[3], params[4]); + let tx = (xy.x - cx) / f; + let ty = (xy.y - cy) / f; + undistort(&|u, v| radial_distort(k1, k2, u, v), tx, ty) +} + +// --------------------------------------------------------------------------- +// OPENCV. +// --------------------------------------------------------------------------- + +#[inline] +fn opencv_distort(k1: f64, k2: f64, p1: f64, p2: f64, u: f64, v: f64) -> (f64, f64) { + let u2 = u * u; + let v2 = v * v; + let uv = u * v; + let r2 = u2 + v2; + let radial = k1 * r2 + k2 * r2 * r2; + let du = u * radial + 2.0 * p1 * uv + p2 * (r2 + 2.0 * u2); + let dv = v * radial + 2.0 * p2 * uv + p1 * (r2 + 2.0 * v2); + (du, dv) +} + +fn opencv_img_from_cam(params: &[f64], p: &Vec3) -> Option { + let (fx, fy, cx, cy) = (params[0], params[1], params[2], params[3]); + let (k1, k2, p1, p2) = (params[4], params[5], params[6], params[7]); + let u = p.x / p.z; + let v = p.y / p.z; + let (du, dv) = opencv_distort(k1, k2, p1, p2, u, v); + Some(Vec2::new(fx * (u + du) + cx, fy * (v + dv) + cy)) +} + +fn opencv_cam_from_img(params: &[f64], xy: &Vec2) -> Option { + let (fx, fy, cx, cy) = (params[0], params[1], params[2], params[3]); + let (k1, k2, p1, p2) = (params[4], params[5], params[6], params[7]); + let tx = (xy.x - cx) / fx; + let ty = (xy.y - cy) / fy; + undistort(&|u, v| opencv_distort(k1, k2, p1, p2, u, v), tx, ty) +} + +// --------------------------------------------------------------------------- +// FULL_OPENCV (rational radial + tangential). +// --------------------------------------------------------------------------- + +#[allow(clippy::too_many_arguments)] +#[inline] +fn full_opencv_distort( + k1: f64, + k2: f64, + p1: f64, + p2: f64, + k3: f64, + k4: f64, + k5: f64, + k6: f64, + u: f64, + v: f64, +) -> (f64, f64) { + let u2 = u * u; + let v2 = v * v; + let uv = u * v; + let r2 = u2 + v2; + let r4 = r2 * r2; + let r6 = r4 * r2; + let radial = (1.0 + k1 * r2 + k2 * r4 + k3 * r6) / (1.0 + k4 * r2 + k5 * r4 + k6 * r6); + let du = u * radial + 2.0 * p1 * uv + p2 * (r2 + 2.0 * u2) - u; + let dv = v * radial + 2.0 * p2 * uv + p1 * (r2 + 2.0 * v2) - v; + (du, dv) +} + +fn full_opencv_img_from_cam(params: &[f64], p: &Vec3) -> Option { + let (fx, fy, cx, cy) = (params[0], params[1], params[2], params[3]); + let (k1, k2, p1, p2) = (params[4], params[5], params[6], params[7]); + let (k3, k4, k5, k6) = (params[8], params[9], params[10], params[11]); + let u = p.x / p.z; + let v = p.y / p.z; + let (du, dv) = full_opencv_distort(k1, k2, p1, p2, k3, k4, k5, k6, u, v); + Some(Vec2::new(fx * (u + du) + cx, fy * (v + dv) + cy)) +} + +fn full_opencv_cam_from_img(params: &[f64], xy: &Vec2) -> Option { + let (fx, fy, cx, cy) = (params[0], params[1], params[2], params[3]); + let (k1, k2, p1, p2) = (params[4], params[5], params[6], params[7]); + let (k3, k4, k5, k6) = (params[8], params[9], params[10], params[11]); + let tx = (xy.x - cx) / fx; + let ty = (xy.y - cy) / fy; + undistort( + &|u, v| full_opencv_distort(k1, k2, p1, p2, k3, k4, k5, k6, u, v), + tx, + ty, + ) +} + +// --------------------------------------------------------------------------- +// FOV (field-of-view model). Closed-form forward and inverse. +// --------------------------------------------------------------------------- + +fn fov_img_from_cam(params: &[f64], p: &Vec3) -> Option { + let (fx, fy, cx, cy, omega) = (params[0], params[1], params[2], params[3], params[4]); + let u = p.x / p.z; + let v = p.y / p.z; + let (du, dv) = fov_distort(omega, u, v); + Some(Vec2::new(fx * du + cx, fy * dv + cy)) +} + +fn fov_cam_from_img(params: &[f64], xy: &Vec2) -> Option { + let (fx, fy, cx, cy, omega) = (params[0], params[1], params[2], params[3], params[4]); + let dx = (xy.x - cx) / fx; + let dy = (xy.y - cy) / fy; + Some(fov_undistort(omega, dx, dy)) +} + +/// FOV forward map: returns the distorted normalized coordinates `(x, y)` +/// (already multiplied by the radial factor), matching COLMAP's `Distortion`. +#[inline] +fn fov_distort(omega: f64, u: f64, v: f64) -> (f64, f64) { + let radius2 = u * u + v * v; + let omega2 = omega * omega; + + let factor; + if omega2 < 1e-8 { + // As omega -> 0, tan(w/2) -> w/2, so atan(2 r tan(w/2)) / (r w) -> + // atan(r w) / (r w) -> 1. The map degenerates to a plain pinhole. + factor = 1.0; + } else if radius2 < 1e-8 { + // Series expansion around r = 0: atan(2 r tan(w/2)) / (r w) -> + // 2 tan(w/2) / w. + factor = 2.0 * (omega / 2.0).tan() / omega; + } else { + let radius = radius2.sqrt(); + let numerator = (2.0 * radius * (omega / 2.0).tan()).atan(); + factor = numerator / (radius * omega); + } + (u * factor, v * factor) +} + +/// FOV inverse map: undistorts `(x, y)` back to normalized `(u, v)`. +#[inline] +fn fov_undistort(omega: f64, x: f64, y: f64) -> Vec2 { + let radius2 = x * x + y * y; + let omega2 = omega * omega; + + let factor; + if omega2 < 1e-8 { + // tan(r w) / (2 r tan(w/2)) -> 1 as omega -> 0. + factor = 1.0; + } else if radius2 < 1e-8 { + // r -> 0: tan(r w)/(2 r tan(w/2)) -> w / (2 tan(w/2)). + factor = omega / (2.0 * (omega / 2.0).tan()); + } else { + let radius = radius2.sqrt(); + factor = (radius * omega).tan() / (2.0 * radius * (omega / 2.0).tan()); + } + Vec2::new(x * factor, y * factor) +} + +// --------------------------------------------------------------------------- +// Fisheye coordinate conversions (shared by the fisheye family). +// --------------------------------------------------------------------------- + +/// Maps perspective-normalized coordinates `(u, v)` (the `(X/Z, Y/Z)` of a +/// pinhole projection) to equidistant fisheye coordinates, where the radius is +/// the incidence angle `theta = atan(r)`. +#[inline] +fn fisheye_from_normal(u: f64, v: f64) -> (f64, f64) { + let r = (u * u + v * v).sqrt(); + if r > 1e-8 { + let theta = r.atan(); + let s = theta / r; + (u * s, v * s) + } else { + (u, v) + } +} + +/// Inverse of [`fisheye_from_normal`]: maps equidistant fisheye coordinates +/// back to perspective-normalized `(u, v)`. +#[inline] +fn normal_from_fisheye(uu: f64, vv: f64) -> (f64, f64) { + let theta = (uu * uu + vv * vv).sqrt(); + if theta > 1e-8 { + let r = theta.tan(); + let s = r / theta; + (uu * s, vv * s) + } else { + (uu, vv) + } +} + +// --------------------------------------------------------------------------- +// SIMPLE_FISHEYE / FISHEYE (equidistant, no extra distortion). +// --------------------------------------------------------------------------- + +fn simple_fisheye_img_from_cam(params: &[f64], p: &Vec3) -> Option { + let (f, cx, cy) = (params[0], params[1], params[2]); + let u = p.x / p.z; + let v = p.y / p.z; + let (fu, fv) = fisheye_from_normal(u, v); + Some(Vec2::new(f * fu + cx, f * fv + cy)) +} + +fn simple_fisheye_cam_from_img(params: &[f64], xy: &Vec2) -> Option { + let (f, cx, cy) = (params[0], params[1], params[2]); + let fu = (xy.x - cx) / f; + let fv = (xy.y - cy) / f; + let (u, v) = normal_from_fisheye(fu, fv); + Some(Vec2::new(u, v)) +} + +fn fisheye_img_from_cam(params: &[f64], p: &Vec3) -> Option { + let (fx, fy, cx, cy) = (params[0], params[1], params[2], params[3]); + let u = p.x / p.z; + let v = p.y / p.z; + let (fu, fv) = fisheye_from_normal(u, v); + Some(Vec2::new(fx * fu + cx, fy * fv + cy)) +} + +fn fisheye_cam_from_img(params: &[f64], xy: &Vec2) -> Option { + let (fx, fy, cx, cy) = (params[0], params[1], params[2], params[3]); + let fu = (xy.x - cx) / fx; + let fv = (xy.y - cy) / fy; + let (u, v) = normal_from_fisheye(fu, fv); + Some(Vec2::new(u, v)) +} + +// --------------------------------------------------------------------------- +// SIMPLE_RADIAL_FISHEYE / RADIAL_FISHEYE. +// --------------------------------------------------------------------------- + +/// Additive radial distortion in fisheye space: `radial = sum(k_i * theta^2i)`. +#[inline] +fn fisheye_radial_distort(ks: &[f64], uu: f64, vv: f64) -> (f64, f64) { + let th2 = uu * uu + vv * vv; + let mut radial = 0.0; + let mut pw = th2; + for &k in ks { + radial += k * pw; + pw *= th2; + } + (uu * radial, vv * radial) +} + +fn simple_radial_fisheye_img_from_cam(params: &[f64], p: &Vec3) -> Option { + let (f, cx, cy, k) = (params[0], params[1], params[2], params[3]); + let u = p.x / p.z; + let v = p.y / p.z; + let (fu, fv) = fisheye_from_normal(u, v); + let (du, dv) = fisheye_radial_distort(&[k], fu, fv); + Some(Vec2::new(f * (fu + du) + cx, f * (fv + dv) + cy)) +} + +fn simple_radial_fisheye_cam_from_img(params: &[f64], xy: &Vec2) -> Option { + let (f, cx, cy, k) = (params[0], params[1], params[2], params[3]); + let tx = (xy.x - cx) / f; + let ty = (xy.y - cy) / f; + let fish = undistort(&|uu, vv| fisheye_radial_distort(&[k], uu, vv), tx, ty)?; + let (u, v) = normal_from_fisheye(fish.x, fish.y); + Some(Vec2::new(u, v)) +} + +fn radial_fisheye_img_from_cam(params: &[f64], p: &Vec3) -> Option { + let (f, cx, cy, k1, k2) = (params[0], params[1], params[2], params[3], params[4]); + let u = p.x / p.z; + let v = p.y / p.z; + let (fu, fv) = fisheye_from_normal(u, v); + let (du, dv) = fisheye_radial_distort(&[k1, k2], fu, fv); + Some(Vec2::new(f * (fu + du) + cx, f * (fv + dv) + cy)) +} + +fn radial_fisheye_cam_from_img(params: &[f64], xy: &Vec2) -> Option { + let (f, cx, cy, k1, k2) = (params[0], params[1], params[2], params[3], params[4]); + let tx = (xy.x - cx) / f; + let ty = (xy.y - cy) / f; + let fish = undistort(&|uu, vv| fisheye_radial_distort(&[k1, k2], uu, vv), tx, ty)?; + let (u, v) = normal_from_fisheye(fish.x, fish.y); + Some(Vec2::new(u, v)) +} + +// --------------------------------------------------------------------------- +// OPENCV_FISHEYE. +// --------------------------------------------------------------------------- + +/// OpenCV fisheye distortion: scales the fisheye coordinates by +/// `theta_d / theta` where `theta_d = theta (1 + k1 th^2 + k2 th^4 + ...)`. +/// Returned as an additive `(du, dv)`. +#[inline] +fn opencv_fisheye_distort(k1: f64, k2: f64, k3: f64, k4: f64, uu: f64, vv: f64) -> (f64, f64) { + let th2 = uu * uu + vv * vv; + let th4 = th2 * th2; + let th6 = th4 * th2; + let th8 = th4 * th4; + let radial = k1 * th2 + k2 * th4 + k3 * th6 + k4 * th8; + (uu * radial, vv * radial) +} + +fn opencv_fisheye_img_from_cam(params: &[f64], p: &Vec3) -> Option { + let (fx, fy, cx, cy) = (params[0], params[1], params[2], params[3]); + let (k1, k2, k3, k4) = (params[4], params[5], params[6], params[7]); + let u = p.x / p.z; + let v = p.y / p.z; + let (fu, fv) = fisheye_from_normal(u, v); + let (du, dv) = opencv_fisheye_distort(k1, k2, k3, k4, fu, fv); + Some(Vec2::new(fx * (fu + du) + cx, fy * (fv + dv) + cy)) +} + +fn opencv_fisheye_cam_from_img(params: &[f64], xy: &Vec2) -> Option { + let (fx, fy, cx, cy) = (params[0], params[1], params[2], params[3]); + let (k1, k2, k3, k4) = (params[4], params[5], params[6], params[7]); + let tx = (xy.x - cx) / fx; + let ty = (xy.y - cy) / fy; + let fish = undistort(&|uu, vv| opencv_fisheye_distort(k1, k2, k3, k4, uu, vv), tx, ty)?; + let (u, v) = normal_from_fisheye(fish.x, fish.y); + Some(Vec2::new(u, v)) +} + +// --------------------------------------------------------------------------- +// THIN_PRISM_FISHEYE. +// --------------------------------------------------------------------------- + +#[allow(clippy::too_many_arguments)] +#[inline] +fn thin_prism_fisheye_distort( + k1: f64, + k2: f64, + p1: f64, + p2: f64, + k3: f64, + k4: f64, + sx1: f64, + sy1: f64, + uu: f64, + vv: f64, +) -> (f64, f64) { + let u2 = uu * uu; + let v2 = vv * vv; + let uv = uu * vv; + let r2 = u2 + v2; + let r4 = r2 * r2; + let r6 = r4 * r2; + let r8 = r4 * r4; + let radial = k1 * r2 + k2 * r4 + k3 * r6 + k4 * r8; + let du = uu * radial + 2.0 * p1 * uv + p2 * (r2 + 2.0 * u2) + sx1 * r2; + let dv = vv * radial + 2.0 * p2 * uv + p1 * (r2 + 2.0 * v2) + sy1 * r2; + (du, dv) +} + +fn thin_prism_fisheye_img_from_cam(params: &[f64], p: &Vec3) -> Option { + let (fx, fy, cx, cy) = (params[0], params[1], params[2], params[3]); + let (k1, k2, p1, p2) = (params[4], params[5], params[6], params[7]); + let (k3, k4, sx1, sy1) = (params[8], params[9], params[10], params[11]); + let u = p.x / p.z; + let v = p.y / p.z; + let (fu, fv) = fisheye_from_normal(u, v); + let (du, dv) = thin_prism_fisheye_distort(k1, k2, p1, p2, k3, k4, sx1, sy1, fu, fv); + Some(Vec2::new(fx * (fu + du) + cx, fy * (fv + dv) + cy)) +} + +fn thin_prism_fisheye_cam_from_img(params: &[f64], xy: &Vec2) -> Option { + let (fx, fy, cx, cy) = (params[0], params[1], params[2], params[3]); + let (k1, k2, p1, p2) = (params[4], params[5], params[6], params[7]); + let (k3, k4, sx1, sy1) = (params[8], params[9], params[10], params[11]); + let tx = (xy.x - cx) / fx; + let ty = (xy.y - cy) / fy; + let fish = undistort( + &|uu, vv| thin_prism_fisheye_distort(k1, k2, p1, p2, k3, k4, sx1, sy1, uu, vv), + tx, + ty, + )?; + let (u, v) = normal_from_fisheye(fish.x, fish.y); + Some(Vec2::new(u, v)) +} + +// --------------------------------------------------------------------------- +// RAD_TAN_THIN_PRISM_FISHEYE (Project Aria, 16 parameters). +// +// Layout: fx, fy, cx, cy, k1, k2, k3, k4, k5, k6, p1, p2, sx1, sy1, sx2, sy2. +// +// Following COLMAP's `RadTanThinPrismFisheyeCameraModel`: the fisheye radial +// term uses up to six radial coefficients in theta, plus tangential (p1, p2) +// and thin-prism (sx1, sy1, sx2, sy2) terms. +// --------------------------------------------------------------------------- + +#[allow(clippy::too_many_arguments)] +#[inline] +fn rad_tan_thin_prism_fisheye_distort(params: &[f64], uu: f64, vv: f64) -> (f64, f64) { + let k1 = params[4]; + let k2 = params[5]; + let k3 = params[6]; + let k4 = params[7]; + let k5 = params[8]; + let k6 = params[9]; + let p1 = params[10]; + let p2 = params[11]; + let sx1 = params[12]; + let sy1 = params[13]; + let sx2 = params[14]; + let sy2 = params[15]; + + let u2 = uu * uu; + let v2 = vv * vv; + let uv = uu * vv; + let r2 = u2 + v2; + let r4 = r2 * r2; + let r6 = r4 * r2; + let r8 = r4 * r4; + let r10 = r8 * r2; + let r12 = r6 * r6; + + let radial = k1 * r2 + k2 * r4 + k3 * r6 + k4 * r8 + k5 * r10 + k6 * r12; + let du = uu * radial + 2.0 * p1 * uv + p2 * (r2 + 2.0 * u2) + sx1 * r2 + sx2 * r4; + let dv = vv * radial + 2.0 * p2 * uv + p1 * (r2 + 2.0 * v2) + sy1 * r2 + sy2 * r4; + (du, dv) +} + +fn rad_tan_thin_prism_fisheye_img_from_cam(params: &[f64], p: &Vec3) -> Option { + let (fx, fy, cx, cy) = (params[0], params[1], params[2], params[3]); + let u = p.x / p.z; + let v = p.y / p.z; + let (fu, fv) = fisheye_from_normal(u, v); + let (du, dv) = rad_tan_thin_prism_fisheye_distort(params, fu, fv); + Some(Vec2::new(fx * (fu + du) + cx, fy * (fv + dv) + cy)) +} + +fn rad_tan_thin_prism_fisheye_cam_from_img(params: &[f64], xy: &Vec2) -> Option { + let (fx, fy, cx, cy) = (params[0], params[1], params[2], params[3]); + let tx = (xy.x - cx) / fx; + let ty = (xy.y - cy) / fy; + let fish = undistort( + &|uu, vv| rad_tan_thin_prism_fisheye_distort(params, uu, vv), + tx, + ty, + )?; + let (u, v) = normal_from_fisheye(fish.x, fish.y); + Some(Vec2::new(u, v)) +} + +// --------------------------------------------------------------------------- +// SIMPLE_DIVISION / DIVISION (Fitzgibbon division model, closed-form). +// --------------------------------------------------------------------------- + +fn simple_division_img_from_cam(params: &[f64], p: &Vec3) -> Option { + let (f, cx, cy, k) = (params[0], params[1], params[2], params[3]); + division_img_from_cam_inner(f, f, cx, cy, k, p) +} + +fn simple_division_cam_from_img(params: &[f64], xy: &Vec2) -> Option { + let (f, cx, cy, k) = (params[0], params[1], params[2], params[3]); + Some(division_cam_from_img_inner(f, f, cx, cy, k, xy)) +} + +fn division_img_from_cam(params: &[f64], p: &Vec3) -> Option { + let (fx, fy, cx, cy, k) = (params[0], params[1], params[2], params[3], params[4]); + division_img_from_cam_inner(fx, fy, cx, cy, k, p) +} + +fn division_cam_from_img(params: &[f64], xy: &Vec2) -> Option { + let (fx, fy, cx, cy, k) = (params[0], params[1], params[2], params[3], params[4]); + Some(division_cam_from_img_inner(fx, fy, cx, cy, k, xy)) +} + +/// Forward projection for the division model. The undistorted normalized point +/// `(u, v)` maps to distorted `(u', v') = r * (u, v)` where +/// `r = 2 / (w + sqrt(w^2 - 4 rho^2 k))`, `rho = hypot(u, v)`, `w = Z`. +fn division_img_from_cam_inner(fx: f64, fy: f64, cx: f64, cy: f64, k: f64, p: &Vec3) -> Option { + let w = p.z; + // Work in normalized coords (divide through by z handled via rho on X,Y). + let u = p.x / w; + let v = p.y / w; + let rho2 = u * u + v * v; + let disc = 1.0 - 4.0 * rho2 * k; + if disc < 0.0 { + return None; + } + let r = 2.0 / (1.0 + disc.sqrt()); + Some(Vec2::new(fx * r * u + cx, fy * r * v + cy)) +} + +/// Closed-form inverse for the division model. +fn division_cam_from_img_inner(fx: f64, fy: f64, cx: f64, cy: f64, k: f64, xy: &Vec2) -> Vec2 { + let x0 = (xy.x - cx) / fx; + let y0 = (xy.y - cy) / fy; + let r2 = x0 * x0 + y0 * y0; + let denom = 1.0 + k * r2; + Vec2::new(x0 / denom, y0 / denom) +} + +// --------------------------------------------------------------------------- +// EUCM (enhanced unified camera model). Closed-form forward and inverse. +// +// Params: fx, fy, cx, cy, alpha, beta. Projection of a camera point (X, Y, Z): +// d = sqrt(beta * (X^2 + Y^2) + Z^2) +// denom = alpha * d + (1 - alpha) * Z +// x = fx * X / denom + cx; y = fy * Y / denom + cy. +// Inverse maps a normalized point (mx, my) back to a unit ray, then to (u, v). +// --------------------------------------------------------------------------- + +fn eucm_img_from_cam(params: &[f64], p: &Vec3) -> Option { + let (fx, fy, cx, cy, alpha, beta) = ( + params[0], params[1], params[2], params[3], params[4], params[5], + ); + let x = p.x; + let y = p.y; + let z = p.z; + let d = (beta * (x * x + y * y) + z * z).sqrt(); + let denom = alpha * d + (1.0 - alpha) * z; + if denom <= 0.0 { + return None; + } + Some(Vec2::new(fx * x / denom + cx, fy * y / denom + cy)) +} + +fn eucm_cam_from_img(params: &[f64], xy: &Vec2) -> Option { + let (fx, fy, cx, cy, alpha, beta) = ( + params[0], params[1], params[2], params[3], params[4], params[5], + ); + let mx = (xy.x - cx) / fx; + let my = (xy.y - cy) / fy; + let r2 = mx * mx + my * my; + let gamma = 1.0 - alpha; + // mz solves: solving for the z-component of the unit ray. + // From x = mx = X / (alpha d + gamma Z) with the unit-sphere constraint. + let inner = 1.0 - (2.0 * alpha - 1.0) * beta * r2; + if inner < 0.0 { + return None; + } + let mz = (1.0 - beta * alpha * alpha * r2) / (alpha * inner.sqrt() + gamma); + // Normalize (mx, my, mz) to the perspective plane (divide by mz). + if mz.abs() < 1e-12 { + return None; + } + Some(Vec2::new(mx / mz, my / mz)) +} + +// =========================================================================== +// Tests. +// =========================================================================== + +#[cfg(test)] +mod tests { + use super::*; + + /// Reasonable parameter set for each model, with the correct length. + fn params_for(model: CameraModelId) -> Vec { + match model { + CameraModelId::SimplePinhole => vec![500.0, 320.0, 240.0], + CameraModelId::Pinhole => vec![500.0, 520.0, 320.0, 240.0], + CameraModelId::SimpleRadial => vec![500.0, 320.0, 240.0, 0.02], + CameraModelId::Radial => vec![500.0, 320.0, 240.0, 0.02, -0.005], + CameraModelId::OpenCV => { + vec![500.0, 520.0, 320.0, 240.0, 0.02, -0.005, 0.001, -0.001] + } + CameraModelId::OpenCVFisheye => { + vec![500.0, 520.0, 320.0, 240.0, 0.02, -0.005, 0.001, -0.0005] + } + CameraModelId::FullOpenCV => vec![ + 500.0, 520.0, 320.0, 240.0, 0.02, -0.005, 0.001, -0.001, 0.0001, 0.001, 0.0005, + 0.0002, + ], + CameraModelId::FOV => vec![500.0, 520.0, 320.0, 240.0, 0.5], + CameraModelId::SimpleRadialFisheye => vec![500.0, 320.0, 240.0, 0.02], + CameraModelId::RadialFisheye => vec![500.0, 320.0, 240.0, 0.02, -0.005], + CameraModelId::ThinPrismFisheye => vec![ + 500.0, 520.0, 320.0, 240.0, 0.02, -0.005, 0.001, -0.001, 0.0001, 0.0002, 0.0003, + 0.0004, + ], + CameraModelId::RadTanThinPrismFisheye => vec![ + 500.0, 520.0, 320.0, 240.0, 0.02, -0.005, 0.001, -0.0005, 0.0001, 0.00005, 0.001, + -0.001, 0.0003, 0.0004, 0.00001, 0.00002, + ], + CameraModelId::SimpleDivision => vec![500.0, 320.0, 240.0, -0.05], + CameraModelId::Division => vec![500.0, 520.0, 320.0, 240.0, -0.05], + CameraModelId::SimpleFisheye => vec![500.0, 320.0, 240.0], + CameraModelId::Fisheye => vec![500.0, 520.0, 320.0, 240.0], + CameraModelId::EUCM => vec![500.0, 520.0, 320.0, 240.0, 0.6, 1.1], + } + } + + #[test] + fn params_lengths_match_registry() { + for &m in &CameraModelId::ALL { + assert_eq!(params_for(m).len(), m.num_params(), "{}", m.name()); + } + } + + #[test] + fn pinhole_projects_exactly() { + let p = params_for(CameraModelId::SimplePinhole); + let xy = img_from_cam( + CameraModelId::SimplePinhole, + &p, + &Vec3::new(2.0, -1.0, 4.0), + ) + .unwrap(); + // u = 0.5, v = -0.25 -> x = 500*0.5 + 320 = 570, y = 500*-0.25 + 240 = 115. + assert!((xy.x - 570.0).abs() < 1e-9); + assert!((xy.y - 115.0).abs() < 1e-9); + } + + #[test] + fn pinhole_two_focal_lengths() { + let p = params_for(CameraModelId::Pinhole); + let xy = img_from_cam(CameraModelId::Pinhole, &p, &Vec3::new(1.0, 1.0, 2.0)).unwrap(); + // u = v = 0.5 -> x = 500*0.5+320 = 570, y = 520*0.5+240 = 500. + assert!((xy.x - 570.0).abs() < 1e-9); + assert!((xy.y - 500.0).abs() < 1e-9); + } + + /// Forward-then-inverse should recover the normalized point for all models. + #[test] + fn round_trip_cam_img_cam_all_models() { + // Several normalized directions inside a sane field of view. + let dirs = [ + Vec3::new(0.0, 0.0, 1.0), + Vec3::new(0.1, 0.05, 1.0), + Vec3::new(-0.15, 0.2, 1.0), + Vec3::new(0.3, -0.25, 1.0), + ]; + for &m in &CameraModelId::ALL { + let p = params_for(m); + for d in dirs { + let xy = match img_from_cam(m, &p, &d) { + Some(xy) => xy, + None => continue, + }; + let uv = match cam_from_img(m, &p, &xy) { + Some(uv) => uv, + None => panic!("cam_from_img returned None for {}", m.name()), + }; + let expect = Vec2::new(d.x / d.z, d.y / d.z); + let err = (uv - expect).norm(); + assert!( + err < 1e-6, + "round-trip failed for {} dir {:?}: got {:?} expected {:?} (err {})", + m.name(), + d, + uv, + expect, + err + ); + } + } + } + + #[test] + fn round_trip_img_cam_img_distorted() { + // OpenCV (distorted) round-trip in pixel space. + let m = CameraModelId::OpenCV; + let p = params_for(m); + let pixels = [ + Vec2::new(320.0, 240.0), + Vec2::new(400.0, 300.0), + Vec2::new(200.0, 180.0), + ]; + for px in pixels { + let uv = cam_from_img(m, &p, &px).unwrap(); + let back = img_from_cam(m, &p, &Vec3::new(uv.x, uv.y, 1.0)).unwrap(); + assert!( + (back - px).norm() < 1e-5, + "pixel round-trip failed: {:?} -> {:?}", + px, + back + ); + } + } + + #[test] + fn cam_ray_is_unit_and_consistent() { + let m = CameraModelId::Radial; + let p = params_for(m); + let px = Vec2::new(400.0, 300.0); + let ray = cam_ray_from_img(m, &p, &px).unwrap(); + assert!((ray.norm() - 1.0).abs() < 1e-12); + let uv = cam_from_img(m, &p, &px).unwrap(); + let expected = Vec3::new(uv.x, uv.y, 1.0).normalize(); + assert!((ray - expected).norm() < 1e-12); + } + + #[test] + fn threshold_uses_mean_focal_length() { + let p = params_for(CameraModelId::Pinhole); + let t = img_from_cam_threshold(CameraModelId::Pinhole, &p, 3.0); + let mean = (500.0 + 520.0) / 2.0; + assert!((t - 3.0 / mean).abs() < 1e-12); + + let p2 = params_for(CameraModelId::SimplePinhole); + let t2 = img_from_cam_threshold(CameraModelId::SimplePinhole, &p2, 2.0); + assert!((t2 - 2.0 / 500.0).abs() < 1e-12); + } + + #[test] + fn wrong_param_length_returns_none() { + let bad = [1.0, 2.0]; + assert!(img_from_cam(CameraModelId::Pinhole, &bad, &Vec3::new(0.0, 0.0, 1.0)).is_none()); + assert!(cam_from_img(CameraModelId::Pinhole, &bad, &Vec2::new(0.0, 0.0)).is_none()); + assert_eq!( + img_from_cam_threshold(CameraModelId::Pinhole, &bad, 1.0), + 0.0 + ); + } + + #[test] + fn fov_zero_omega_is_pinhole_like() { + // As omega -> 0, FOV degenerates to a plain pinhole projection. + let p = vec![500.0, 500.0, 320.0, 240.0, 1e-12]; + let xy = img_from_cam(CameraModelId::FOV, &p, &Vec3::new(0.2, 0.1, 1.0)).unwrap(); + assert!((xy.x - (500.0 * 0.2 + 320.0)).abs() < 1e-3); + assert!((xy.y - (500.0 * 0.1 + 240.0)).abs() < 1e-3); + } + + #[test] + fn division_disc_negative_returns_none() { + // Strong positive k with a wide-angle ray can push the discriminant + // negative; ensure we report None rather than NaN. + let p = vec![500.0, 320.0, 240.0, 10.0]; + let res = img_from_cam(CameraModelId::SimpleDivision, &p, &Vec3::new(1.0, 1.0, 1.0)); + assert!(res.is_none()); + } + + #[test] + fn eucm_round_trip_center() { + let m = CameraModelId::EUCM; + let p = params_for(m); + let xy = img_from_cam(m, &p, &Vec3::new(0.0, 0.0, 1.0)).unwrap(); + // The optical center projects to the principal point. + assert!((xy.x - 320.0).abs() < 1e-9); + assert!((xy.y - 240.0).abs() < 1e-9); + let uv = cam_from_img(m, &p, &xy).unwrap(); + assert!(uv.norm() < 1e-9); + } +} diff --git a/src/sfm/mod.rs b/src/sfm/mod.rs new file mode 100644 index 0000000..53c658a --- /dev/null +++ b/src/sfm/mod.rs @@ -0,0 +1,175 @@ +//! Incremental Structure-from-Motion (SfM). +//! +//! This module mirrors the high-level incremental mapping pipeline of +//! [PyCOLMAP](https://colmap.github.io/pycolmap/) (`pycolmap.incremental_mapping` +//! and the underlying `IncrementalMapper` / `IncrementalPipeline` controllers). +//! +//! It exposes the **option structs** and **driver function signatures** that the +//! C++ implementation uses, so that calling code can be written and type-checked +//! against the stable public surface. The heavy numerical core — robust two-view +//! initialization, absolute-pose registration, triangulation and bundle +//! adjustment scheduling — is not ported in this pure-Rust crate yet and the +//! driver functions therefore return [`crate::Error::Unimplemented`]. + +use std::path::Path; + +use crate::scene::Reconstruction; + +/// Options that control a single run of the incremental mapper. +/// +/// These mirror a subset of COLMAP's `IncrementalMapperOptions`. Only the most +/// load-bearing knobs are surfaced here; the defaults match COLMAP's defaults. +#[derive(Debug, Clone, PartialEq)] +#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))] +pub struct IncrementalMapperOptions { + /// Minimum number of two-view inliers required to initialize the model + /// from the seed image pair. + pub init_min_num_inliers: usize, + /// Minimum number of inliers required to accept an absolute-pose + /// (image registration) estimate. + pub abs_pose_min_num_inliers: usize, + /// Minimum number of feature matches an image pair must have to be used + /// at all during mapping. + pub min_num_matches: usize, + /// Whether to reconstruct multiple disjoint models from the remaining, + /// not-yet-registered images. + pub multiple_models: bool, + /// Upper bound on the number of distinct models to reconstruct when + /// [`multiple_models`](Self::multiple_models) is enabled. + pub max_num_models: usize, +} + +impl Default for IncrementalMapperOptions { + fn default() -> Self { + Self { + init_min_num_inliers: 100, + abs_pose_min_num_inliers: 30, + min_num_matches: 15, + multiple_models: true, + max_num_models: 50, + } + } +} + +/// Options that control the full incremental SfM pipeline. +/// +/// Mirrors a subset of COLMAP's `IncrementalPipelineOptions`, which wraps an +/// [`IncrementalMapperOptions`] together with pipeline-level scheduling knobs +/// such as how often global bundle adjustment is triggered. +#[derive(Debug, Clone, PartialEq)] +#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))] +pub struct IncrementalPipelineOptions { + /// Minimum number of feature matches an image pair must have to be used. + pub min_num_matches: usize, + /// The growth ratio of registered images that triggers a new round of + /// global bundle adjustment (e.g. `1.1` => re-run when the model grows + /// by 10%). + pub ba_global_images_ratio: f64, + /// Options forwarded to the underlying incremental mapper. + pub mapper: IncrementalMapperOptions, +} + +impl Default for IncrementalPipelineOptions { + fn default() -> Self { + Self { + min_num_matches: 15, + ba_global_images_ratio: 1.1, + mapper: IncrementalMapperOptions::default(), + } + } +} + +/// Run the full incremental Structure-from-Motion pipeline. +/// +/// Reads features and matches from the COLMAP feature database at +/// `database_path`, registers images one by one, triangulates points and +/// bundle-adjusts, writing the resulting sparse model(s) under `output_path`. +/// Mirrors `pycolmap.incremental_mapping`. +/// +/// # Arguments +/// +/// * `database_path` — path to the SQLite feature database. +/// * `image_path` — root directory containing the source images. +/// * `output_path` — directory under which reconstructed model(s) are written. +/// * `options` — pipeline configuration. +/// +/// # Errors +/// +/// Returns [`crate::Error::Unimplemented`]; the numerical SfM core is not ported +/// in this pure-Rust crate. +pub fn incremental_mapping( + database_path: &Path, + image_path: &Path, + output_path: &Path, + options: &IncrementalPipelineOptions, +) -> crate::Result> { + let _ = (database_path, image_path, output_path, options); + Err(crate::Error::Unimplemented("incremental SfM mapping")) +} + +/// Triangulate additional 3D points into an existing reconstruction. +/// +/// Given an already-registered set of images (poses fixed), this completes and +/// merges feature tracks read from the database into new and existing 3D points. +/// Mirrors `pycolmap.triangulate_points`. +/// +/// # Errors +/// +/// Returns [`crate::Error::Unimplemented`]; triangulation is not ported in this +/// pure-Rust crate. +pub fn triangulate_points( + rec: &mut Reconstruction, + database_path: &Path, +) -> crate::Result<()> { + let _ = (rec, database_path); + Err(crate::Error::Unimplemented("point triangulation")) +} + +#[cfg(test)] +mod tests { + use super::*; + use std::path::Path; + + #[test] + fn mapper_options_defaults() { + let o = IncrementalMapperOptions::default(); + assert_eq!(o.init_min_num_inliers, 100); + assert_eq!(o.abs_pose_min_num_inliers, 30); + assert_eq!(o.min_num_matches, 15); + assert!(o.multiple_models); + assert_eq!(o.max_num_models, 50); + } + + #[test] + fn pipeline_options_defaults() { + let o = IncrementalPipelineOptions::default(); + assert_eq!(o.min_num_matches, 15); + assert!((o.ba_global_images_ratio - 1.1).abs() < 1e-12); + assert_eq!(o.mapper, IncrementalMapperOptions::default()); + } + + #[test] + fn incremental_mapping_is_unimplemented() { + let err = incremental_mapping( + Path::new("db.db"), + Path::new("images"), + Path::new("out"), + &IncrementalPipelineOptions::default(), + ) + .unwrap_err(); + assert!(matches!(err, crate::Error::Unimplemented(_))); + } + + #[test] + fn triangulate_points_is_unimplemented() { + let mut rec = Reconstruction { + rigs: Default::default(), + cameras: Default::default(), + frames: Default::default(), + images: Default::default(), + points3d: Default::default(), + }; + let err = triangulate_points(&mut rec, Path::new("db.db")).unwrap_err(); + assert!(matches!(err, crate::Error::Unimplemented(_))); + } +} diff --git a/src/types.rs b/src/types.rs new file mode 100644 index 0000000..195d707 --- /dev/null +++ b/src/types.rs @@ -0,0 +1,412 @@ +//! Fundamental identifier types, sentinel constants and the camera-model registry. +//! +//! These mirror `colmap/util/types.h` and `colmap/sensor/models.h`. They form the +//! *frozen contract* that every other module of the crate is built on: ids are +//! plain integer aliases (so they are `Copy`, hashable and cheap), invalid ids use +//! the maximum value of their integer type, and [`CameraModelId`] carries the +//! parameter layout that the file I/O and projection code both depend on. + +// --------------------------------------------------------------------------- +// Identifier aliases (see colmap/util/types.h) +// --------------------------------------------------------------------------- + +/// Unique identifier of a [`Camera`](crate::scene::Camera) (`uint32_t`). +pub type CameraId = u32; +/// Unique identifier of an [`Image`](crate::scene::Image) (`uint32_t`). +pub type ImageId = u32; +/// Index of a 2D point within an image's feature list (`uint32_t`). +pub type Point2DIdx = u32; +/// Unique identifier of a 3D point (`uint64_t`). +pub type Point3DId = u64; +/// Unique identifier of a [`Frame`](crate::scene::Frame) (`uint32_t`). +pub type FrameId = u32; +/// Unique identifier of a [`Rig`](crate::scene::Rig) (`uint32_t`). +pub type RigId = u32; +/// Encoded identifier of an unordered image pair (`uint64_t`). +pub type ImagePairId = u64; +/// Unique identifier of a pose prior (`uint32_t`). +pub type PosePriorId = u32; + +/// Sentinel value for an invalid [`CameraId`]. +pub const INVALID_CAMERA_ID: CameraId = u32::MAX; +/// Sentinel value for an invalid [`ImageId`]. +pub const INVALID_IMAGE_ID: ImageId = u32::MAX; +/// Sentinel value for an invalid [`Point2DIdx`]. +pub const INVALID_POINT2D_IDX: Point2DIdx = u32::MAX; +/// Sentinel value for an invalid [`Point3DId`]. +pub const INVALID_POINT3D_ID: Point3DId = u64::MAX; +/// Sentinel value for an invalid [`FrameId`]. +pub const INVALID_FRAME_ID: FrameId = u32::MAX; +/// Sentinel value for an invalid [`RigId`]. +pub const INVALID_RIG_ID: RigId = u32::MAX; +/// Sentinel value for an invalid [`PosePriorId`]. +pub const INVALID_POSE_PRIOR_ID: PosePriorId = u32::MAX; + +/// Maximum number of images, used when encoding [`ImagePairId`]s. +/// +/// Equal to `i32::MAX` (`2_147_483_647`), matching COLMAP's `kMaxNumImages`. +pub const MAX_NUM_IMAGES: u64 = i32::MAX as u64; + +// --------------------------------------------------------------------------- +// Image pair id encoding (see colmap/scene/database.cc) +// --------------------------------------------------------------------------- + +/// Returns `true` if the pair `(image_id1, image_id2)` should be swapped so the +/// smaller id comes first when encoding a pair id. +#[inline] +pub fn should_swap_image_pair(image_id1: ImageId, image_id2: ImageId) -> bool { + image_id1 > image_id2 +} + +/// Encodes an unordered image pair into a single [`ImagePairId`]. +/// +/// The smaller id is always stored first, so the result is invariant to the +/// argument order. +#[inline] +pub fn image_pair_to_pair_id(image_id1: ImageId, image_id2: ImageId) -> ImagePairId { + if should_swap_image_pair(image_id1, image_id2) { + MAX_NUM_IMAGES * image_id2 as u64 + image_id1 as u64 + } else { + MAX_NUM_IMAGES * image_id1 as u64 + image_id2 as u64 + } +} + +/// Decodes an [`ImagePairId`] back into its two [`ImageId`]s `(image_id1, image_id2)`. +#[inline] +pub fn pair_id_to_image_pair(pair_id: ImagePairId) -> (ImageId, ImageId) { + let image_id2 = (pair_id % MAX_NUM_IMAGES) as ImageId; + let image_id1 = ((pair_id - image_id2 as u64) / MAX_NUM_IMAGES) as ImageId; + (image_id1, image_id2) +} + +// --------------------------------------------------------------------------- +// Sensors (see colmap/util/types.h) +// --------------------------------------------------------------------------- + +/// The kind of a sensor in a rig. +#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash, PartialOrd, Ord)] +#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))] +#[repr(i32)] +pub enum SensorType { + /// Unset / invalid sensor. + Invalid = -1, + /// A camera sensor. + Camera = 0, + /// An inertial measurement unit. + Imu = 1, +} + +impl SensorType { + /// The integer code used in files and the database. + #[inline] + pub fn id(self) -> i32 { + self as i32 + } + + /// Builds a [`SensorType`] from its integer code, if valid. + #[inline] + pub fn from_id(id: i32) -> Option { + match id { + -1 => Some(Self::Invalid), + 0 => Some(Self::Camera), + 1 => Some(Self::Imu), + _ => None, + } + } + + /// The uppercase string used in text model files (`"CAMERA"`, `"IMU"`, `"INVALID"`). + #[inline] + pub fn name(self) -> &'static str { + match self { + Self::Invalid => "INVALID", + Self::Camera => "CAMERA", + Self::Imu => "IMU", + } + } + + /// Parses a [`SensorType`] from its text-file name. + #[inline] + pub fn from_name(name: &str) -> Option { + match name { + "INVALID" => Some(Self::Invalid), + "CAMERA" => Some(Self::Camera), + "IMU" => Some(Self::Imu), + _ => None, + } + } +} + +/// Identifies one sensor within a rig: its [`SensorType`] plus a per-type id. +/// +/// Mirrors COLMAP's `sensor_t`. For a camera sensor the `id` is the [`CameraId`]. +#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash, PartialOrd, Ord)] +#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))] +pub struct Sensor { + /// The sensor kind. + pub sensor_type: SensorType, + /// The per-type identifier (e.g. a [`CameraId`] when `sensor_type == Camera`). + pub id: u32, +} + +impl Sensor { + /// Creates a new sensor identifier. + #[inline] + pub fn new(sensor_type: SensorType, id: u32) -> Self { + Self { sensor_type, id } + } + + /// Convenience constructor for a camera sensor. + #[inline] + pub fn camera(camera_id: CameraId) -> Self { + Self::new(SensorType::Camera, camera_id) + } +} + +/// Identifies a single measurement of a sensor (e.g. one image of a camera). +/// +/// Mirrors COLMAP's `data_t`. For an image, `id` is the [`ImageId`]. +#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash, PartialOrd, Ord)] +#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))] +pub struct DataId { + /// The sensor that produced this measurement. + pub sensor: Sensor, + /// The per-sensor measurement id (e.g. an [`ImageId`]). + pub id: u64, +} + +impl DataId { + /// Creates a new data identifier. + #[inline] + pub fn new(sensor: Sensor, id: u64) -> Self { + Self { sensor, id } + } +} + +// --------------------------------------------------------------------------- +// Camera-model registry (see colmap/sensor/models.h) +// --------------------------------------------------------------------------- + +/// All camera models supported by COLMAP, with their stable integer ids. +/// +/// The integer value of each variant is the on-disk `model_id`. The parameter +/// layout (focal length / principal point / extra distortion indices) is exposed +/// through [`CameraModelId::focal_length_idxs`], [`principal_point_idxs`] and +/// [`extra_params_idxs`], and is shared by the projection code ([`crate::sensor`]) +/// and the file/database I/O. +/// +/// [`principal_point_idxs`]: CameraModelId::principal_point_idxs +/// [`extra_params_idxs`]: CameraModelId::extra_params_idxs +#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash, PartialOrd, Ord)] +#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))] +#[repr(i32)] +#[allow(missing_docs)] // variants are documented by the COLMAP model names below +pub enum CameraModelId { + /// `f, cx, cy` — ideal pinhole, shared focal length. + SimplePinhole = 0, + /// `fx, fy, cx, cy` — ideal pinhole. + Pinhole = 1, + /// `f, cx, cy, k` — one radial distortion term. + SimpleRadial = 2, + /// `f, cx, cy, k1, k2` — two radial distortion terms. + Radial = 3, + /// `fx, fy, cx, cy, k1, k2, p1, p2` — OpenCV radial + tangential. + OpenCV = 4, + /// `fx, fy, cx, cy, k1, k2, k3, k4` — OpenCV fisheye. + OpenCVFisheye = 5, + /// `fx, fy, cx, cy, k1, k2, p1, p2, k3, k4, k5, k6` — rational OpenCV. + FullOpenCV = 6, + /// `fx, fy, cx, cy, omega` — field-of-view model. + FOV = 7, + /// `f, cx, cy, k` — single-parameter radial fisheye. + SimpleRadialFisheye = 8, + /// `f, cx, cy, k1, k2` — two-parameter radial fisheye. + RadialFisheye = 9, + /// `fx, fy, cx, cy, k1, k2, p1, p2, k3, k4, sx1, sy1` — thin-prism fisheye. + ThinPrismFisheye = 10, + /// 16-parameter radial-tangential thin-prism fisheye (Project Aria). + RadTanThinPrismFisheye = 11, + /// `f, cx, cy, k` — Fitzgibbon division model (closed-form). + SimpleDivision = 12, + /// `fx, fy, cx, cy, k` — division model (closed-form). + Division = 13, + /// `f, cx, cy` — equidistant fisheye, no distortion. + SimpleFisheye = 14, + /// `fx, fy, cx, cy` — equidistant fisheye, no distortion. + Fisheye = 15, + /// `fx, fy, cx, cy, alpha, beta` — enhanced unified camera model. + EUCM = 16, +} + +impl CameraModelId { + /// Every camera model, in id order. + pub const ALL: [CameraModelId; 17] = [ + Self::SimplePinhole, + Self::Pinhole, + Self::SimpleRadial, + Self::Radial, + Self::OpenCV, + Self::OpenCVFisheye, + Self::FullOpenCV, + Self::FOV, + Self::SimpleRadialFisheye, + Self::RadialFisheye, + Self::ThinPrismFisheye, + Self::RadTanThinPrismFisheye, + Self::SimpleDivision, + Self::Division, + Self::SimpleFisheye, + Self::Fisheye, + Self::EUCM, + ]; + + /// The on-disk integer `model_id`. + #[inline] + pub fn id(self) -> i32 { + self as i32 + } + + /// Builds a model from its integer `model_id`, if known. + #[inline] + pub fn from_id(id: i32) -> Option { + Self::ALL.into_iter().find(|m| m.id() == id) + } + + /// The uppercase model name used in text files (e.g. `"SIMPLE_RADIAL"`). + #[inline] + pub fn name(self) -> &'static str { + match self { + Self::SimplePinhole => "SIMPLE_PINHOLE", + Self::Pinhole => "PINHOLE", + Self::SimpleRadial => "SIMPLE_RADIAL", + Self::Radial => "RADIAL", + Self::OpenCV => "OPENCV", + Self::OpenCVFisheye => "OPENCV_FISHEYE", + Self::FullOpenCV => "FULL_OPENCV", + Self::FOV => "FOV", + Self::SimpleRadialFisheye => "SIMPLE_RADIAL_FISHEYE", + Self::RadialFisheye => "RADIAL_FISHEYE", + Self::ThinPrismFisheye => "THIN_PRISM_FISHEYE", + Self::RadTanThinPrismFisheye => "RAD_TAN_THIN_PRISM_FISHEYE", + Self::SimpleDivision => "SIMPLE_DIVISION", + Self::Division => "DIVISION", + Self::SimpleFisheye => "SIMPLE_FISHEYE", + Self::Fisheye => "FISHEYE", + Self::EUCM => "EUCM", + } + } + + /// Parses a model from its text-file name. + #[inline] + pub fn from_name(name: &str) -> Option { + Self::ALL.into_iter().find(|m| m.name() == name) + } + + /// Total number of intrinsic parameters for this model. + #[inline] + pub fn num_params(self) -> usize { + self.focal_length_idxs().len() + + self.principal_point_idxs().len() + + self.extra_params_idxs().len() + } + + /// Indices (into the params vector) of the focal-length parameters. + #[inline] + pub fn focal_length_idxs(self) -> &'static [usize] { + match self { + Self::SimplePinhole + | Self::SimpleRadial + | Self::Radial + | Self::SimpleRadialFisheye + | Self::RadialFisheye + | Self::SimpleDivision + | Self::SimpleFisheye => &[0], + _ => &[0, 1], + } + } + + /// Indices (into the params vector) of the principal-point parameters. + #[inline] + pub fn principal_point_idxs(self) -> &'static [usize] { + match self { + Self::SimplePinhole + | Self::SimpleRadial + | Self::Radial + | Self::SimpleRadialFisheye + | Self::RadialFisheye + | Self::SimpleDivision + | Self::SimpleFisheye => &[1, 2], + _ => &[2, 3], + } + } + + /// Indices (into the params vector) of the extra (distortion) parameters. + #[inline] + pub fn extra_params_idxs(self) -> &'static [usize] { + match self { + Self::SimplePinhole | Self::Pinhole | Self::SimpleFisheye | Self::Fisheye => &[], + Self::SimpleRadial | Self::SimpleRadialFisheye | Self::SimpleDivision => &[3], + Self::Radial | Self::RadialFisheye => &[3, 4], + Self::Division => &[4], + Self::FOV => &[4], + Self::EUCM => &[4, 5], + Self::OpenCV | Self::OpenCVFisheye => &[4, 5, 6, 7], + Self::FullOpenCV | Self::ThinPrismFisheye => &[4, 5, 6, 7, 8, 9, 10, 11], + Self::RadTanThinPrismFisheye => { + &[4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15] + } + } + } +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn pair_id_roundtrips() { + for (a, b) in [(1u32, 2u32), (2, 1), (5, 5), (1000, 7)] { + let pid = image_pair_to_pair_id(a, b); + let (x, y) = pair_id_to_image_pair(pid); + // Pair id is order-independent; smaller id comes first. + assert_eq!((x, y), (a.min(b), a.max(b))); + assert_eq!(image_pair_to_pair_id(a, b), image_pair_to_pair_id(b, a)); + } + } + + #[test] + fn camera_model_metadata_is_consistent() { + let expected = [ + (CameraModelId::SimplePinhole, "SIMPLE_PINHOLE", 3), + (CameraModelId::Pinhole, "PINHOLE", 4), + (CameraModelId::SimpleRadial, "SIMPLE_RADIAL", 4), + (CameraModelId::Radial, "RADIAL", 5), + (CameraModelId::OpenCV, "OPENCV", 8), + (CameraModelId::OpenCVFisheye, "OPENCV_FISHEYE", 8), + (CameraModelId::FullOpenCV, "FULL_OPENCV", 12), + (CameraModelId::FOV, "FOV", 5), + (CameraModelId::SimpleRadialFisheye, "SIMPLE_RADIAL_FISHEYE", 4), + (CameraModelId::RadialFisheye, "RADIAL_FISHEYE", 5), + (CameraModelId::ThinPrismFisheye, "THIN_PRISM_FISHEYE", 12), + (CameraModelId::RadTanThinPrismFisheye, "RAD_TAN_THIN_PRISM_FISHEYE", 16), + (CameraModelId::SimpleDivision, "SIMPLE_DIVISION", 4), + (CameraModelId::Division, "DIVISION", 5), + (CameraModelId::SimpleFisheye, "SIMPLE_FISHEYE", 3), + (CameraModelId::Fisheye, "FISHEYE", 4), + (CameraModelId::EUCM, "EUCM", 6), + ]; + for (model, name, num_params) in expected { + assert_eq!(model.name(), name); + assert_eq!(model.num_params(), num_params, "{name}"); + assert_eq!(CameraModelId::from_name(name), Some(model)); + assert_eq!(CameraModelId::from_id(model.id()), Some(model)); + } + } + + #[test] + fn sensor_type_roundtrips() { + for t in [SensorType::Invalid, SensorType::Camera, SensorType::Imu] { + assert_eq!(SensorType::from_id(t.id()), Some(t)); + assert_eq!(SensorType::from_name(t.name()), Some(t)); + } + } +} diff --git a/tests/pipeline_example.rs b/tests/pipeline_example.rs new file mode 100644 index 0000000..085116b --- /dev/null +++ b/tests/pipeline_example.rs @@ -0,0 +1,48 @@ +//! Integration tests for the documented pipeline examples (see the crate-level docs). +//! +//! These assert that the runnable parts of the examples behave as documented and +//! that the scaffolded steps report `Error::Unimplemented` rather than panicking. + +use std::path::Path; + +use colmap::math::{Vec2, Vec3}; +use colmap::scene::Camera; +use colmap::types::CameraModelId; +use colmap::Error; + +/// Example 1 — basic building blocks: a pinhole camera projects an on-axis point +/// onto its principal point. +#[test] +fn basic_building_blocks_example() { + let camera = Camera::new_with_model(1, CameraModelId::Pinhole, 800.0, 640, 480); + assert_eq!(camera.model_name(), "PINHOLE"); + assert_eq!((camera.width, camera.height), (640, 480)); + + let uv = camera.img_from_cam(&Vec3::new(0.0, 0.0, 1.0)).unwrap(); + assert_eq!(uv, Vec2::new(320.0, 240.0)); + + // Option structs construct with sane defaults. + let features = colmap::feature::FeatureExtractionOptions::default(); + assert!(features.sift.max_num_features > 0); +} + +/// Example 2 — the workflow's option structs compose as documented. +#[test] +fn workflow_options_compose() { + let sfm = colmap::sfm::IncrementalPipelineOptions::default(); + assert!(sfm.min_num_matches >= 1); + let mvs = colmap::mvs::PatchMatchOptions::default(); + assert!(mvs.window_radius >= 1); +} + +/// Example 3 — error handling: a scaffolded step returns `Error::Unimplemented`. +#[test] +fn scaffolded_step_reports_unimplemented() { + use colmap::mvs::{patch_match_stereo, PatchMatchOptions}; + + let result = patch_match_stereo(Path::new("/tmp/workspace"), &PatchMatchOptions::default()); + match result { + Err(Error::Unimplemented(what)) => assert_eq!(what, "dense patch-match stereo"), + other => panic!("expected Unimplemented, got {other:?}"), + } +}