sorlinv
/
colmap-rs
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Initial commit: pure-Rust COLMAP port

master
sorlinv 2026-06-15 14:06:42 +02:00
commit fc66f02c9c
51 changed files with 12837 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
.gitignore vendored Normal file
View File

@ -0,0 +1 @@
/target

772
Cargo.lock generated Normal file
View File

@ -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"

36
Cargo.toml Normal file
View File

@ -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"]

222
README.md Normal file
View File

@ -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.

89
examples/full_pipeline.rs Normal file
View File

@ -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);
}
}

54
examples/infer_camera.rs Normal file
View File

@ -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(())
}

67
examples/reconstruct.rs Normal file
View File

@ -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()));
}

911
src/database/mod.rs Normal file
View File

@ -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<Database> {
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<Database> {
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<Database> {
// `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<usize> {
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<usize> {
self.count("cameras")
}
/// Number of images stored in the database.
pub fn num_images(&self) -> crate::Result<usize> {
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<usize> {
let n: Option<i64> = 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<usize> {
let n: Option<i64> = 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<usize> {
let n: Option<i64> = 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<bool> {
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<bool> {
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<bool> {
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<bool> {
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<bool> {
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<bool> {
let sql = format!("SELECT 1 FROM {table} WHERE {key} = ?1");
let found: Option<i64> = 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<CameraId> {
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<Camera> {
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<Vec<Camera>> {
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<crate::Result<Camera>> {
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<u8> = 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(&params_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<ImageId>,
name: &str,
camera_id: CameraId,
) -> crate::Result<ImageId> {
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<Vec<(ImageId, String, CameraId)>> {
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::<LittleEndian>(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<FeatureKeypoints> {
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::<LittleEndian>()?;
let y = cursor.read_f32::<LittleEndian>()?;
let a11 = cursor.read_f32::<LittleEndian>()?;
let a12 = cursor.read_f32::<LittleEndian>()?;
let a21 = cursor.read_f32::<LittleEndian>()?;
let a22 = cursor.read_f32::<LittleEndian>()?;
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<FeatureDescriptors> {
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::<LittleEndian>(c0)?;
data.write_u32::<LittleEndian>(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<FeatureMatches> {
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::<LittleEndian>()?;
let c1 = cursor.read_u32::<LittleEndian>()?;
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<Option<(usize, usize, Vec<u8>)>> {
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<Option<(usize, usize, Vec<u8>)>> {
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<u8>)> {
let rows: i64 = row.get(0)?;
let cols: i64 = row.get(1)?;
let data: Option<Vec<u8>> = 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<u8> {
let mut blob = Vec::with_capacity(values.len() * 8);
for &v in values {
// Writing into a `Vec<u8>` via byteorder is infallible.
blob.write_f64::<LittleEndian>(v).expect("vec write is infallible");
}
blob
}
/// Decode a little-endian `f64` BLOB back into a `Vec<f64>`.
///
/// Returns [`crate::Error::Malformed`] if the byte length is not a multiple of 8.
fn blob_to_f64_vec(blob: &[u8]) -> crate::Result<Vec<f64>> {
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::<LittleEndian>()?);
}
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(_))
));
}
}

50
src/error.rs Normal file
View File

@ -0,0 +1,50 @@
//! Crate-wide error type.
//!
//! Every fallible operation in this crate returns [`Result<T>`], an alias for
//! `std::result::Result<T, `[`Error`]`>`.
/// 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<T> = std::result::Result<T, Error>;

463
src/estimators/mod.rs Normal file
View File

@ -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<bool>,
}
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<bool>) -> 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<Rigid3d> {
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<Sim3d> {
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<Vec3> {
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<Vec3> {
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<Vec3> = 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<Vec3> = 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<Vec3> = 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<Vec3> = Vec::new();
let empty2: Vec<Vec2> = 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(_))
));
}
}

350
src/feature/mod.rs Normal file
View File

@ -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<f32> {
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<FeatureKeypoints> {
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(_))
));
}
}

223
src/feature/types.rs Normal file
View File

@ -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<FeatureKeypoint>;
/// 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<FeatureMatch>;
/// 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<u8>,
}
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<u8>) -> crate::Result<Self> {
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());
}
}

237
src/geometry/essential_matrix.rs Normal file
View File

@ -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<Vec3>) {
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<Vec3> = 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<Vec3> = world_points.to_vec();
let rays2: Vec<Vec3> = 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);
}
}

217
src/geometry/gps.rs Normal file
View File

@ -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<Vec3> {
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<Vec3> {
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<Vec3> {
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<Vec3> {
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<Vec3> = 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);
}
}

280
src/geometry/homography_matrix.rs Normal file
View File

@ -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<Rigid3d>, Vec<Vec3>) {
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<Vec3>) {
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<Vec3> = points1
.iter()
.map(|p| k1_inv * Vec3::new(p.x, p.y, 1.0))
.collect();
let rays2: Vec<Vec3> = 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<Vec3> = 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<Rigid3d>, Vec<Vec3>) {
// 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<Vec2> = world_points
.iter()
.map(|p| Vec2::new(p.x / p.z, p.y / p.z))
.collect();
let points2: Vec<Vec2> = 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());
}
}

49
src/geometry/mod.rs Normal file
View File

@ -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,
};

145
src/geometry/normalization.rs Normal file
View File

@ -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::<f64>() / norm.len() as f64;
/// assert!(cx.abs() < 1e-12);
/// let _ = t;
/// ```
pub fn center_and_normalize_image_points(points: &[Vec2]) -> (Vec<Vec2>, 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<Vec2> = 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::<f64>() / 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());
}
}

439
src/geometry/pose.rs Normal file
View File

@ -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<Vec3>, Vec<usize>) {
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);
}
}

