colmap · sarlinpe · Jul 3, 2024 · Jun 27, 2024 · Jun 27, 2024 · Jun 27, 2024
diff --git a/scripts/python/database.py b/scripts/python/database.py
@@ -58,19 +58,18 @@
     image_id INTEGER PRIMARY KEY AUTOINCREMENT NOT NULL,
     name TEXT NOT NULL UNIQUE,
     camera_id INTEGER NOT NULL,
-    prior_qw REAL,
-    prior_qx REAL,
-    prior_qy REAL,
-    prior_qz REAL,
-    prior_tx REAL,
-    prior_ty REAL,
-    prior_tz REAL,
     CONSTRAINT image_id_check CHECK(image_id >= 0 and image_id < {}),
     FOREIGN KEY(camera_id) REFERENCES cameras(camera_id))
 """.format(
     MAX_IMAGE_ID
 )
 
+CREATE_POSE_PRIORS_TABLE = """CREATE TABLE IF NOT EXISTS pose_priors (
+    image_id INTEGER PRIMARY KEY NOT NULL,
+    position BLOB,
+    coordinate_system INTEGER NOT NULL,
+    FOREIGN KEY(image_id) REFERENCES images(image_id) ON DELETE CASCADE)"""
+
 CREATE_TWO_VIEW_GEOMETRIES_TABLE = """
 CREATE TABLE IF NOT EXISTS two_view_geometries (
     pair_id INTEGER PRIMARY KEY NOT NULL,
@@ -107,6 +106,7 @@
     [
         CREATE_CAMERAS_TABLE,
         CREATE_IMAGES_TABLE,
+        CREATE_POSE_PRIORS_TABLE,
         CREATE_KEYPOINTS_TABLE,
         CREATE_DESCRIPTORS_TABLE,
         CREATE_MATCHES_TABLE,
@@ -160,6 +160,9 @@ def __init__(self, *args, **kwargs):
         self.create_images_table = lambda: self.executescript(
             CREATE_IMAGES_TABLE
         )
+        self.create_pose_priors_table = lambda: self.executescript(
+            CREATE_POSE_PRIORS_TABLE
+        )
         self.create_two_view_geometries_table = lambda: self.executescript(
             CREATE_TWO_VIEW_GEOMETRIES_TABLE
         )
@@ -219,6 +222,13 @@ def add_image(
         )
         return cursor.lastrowid
 
+    def add_pose_prior(self, image_id, position, coordinate_system=-1):
+        position = np.asarray(position, dtype=np.float64)
+        self.execute(
+            "INSERT INTO pose_priors VALUES (?, ?, ?)",
+            (image_id, array_to_blob(position), coordinate_system),
+        )
+
     def add_keypoints(self, image_id, keypoints):
         assert len(keypoints.shape) == 2
         assert keypoints.shape[1] in [2, 4, 6]

diff --git a/scripts/python/migrate_database_pose_prior.py b/scripts/python/migrate_database_pose_prior.py
@@ -0,0 +1,80 @@
+# Copyright (c) 2023, ETH Zurich and UNC Chapel Hill.
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+#     * Redistributions of source code must retain the above copyright
+#       notice, this list of conditions and the following disclaimer.
+#
+#     * Redistributions in binary form must reproduce the above copyright
+#       notice, this list of conditions and the following disclaimer in the
+#       documentation and/or other materials provided with the distribution.
+#
+#     * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of
+#       its contributors may be used to endorse or promote products derived
+#       from this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+
+import numpy as np
+import argparse
+from database import COLMAPDatabase
+
+
+if __name__ == "__main__":
+    parser = argparse.ArgumentParser()
+    parser.add_argument("--database_path", type=str, required=True)
+    parser.add_argument("--is_cartesian", action="store_true")
+    parser.add_argument("--cleanup", action="store_true")
+    args = parser.parse_args()
+
+    db = COLMAPDatabase.connect(args.database_path)
+
+    pose_priors = {}
+    rows = db.execute("SELECT * FROM images")
+    for image_id, _, _, *cam_from_world_prior in rows:
+        if not cam_from_world_prior:  # newer format database
+            continue
+        qvec = np.array(cam_from_world_prior[:4], dtype=float)
+        tvec = np.array(cam_from_world_prior[4:], dtype=float)
+        if np.isfinite(qvec).any():
+            print(
+                f"Warning: rotation prior for image {image_id} "
+                "will be lost during migration."
+            )
+        if np.isfinite(tvec).any():
+            pose_priors[image_id] = tvec
+    print(f"Found location priors for {len(pose_priors)} images.")
+
+    coordinate_systems = {"UNKNOWN": -1, "WGS84": 0, "CARTESIAN": 1}
+    coordinate_system = coordinate_systems[
+        "CARTESIAN" if args.is_cartesian else "WGS84"
+    ]
+    db.create_pose_priors_table()
+    for image_id, position in pose_priors.items():
+        (exists,) = db.execute(
+            "SELECT COUNT(*) FROM pose_priors WHERE image_id = ?",
+            (image_id,),
+        ).fetchone()
+        if exists:
+            print(f"Location prior for {image_id} already exists, skipping.")
+            continue
+        db.add_pose_prior(image_id, position, coordinate_system)
+
+    if args.cleanup:
+        for col in ["qw", "qx", "qy", "qz", "tx", "ty", "tz"]:
+            db.execute(f"ALTER TABLE images DROP COLUMN prior_{col}")
+
+    db.commit()
diff --git a/src/colmap/controllers/CMakeLists.txt b/src/colmap/controllers/CMakeLists.txt
@@ -45,6 +45,7 @@ COLMAP_ADD_LIBRARY(
     PUBLIC_LINK_LIBS
         colmap_estimators
         colmap_feature
+        colmap_geometry
         colmap_scene
         colmap_util
         Eigen3::Eigen

diff --git a/src/colmap/controllers/feature_extraction.cc b/src/colmap/controllers/feature_extraction.cc
@@ -30,6 +30,7 @@
 #include "colmap/controllers/feature_extraction.h"
 
 #include "colmap/feature/sift.h"
+#include "colmap/geometry/gps.h"
 #include "colmap/scene/database.h"
 #include "colmap/util/cuda.h"
 #include "colmap/util/misc.h"
@@ -87,6 +88,7 @@ struct ImageData {
 
   Camera camera;
   Image image;
+  PosePrior pose_prior;
   Bitmap bitmap;
   Bitmap mask;
 
@@ -291,22 +293,22 @@ class FeatureWriterThread : public Thread {
             "  Focal Length:    %.2fpx%s",
             image_data.camera.MeanFocalLength(),
             image_data.camera.has_prior_focal_length ? " (Prior)" : "");
-        const Eigen::Vector3d& translation_prior =
-            image_data.image.CamFromWorldPrior().translation;
-        if (translation_prior.array().isFinite().any()) {
-          LOG(INFO) << StringPrintf(
-              "  GPS:             LAT=%.3f, LON=%.3f, ALT=%.3f",
-              translation_prior.x(),
-              translation_prior.y(),
-              translation_prior.z());
-        }
         LOG(INFO) << StringPrintf("  Features:        %d",
                                   image_data.keypoints.size());
 
         DatabaseTransaction database_transaction(database_);
 
         if (image_data.image.ImageId() == kInvalidImageId) {
           image_data.image.SetImageId(database_->WriteImage(image_data.image));
+          if (image_data.pose_prior.IsValid()) {
+            LOG(INFO) << StringPrintf(
+                "  GPS:             LAT=%.3f, LON=%.3f, ALT=%.3f",
+                image_data.pose_prior.position.x(),
+                image_data.pose_prior.position.y(),
+                image_data.pose_prior.position.z());
+            database_->WritePosePrior(image_data.image.ImageId(),
+                                      image_data.pose_prior);
+          }
         }
 
         if (!database_->ExistsKeypoints(image_data.image.ImageId())) {
@@ -460,6 +462,7 @@ class FeatureExtractorController : public Thread {
       ImageData image_data;
       image_data.status = image_reader_.Next(&image_data.camera,
                                              &image_data.image,
+                                             &image_data.pose_prior,
                                              &image_data.bitmap,
                                              &image_data.mask);
 
@@ -542,8 +545,9 @@ class FeatureImporterController : public Thread {
       // Load image data and possibly save camera to database.
       Camera camera;
       Image image;
+      PosePrior pose_prior;
       Bitmap bitmap;
-      if (image_reader.Next(&camera, &image, &bitmap, nullptr) !=
+      if (image_reader.Next(&camera, &image, &pose_prior, &bitmap, nullptr) !=
           ImageReader::Status::SUCCESS) {
         continue;
       }
@@ -561,6 +565,9 @@ class FeatureImporterController : public Thread {
 
         if (image.ImageId() == kInvalidImageId) {
           image.SetImageId(database.WriteImage(image));
+          if (pose_prior.IsValid()) {
+            database.WritePosePrior(image.ImageId(), pose_prior);
+          }
         }
 
         if (!database.ExistsKeypoints(image.ImageId())) {

diff --git a/src/colmap/controllers/image_reader.cc b/src/colmap/controllers/image_reader.cc
@@ -87,6 +87,7 @@ ImageReader::ImageReader(const ImageReaderOptions& options, Database* database)

 ImageReader::Status ImageReader::Next(Camera* camera,
                                       Image* image,
+                                      PosePrior* pose_prior,
                                       Bitmap* bitmap,
                                       Bitmap* mask) {
   THROW_CHECK_NOTNULL(camera);
@@ -249,11 +250,12 @@ ImageReader::Status ImageReader::Next(Camera* camera,
     // Extract GPS data.
     //////////////////////////////////////////////////////////////////////////////
 
-    Eigen::Vector3d& translation_prior = image->CamFromWorldPrior().translation;
-    if (!bitmap->ExifLatitude(&translation_prior.x()) ||
-        !bitmap->ExifLongitude(&translation_prior.y()) ||
-        !bitmap->ExifAltitude(&translation_prior.z())) {
-      translation_prior.setConstant(std::numeric_limits<double>::quiet_NaN());
+    Eigen::Vector3d position_prior;
+    if (bitmap->ExifLatitude(&position_prior.x()) &&
+        bitmap->ExifLongitude(&position_prior.y()) &&
+        bitmap->ExifAltitude(&position_prior.z())) {
+      pose_prior->position = position_prior;
+      pose_prior->coordinate_system = PosePrior::CoordinateSystem::WGS84;
     }
   }
 

diff --git a/src/colmap/controllers/image_reader.h b/src/colmap/controllers/image_reader.h
@@ -29,6 +29,7 @@
 
 #pragma once
 
+#include "colmap/geometry/gps.h"
 #include "colmap/scene/database.h"
 #include "colmap/sensor/bitmap.h"
 #include "colmap/util/threading.h"
@@ -107,7 +108,11 @@ class ImageReader {
 
   ImageReader(const ImageReaderOptions& options, Database* database);
 
-  Status Next(Camera* camera, Image* image, Bitmap* bitmap, Bitmap* mask);
+  Status Next(Camera* camera,
+              Image* image,
+              PosePrior* pose_prior,
+              Bitmap* bitmap,
+              Bitmap* mask);
   size_t NextIndex() const;
   size_t NumImages() const;
 

diff --git a/src/colmap/controllers/option_manager.cc b/src/colmap/controllers/option_manager.cc
@@ -407,8 +407,6 @@ void OptionManager::AddSpatialMatchingOptions() {
 
   AddMatchingOptions();
 
-  AddAndRegisterDefaultOption("SpatialMatching.is_gps",
-                              &spatial_matching->is_gps);
   AddAndRegisterDefaultOption("SpatialMatching.ignore_z",
                               &spatial_matching->ignore_z);
   AddAndRegisterDefaultOption("SpatialMatching.max_num_neighbors",

diff --git a/src/colmap/exe/model.cc b/src/colmap/exe/model.cc
@@ -163,9 +163,14 @@ void ReadDatabaseCameraLocations(const std::string& database_path,
                                  std::vector<Eigen::Vector3d>* ref_locations) {
   Database database(database_path);
   for (const auto& image : database.ReadAllImages()) {
-    if (image.CamFromWorldPrior().translation.array().isFinite().all()) {
+    if (database.ExistsPosePrior(image.ImageId())) {
       ref_image_names->push_back(image.Name());
-      ref_locations->push_back(image.CamFromWorldPrior().translation);
+      const auto pose_prior = database.ReadPosePrior(image.ImageId());
+      if (ref_is_gps) {
+        THROW_CHECK_EQ(static_cast<int>(pose_prior.coordinate_system),
+                       static_cast<int>(PosePrior::CoordinateSystem::WGS84));
+      }
+      ref_locations->push_back(pose_prior.position);
     }
   }
 

diff --git a/src/colmap/feature/CMakeLists.txt b/src/colmap/feature/CMakeLists.txt
@@ -56,11 +56,11 @@ COLMAP_ADD_LIBRARY(
         utils.h utils.cc
     PUBLIC_LINK_LIBS
         colmap_feature_types
+        colmap_geometry
         colmap_retrieval
         colmap_scene
         colmap_util
     PRIVATE_LINK_LIBS
-        colmap_geometry
         colmap_math
         colmap_sensor
         colmap_vlfeat

diff --git a/src/colmap/feature/matcher.cc b/src/colmap/feature/matcher.cc
@@ -54,6 +54,14 @@ void FeatureMatcherCache::Setup() {
     images_cache_.emplace(image.ImageId(), std::move(image));
   }
 
+  locations_priors_cache_.reserve(database_->NumPosePriors());
+  for (const auto& id_and_image : images_cache_) {
+    if (database_->ExistsPosePrior(id_and_image.first)) {
+      locations_priors_cache_.emplace(
+          id_and_image.first, database_->ReadPosePrior(id_and_image.first));
+    }
+  }
+
   keypoints_cache_ =
       std::make_unique<LRUCache<image_t, std::shared_ptr<FeatureKeypoints>>>(
           cache_size_, [this](const image_t image_id) {
@@ -87,6 +95,11 @@ const Image& FeatureMatcherCache::GetImage(const image_t image_id) const {
   return images_cache_.at(image_id);
 }
 
+const PosePrior& FeatureMatcherCache::GetPosePrior(
+    const image_t image_id) const {
+  return locations_priors_cache_.at(image_id);
+}
+
 std::shared_ptr<FeatureKeypoints> FeatureMatcherCache::GetKeypoints(
     const image_t image_id) {
   std::lock_guard<std::mutex> lock(database_mutex_);
@@ -114,6 +127,11 @@ std::vector<image_t> FeatureMatcherCache::GetImageIds() const {
   return image_ids;
 }
 
+bool FeatureMatcherCache::ExistsPosePrior(const image_t image_id) const {
+  return locations_priors_cache_.find(image_id) !=
+         locations_priors_cache_.end();
+}
+
 bool FeatureMatcherCache::ExistsKeypoints(const image_t image_id) {
   std::lock_guard<std::mutex> lock(database_mutex_);
   return keypoints_exists_cache_->Get(image_id);

diff --git a/src/colmap/feature/matcher.h b/src/colmap/feature/matcher.h
@@ -30,6 +30,7 @@
 #pragma once
 
 #include "colmap/feature/types.h"
+#include "colmap/geometry/gps.h"
 #include "colmap/scene/camera.h"
 #include "colmap/scene/database.h"
 #include "colmap/scene/image.h"
@@ -79,11 +80,13 @@ class FeatureMatcherCache {
 
   const Camera& GetCamera(camera_t camera_id) const;
   const Image& GetImage(image_t image_id) const;
+  const PosePrior& GetPosePrior(image_t image_id) const;
   std::shared_ptr<FeatureKeypoints> GetKeypoints(image_t image_id);
   std::shared_ptr<FeatureDescriptors> GetDescriptors(image_t image_id);
   FeatureMatches GetMatches(image_t image_id1, image_t image_id2);
   std::vector<image_t> GetImageIds() const;
 
+  bool ExistsPosePrior(image_t image_id) const;
   bool ExistsKeypoints(image_t image_id);
   bool ExistsDescriptors(image_t image_id);
 
@@ -106,6 +109,7 @@ class FeatureMatcherCache {
   std::mutex database_mutex_;
   std::unordered_map<camera_t, Camera> cameras_cache_;
   std::unordered_map<image_t, Image> images_cache_;
+  std::unordered_map<image_t, PosePrior> locations_priors_cache_;
   std::unique_ptr<LRUCache<image_t, std::shared_ptr<FeatureKeypoints>>>
       keypoints_cache_;
   std::unique_ptr<LRUCache<image_t, std::shared_ptr<FeatureDescriptors>>>