117
src/geometry/pose_prior.rs Normal file
View File

@ -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
);
}
}

157
src/geometry/rigid3.rs Normal file
View File

@ -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<Rigid3d> 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<Vec3> 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);
}
}

126
src/geometry/rotation.rs Normal file
View File

@ -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<UnitQuat> 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);
}
}

142
src/geometry/sim3.rs Normal file
View File

@ -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<Sim3d> 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<Vec3> 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);
}
}

252
src/geometry/triangulation.rs Normal file
View File

@ -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<Vec3> {
// Build the 4x4 system A X = 0, two rows per view.
let mut a = Matrix4::<f64>::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<f64> = 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<Vec3> {
// 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<Vec3> {
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::<f64>::zeros();
for i in 0..n {
let p = &proj_matrices[i];
let x = &points[i];
let row0: Vector4<f64> = (x.x * p.row(2) - p.row(0)).transpose();
let row1: Vector4<f64> = (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<Mat3x4> = poses.iter().map(|p| p.to_matrix()).collect();
let pts: Vec<Vec2> = 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);
}
}

1199
src/highlevel/mod.rs Normal file
View File

File diff suppressed because it is too large Load Diff

532
src/image/mod.rs Normal file
View File

@ -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<String>,
/// Camera model (EXIF tag `Model`).
pub model: Option<String>,
/// Focal length in millimetres (EXIF tag `FocalLength`).
pub focal_length_mm: Option<f64>,
/// 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<f64>,
/// Focal-plane resolution unit (EXIF tag `FocalPlaneResolutionUnit`):
/// `2 = inch`, `3 = centimetre`.
pub focal_plane_resolution_unit: Option<u16>,
/// EXIF `PixelXDimension`, the full-resolution image width the focal-plane
/// resolution refers to.
pub pixel_x_dimension: Option<u32>,
}
/// Reads the EXIF focal length in millimetres from a JPEG file, if present.
pub fn read_exif_focal_length_mm(path: &Path) -> Result<Option<f64>> {
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<Option<Exif>> {
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<Exif> {
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<u16> {
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<u32> {
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<Exif> {
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<usize> = 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<usize>,
) {
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<String> {
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<u32> {
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<f64> {
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<f64> {
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<Camera> {
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<Vec<PathBuf>> {
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<Vec<(String, Camera)>> {
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<Camera>, BTreeMap<String, CameraId>)> {
let mut cameras: Vec<Camera> = 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<PathBuf> {
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());
}
}

445
src/io/binary.rs Normal file
View File

@ -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<Reconstruction> {
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<CameraId, Camera>) -> Result<()> {
let mut reader = BufReader::new(File::open(path)?);
let num = reader.read_u64::<LittleEndian>()?;
for _ in 0..num {
let camera_id = reader.read_u32::<LittleEndian>()?;
let model_id_raw = reader.read_i32::<LittleEndian>()?;
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::<LittleEndian>()?;
let height = reader.read_u64::<LittleEndian>()?;
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::<LittleEndian>()?);
}
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<CameraId, Camera>) -> Result<()> {
let mut writer = BufWriter::new(File::create(path)?);
writer.write_u64::<LittleEndian>(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::<LittleEndian>(camera.camera_id)?;
writer.write_i32::<LittleEndian>(camera.model_id.id())?;
writer.write_u64::<LittleEndian>(camera.width)?;
writer.write_u64::<LittleEndian>(camera.height)?;
for &p in &camera.params {
writer.write_f64::<LittleEndian>(p)?;
}
}
writer.flush()?;
Ok(())
}
// ---------------------------------------------------------------------------
// images.bin
// ---------------------------------------------------------------------------
/// Reads `images.bin` into `images`.
fn read_images_binary(path: &Path, images: &mut HashMap<ImageId, Image>) -> Result<()> {
let mut reader = BufReader::new(File::open(path)?);
let num = reader.read_u64::<LittleEndian>()?;
for _ in 0..num {
let image_id = reader.read_u32::<LittleEndian>()?;
let qw = reader.read_f64::<LittleEndian>()?;
let qx = reader.read_f64::<LittleEndian>()?;
let qy = reader.read_f64::<LittleEndian>()?;
let qz = reader.read_f64::<LittleEndian>()?;
let tx = reader.read_f64::<LittleEndian>()?;
let ty = reader.read_f64::<LittleEndian>()?;
let tz = reader.read_f64::<LittleEndian>()?;
let camera_id = reader.read_u32::<LittleEndian>()?;
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::<LittleEndian>()?;
let mut points2d = Vec::with_capacity(num_points2d as usize);
for _ in 0..num_points2d {
let x = reader.read_f64::<LittleEndian>()?;
let y = reader.read_f64::<LittleEndian>()?;
let point3d_id = reader.read_u64::<LittleEndian>()?;
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<ImageId, Image>) -> Result<()> {
let mut writer = BufWriter::new(File::create(path)?);
writer.write_u64::<LittleEndian>(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::<LittleEndian>(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::<LittleEndian>(q.w)?;
writer.write_f64::<LittleEndian>(q.i)?;
writer.write_f64::<LittleEndian>(q.j)?;
writer.write_f64::<LittleEndian>(q.k)?;
writer.write_f64::<LittleEndian>(pose.translation.x)?;
writer.write_f64::<LittleEndian>(pose.translation.y)?;
writer.write_f64::<LittleEndian>(pose.translation.z)?;
writer.write_u32::<LittleEndian>(image.camera_id)?;
writer.write_all(image.name.as_bytes())?;
writer.write_u8(0)?; // NUL terminator
writer.write_u64::<LittleEndian>(image.points2d.len() as u64)?;
for p in &image.points2d {
writer.write_f64::<LittleEndian>(p.xy.x)?;
writer.write_f64::<LittleEndian>(p.xy.y)?;
writer.write_u64::<LittleEndian>(p.point3d_id)?;
}
}
writer.flush()?;
Ok(())
}
// ---------------------------------------------------------------------------
// points3D.bin
// ---------------------------------------------------------------------------
/// Reads `points3D.bin` into `points3d`.
fn read_points3d_binary(
path: &Path,
points3d: &mut HashMap<Point3DId, Point3D>,
) -> Result<()> {
let mut reader = BufReader::new(File::open(path)?);
let num = reader.read_u64::<LittleEndian>()?;
for _ in 0..num {
let point3d_id = reader.read_u64::<LittleEndian>()?;
let x = reader.read_f64::<LittleEndian>()?;
let y = reader.read_f64::<LittleEndian>()?;
let z = reader.read_f64::<LittleEndian>()?;
let r = reader.read_u8()?;
let g = reader.read_u8()?;
let b = reader.read_u8()?;
let error = reader.read_f64::<LittleEndian>()?;
let track_len = reader.read_u64::<LittleEndian>()?;
let mut elements = Vec::with_capacity(track_len as usize);
for _ in 0..track_len {
let image_id = reader.read_u32::<LittleEndian>()?;
let point2d_idx = reader.read_u32::<LittleEndian>()?;
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<Point3DId, Point3D>,
) -> Result<()> {
let mut writer = BufWriter::new(File::create(path)?);
writer.write_u64::<LittleEndian>(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::<LittleEndian>(*id)?;
writer.write_f64::<LittleEndian>(point.xyz.x)?;
writer.write_f64::<LittleEndian>(point.xyz.y)?;
writer.write_f64::<LittleEndian>(point.xyz.z)?;
writer.write_u8(point.color.x)?;
writer.write_u8(point.color.y)?;
writer.write_u8(point.color.z)?;
writer.write_f64::<LittleEndian>(point.error)?;
writer.write_u64::<LittleEndian>(point.track.elements.len() as u64)?;
for el in &point.track.elements {
writer.write_u32::<LittleEndian>(el.image_id)?;
writer.write_u32::<LittleEndian>(el.point2d_idx)?;
}
}
writer.flush()?;
Ok(())
}
// ---------------------------------------------------------------------------
// Helpers
// ---------------------------------------------------------------------------
/// Reads a NUL-terminated UTF-8 string from `reader`, consuming the terminator.
fn read_cstring<R: Read>(reader: &mut R) -> Result<String> {
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");
}
}

150
src/io/mod.rs Normal file
View File

@ -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<Reconstruction> {
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");
}
}

622
src/io/text.rs Normal file
View File

@ -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<Reconstruction> {
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<T: FromStr>(token: &str, context: &str) -> Result<T> {
token
.parse::<T>()
.map_err(|_| Error::Parse(format!("invalid {context}: {token:?}")))
}
// ---------------------------------------------------------------------------
// cameras.txt
// ---------------------------------------------------------------------------
/// Reads `cameras.txt` into `cameras`.
fn read_cameras_text(path: &Path, cameras: &mut HashMap<CameraId, Camera>) -> 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::<f64>(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<CameraId, Camera>) -> 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<ImageId, Image>) -> 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::<Vec<_>>().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::<f64>(t, "point2d x")?;
let y = parse_token::<f64>(
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<Point3DId> {
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::<u64>(token, "point3d id")
}
/// Writes `images` to `images.txt`, sorted by image id, two lines per image.
fn write_images_text(path: &Path, images: &HashMap<ImageId, Image>) -> 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<ImageId, Image>) -> 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<Point3DId, Point3D>,
) -> 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::<ImageId>(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<Point3DId, Point3D>,
) -> 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<Point3DId, Point3D>) -> 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<Item = &'a str>>(
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);
}
}

226
src/lib.rs Normal file
View File

@ -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,
};
}

37
src/math.rs Normal file
View File

@ -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<f64>;
/// A 3D column vector of `f64` (world / camera coordinates).
pub type Vec3 = Vector3<f64>;
/// A 4D column vector of `f64` (homogeneous coordinates, quaternion coeffs).
pub type Vec4 = Vector4<f64>;
/// An RGB color, one byte per channel, matching `Eigen::Vector3ub`.
pub type Rgb = Vector3<u8>;
/// A 3×3 matrix of `f64` (rotations, intrinsics `K`, `E`/`F`/`H`).
pub type Mat3 = Matrix3<f64>;
/// A 3×4 matrix of `f64` (a `[R | t]` pose / projection matrix).
pub type Mat3x4 = Matrix3x4<f64>;
/// A 4×4 matrix of `f64` (homogeneous transforms).
pub type Mat4 = Matrix4<f64>;
/// A raw quaternion of `f64` (`w` scalar, `i j k` vector parts).
pub type Quat = Quaternion<f64>;
/// A unit quaternion of `f64`, the canonical rotation representation.
pub type UnitQuat = UnitQuaternion<f64>;

221
src/mvs/mod.rs Normal file
View File

@ -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<f32>,
}
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<f32>,
}
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(_)
));
}
}

89
src/optim/mod.rs Normal file
View File

@ -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(_)));
}
}

149
src/pipeline/mod.rs Normal file
View File

@ -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(_)
));
}
}

142
src/retrieval/mod.rs Normal file
View File

@ -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<ImageId>,
}
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<Vec<ImageId>> {
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(_)));
}
}

320
src/scene/camera.rs Normal file
View File

@ -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<f64>,
/// 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<Vec2> {
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<Vec2> {
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<Vec3> {
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::<Vec<_>>()
.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);
}
}
}

164
src/scene/correspondence_graph.rs Normal file
View File

@ -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<ImageId, Vec<Vec<Correspondence>>>,
}
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);
}
}

106
src/scene/frame.rs Normal file
View File

@ -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<Rigid3d>,
/// The data ids (one per measuring sensor) captured in this frame.
pub data_ids: BTreeSet<DataId>,
}
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<ImageId> {
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());
}
}

161
src/scene/image.rs Normal file
View File

@ -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<Rigid3d>,
/// The 2D observations detected in this image.
pub points2d: Vec<Point2D>,
/// The id of the frame this image belongs to, if any.
pub frame_id: Option<FrameId>,
}
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<String>) -> 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<Vec3> {
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<Vec3> {
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<Vec2> {
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());
}
}

43
src/scene/mod.rs Normal file
View File

@ -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};

72
src/scene/point2d.rs Normal file
View File

@ -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());
}
}

65
src/scene/point3d.rs Normal file
View File

@ -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());
}
}

347
src/scene/reconstruction.rs Normal file
View File

@ -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<RigId, Rig>,
/// All cameras (intrinsics), keyed by [`CameraId`].
pub cameras: HashMap<CameraId, Camera>,
/// All frames (rig captures), keyed by [`FrameId`].
pub frames: HashMap<FrameId, Frame>,
/// All images, keyed by [`ImageId`].
pub images: HashMap<ImageId, Image>,
/// All triangulated 3D points, keyed by [`Point3DId`].
pub points3d: HashMap<Point3DId, Point3D>,
}
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<ImageId> {
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<Point3DId> {
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<Reconstruction> {
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<Reconstruction> {
crate::io::read_model_text(path)
}
/// Reads a model from `path` in the binary format.
pub fn read_binary(path: &Path) -> crate::Result<Reconstruction> {
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);
}
}

117
src/scene/rig.rs Normal file
View File

@ -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<Sensor, Option<Rigid3d>>,
}
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<Rigid3d>) {
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<Sensor> {
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);
}
}

102
src/scene/track.rs Normal file
View File

@ -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<TrackElement>,
}
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);
}
}

165
src/scene/two_view_geometry.rs Normal file
View File

@ -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<Self> {
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<Mat3>,
/// The fundamental matrix, if estimated.
pub f: Option<Mat3>,
/// The homography matrix, if estimated.
pub h: Option<Mat3>,
/// The relative pose `cam2_from_cam1`, if recovered.
pub cam2_from_cam1: Option<Rigid3d>,
/// 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)
);
}
}

264
src/sensor/bitmap.rs Normal file
View File

@ -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<u8>,
}
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<u8> {
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<P: AsRef<Path>>(_path: P) -> Result<Bitmap> {
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<P: AsRef<Path>>(&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());
}
}

19
src/sensor/mod.rs Normal file
View File

@ -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::*;

1046
src/sensor/models.rs Normal file
View File

File diff suppressed because it is too large Load Diff

175
src/sfm/mod.rs Normal file
View File

@ -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<Vec<Reconstruction>> {
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(_)));
}
}

412
src/types.rs Normal file
View File

@ -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<Self> {
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<Self> {
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> {
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> {
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));
}
}
}

48
tests/pipeline_example.rs Normal file
View File

@ -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:?}"),
}
}