Skip to content

Commit 3db6eb0

Browse files
authored
Extract ConvertPosePriorsToENU and skip pose priors without position (colmap#4937)
## Problem During feature extraction (`ImageReader::Next`), COLMAP writes a `PosePrior` with `gravity` from EXIF orientation and `coordinate_system = UNDEFINED` (`position = NaN`) even when an image has no GPS tag. If other images in the same database have GPS tags (`coordinate_system = WGS84`), `DatabaseCache::ConvertPosePriorsToENU()` and `colmap model_aligner --database_path` fail on `THROW_CHECK` or feed `NaN` positions into `GPSTransform`. In addition, the ENU conversion lived privately inside `DatabaseCache`, so standalone bundle adjustment callers reading `PosePrior`s from a database could not reuse it. ## Fix - Extract `void ConvertPosePriorsToENU(std::vector<PosePrior>& pose_priors)` into `colmap/geometry/pose_prior.{h,cc}` and call it from `DatabaseCache::Create` and `DatabaseCache::CreateFromCache`. - Skip `!pose_prior.HasPosition()` entries in `ConvertPosePriorsToENU` and in `ReadDatabaseCameraLocations` (`src/colmap/exe/model.cc`) so gravity-only `PosePrior` records are ignored by position-prior conversion and `model_aligner`. ## Tests - Added `PosePrior.ConvertPosePriorsToENU` in `src/colmap/geometry/pose_prior_test.cc` covering empty inputs, Cartesian/gravity-only no-ops, mixed gravity-only + `WGS84` priors, and inconsistent coordinate systems with valid positions. - Added mixed gravity-only + `WGS84` test coverage in `src/colmap/scene/database_cache_test.cc`. <sub>Stack created with <a href="https://github.com/github/gh-stack">GitHub Stacks CLI</a> • <a href="https://gh.io/stacks-feedback">Give Feedback 💬</a></sub>
1 parent e1317dd commit 3db6eb0

7 files changed

Lines changed: 153 additions & 46 deletions

File tree

‎src/colmap/exe/model.cc‎

Lines changed: 7 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -71,14 +71,16 @@ std::vector<Eigen::Vector3d> ConvertCameraLocations(
7171
const bool ref_is_gps,
7272
const std::string& alignment_type,
7373
const std::vector<Eigen::Vector3d>& ref_locations) {
74+
if (ref_locations.empty()) {
75+
return {};
76+
}
7477
if (ref_is_gps) {
7578
const GPSTransform gps_transform(GPSTransform::Ellipsoid::WGS84);
7679
if (alignment_type != "enu") {
7780
LOG(INFO) << "Converting Alignment Coordinates from GPS (lat/lon/alt) "
7881
"to ECEF.";
7982
return gps_transform.EllipsoidToECEF(ref_locations);
8083
} else {
81-
THROW_CHECK(!ref_locations.empty());
8284
LOG(INFO) << "Converting Alignment Coordinates from GPS (lat/lon/alt) "
8385
"to ENU. Using the first GPS coordinate as the ENU origin: "
8486
<< ref_locations[0].transpose();
@@ -121,10 +123,12 @@ void ReadDatabaseCameraLocations(const std::filesystem::path& database_path,
121123
std::vector<Eigen::Vector3d>* ref_locations) {
122124
auto database = Database::Open(database_path);
123125

124-
// Index pose priors by their associated data ID.
126+
// Index pose priors with finite positions by their associated data ID.
125127
NodeHashMap<data_t, PosePrior> pose_priors_by_data_id;
126128
for (const auto& pose_prior : database->ReadAllPosePriors()) {
127-
pose_priors_by_data_id.emplace(pose_prior.corr_data_id, pose_prior);
129+
if (pose_prior.HasPosition()) {
130+
pose_priors_by_data_id.emplace(pose_prior.corr_data_id, pose_prior);
131+
}
128132
}
129133

130134
for (const auto& image : database->ReadAllImages()) {

‎src/colmap/geometry/pose_prior.cc‎

Lines changed: 39 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -2,6 +2,7 @@
22

33
#include "colmap/geometry/pose_prior.h"
44

5+
#include "colmap/geometry/gps.h"
56
#include "colmap/math/math.h"
67
#include "colmap/util/logging.h"
78

@@ -88,4 +89,42 @@ int ComputeRot90FromGravity(const Eigen::Vector3d& gravity) {
8889
return rot90_ccw;
8990
}
9091

92+
void ConvertPosePriorsToENU(std::vector<PosePrior>& pose_priors) {
93+
std::optional<PosePrior::CoordinateSystem> coordinate_system;
94+
std::vector<PosePrior*> gps_priors;
95+
std::vector<Eigen::Vector3d> gps_positions;
96+
for (PosePrior& pose_prior : pose_priors) {
97+
if (!pose_prior.HasPosition()) {
98+
continue;
99+
}
100+
if (!coordinate_system.has_value()) {
101+
coordinate_system = pose_prior.coordinate_system;
102+
} else {
103+
THROW_CHECK(*coordinate_system == pose_prior.coordinate_system)
104+
<< "Inconsistent coordinate systems defined in pose priors";
105+
}
106+
if (pose_prior.coordinate_system == PosePrior::CoordinateSystem::WGS84) {
107+
gps_priors.push_back(&pose_prior);
108+
gps_positions.push_back(pose_prior.position);
109+
}
110+
}
111+
112+
if (gps_priors.empty()) {
113+
return;
114+
}
115+
116+
const GPSTransform gps_transform(GPSTransform::Ellipsoid::WGS84);
117+
const std::vector<Eigen::Vector3d> enu_positions =
118+
gps_transform.EllipsoidToENU(gps_positions,
119+
gps_positions[0].x(),
120+
gps_positions[0].y(),
121+
gps_positions[0].z());
122+
auto enu_it = enu_positions.begin();
123+
for (PosePrior* pose_prior : gps_priors) {
124+
pose_prior->position = *enu_it;
125+
pose_prior->coordinate_system = PosePrior::CoordinateSystem::CARTESIAN;
126+
++enu_it;
127+
}
128+
}
129+
91130
} // namespace colmap

‎src/colmap/geometry/pose_prior.h‎

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -8,6 +8,7 @@
88

99
#include <optional>
1010
#include <ostream>
11+
#include <vector>
1112

1213
#include <Eigen/Core>
1314

@@ -59,4 +60,10 @@ std::optional<Eigen::Vector3d> GravityFromExifOrientation(int orientation);
5960
// sensor upright.
6061
int ComputeRot90FromGravity(const Eigen::Vector3d& gravity);
6162

63+
// Convert WGS84 pose prior positions in-place to Cartesian ENU coordinates
64+
// anchored at the first finite WGS84 position. Priors without a finite position
65+
// (e.g., gravity-only priors) are skipped. Throws if finite-position priors mix
66+
// coordinate systems.
67+
void ConvertPosePriorsToENU(std::vector<PosePrior>& pose_priors);
68+
6269
} // namespace colmap

‎src/colmap/geometry/pose_prior_test.cc‎

Lines changed: 59 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -2,6 +2,10 @@
22

33
#include "colmap/geometry/pose_prior.h"
44

5+
#include "colmap/geometry/gps.h"
6+
#include "colmap/util/eigen_matchers.h"
7+
8+
#include <gmock/gmock.h>
59
#include <gtest/gtest.h>
610

711
namespace colmap {
@@ -95,5 +99,60 @@ TEST(PosePrior, ComputeRot90FromGravity) {
9599
EXPECT_EQ(ComputeRot90FromGravity(gravity), 3);
96100
}
97101

102+
TEST(PosePrior, ConvertPosePriorsToENU) {
103+
std::vector<PosePrior> empty_priors;
104+
ConvertPosePriorsToENU(empty_priors);
105+
EXPECT_TRUE(empty_priors.empty());
106+
107+
// Gravity-only and already-Cartesian priors are left unchanged.
108+
PosePrior gravity_only_prior;
109+
gravity_only_prior.gravity = Eigen::Vector3d(0, 1, 0);
110+
PosePrior cartesian_prior;
111+
cartesian_prior.position = Eigen::Vector3d(1, 2, 3);
112+
cartesian_prior.coordinate_system = PosePrior::CoordinateSystem::CARTESIAN;
113+
std::vector<PosePrior> non_gps_priors = {gravity_only_prior, cartesian_prior};
114+
ConvertPosePriorsToENU(non_gps_priors);
115+
EXPECT_EQ(non_gps_priors[0], gravity_only_prior);
116+
EXPECT_EQ(non_gps_priors[1], cartesian_prior);
117+
118+
// Mixed WGS84 + gravity-only (UNDEFINED, NaN position) priors convert only
119+
// the WGS84 entries and preserve gravity-only entries intact.
120+
PosePrior gps_prior1;
121+
gps_prior1.position = Eigen::Vector3d(47.3769, 8.5417, 408.0);
122+
gps_prior1.coordinate_system = PosePrior::CoordinateSystem::WGS84;
123+
gps_prior1.gravity = Eigen::Vector3d(0, 1, 0);
124+
PosePrior gps_prior2;
125+
gps_prior2.position = Eigen::Vector3d(47.3779, 8.5427, 418.0);
126+
gps_prior2.coordinate_system = PosePrior::CoordinateSystem::WGS84;
127+
128+
std::vector<PosePrior> mixed_priors = {
129+
gravity_only_prior, gps_prior1, gravity_only_prior, gps_prior2};
130+
ConvertPosePriorsToENU(mixed_priors);
131+
EXPECT_EQ(mixed_priors[0], gravity_only_prior);
132+
EXPECT_EQ(mixed_priors[2], gravity_only_prior);
133+
EXPECT_EQ(mixed_priors[1].coordinate_system,
134+
PosePrior::CoordinateSystem::CARTESIAN);
135+
EXPECT_EQ(mixed_priors[1].gravity, gps_prior1.gravity);
136+
EXPECT_THAT(mixed_priors[1].position,
137+
EigenMatrixNear(Eigen::Vector3d(0, 0, 0), 1e-6));
138+
EXPECT_EQ(mixed_priors[3].coordinate_system,
139+
PosePrior::CoordinateSystem::CARTESIAN);
140+
const GPSTransform gps_transform(GPSTransform::Ellipsoid::WGS84);
141+
const Eigen::Vector3d expected_enu2 =
142+
gps_transform
143+
.EllipsoidToENU({gps_prior2.position},
144+
gps_prior1.position.x(),
145+
gps_prior1.position.y(),
146+
gps_prior1.position.z())
147+
.front();
148+
EXPECT_THAT(mixed_priors[3].position, EigenMatrixNear(expected_enu2, 1e-6));
149+
150+
// Mixing WGS84 and CARTESIAN finite positions throws.
151+
std::vector<PosePrior> inconsistent_priors = {gps_prior1, cartesian_prior};
152+
EXPECT_THAT([&]() { ConvertPosePriorsToENU(inconsistent_priors); },
153+
testing::ThrowsMessage<std::invalid_argument>(
154+
testing::HasSubstr("Inconsistent coordinate systems")));
155+
}
156+
98157
} // namespace
99158
} // namespace colmap

‎src/colmap/scene/database_cache.cc‎

Lines changed: 3 additions & 41 deletions
Original file line numberDiff line numberDiff line change
@@ -2,7 +2,7 @@
22

33
#include "colmap/scene/database_cache.h"
44

5-
#include "colmap/geometry/gps.h"
5+
#include "colmap/geometry/pose_prior.h"
66
#include "colmap/util/hash_containers.h"
77
#include "colmap/util/string.h"
88
#include "colmap/util/timer.h"
@@ -257,7 +257,7 @@ void DatabaseCache::Load(const Database& database, const Options& options) {
257257
pose_priors_ = database.ReadAllPosePriors();
258258

259259
if (options.convert_pose_priors_to_enu) {
260-
ConvertPosePriorsToENU();
260+
ConvertPosePriorsToENU(pose_priors_);
261261
}
262262

263263
LOG(INFO) << StringPrintf(
@@ -393,7 +393,7 @@ std::shared_ptr<DatabaseCache> DatabaseCache::CreateFromCache(
393393
// Copy pose priors.
394394
cache->pose_priors_ = database_cache.PosePriors();
395395
if (options.convert_pose_priors_to_enu) {
396-
cache->ConvertPosePriorsToENU();
396+
ConvertPosePriorsToENU(cache->pose_priors_);
397397
}
398398

399399
// Build filtered correspondence graph with all images from connected frames.
@@ -473,42 +473,4 @@ const class Image* DatabaseCache::FindImageWithName(
473473
return nullptr;
474474
}
475475

476-
void DatabaseCache::ConvertPosePriorsToENU() {
477-
bool prior_is_gps = true;
478-
479-
std::vector<Eigen::Vector3d> gps_prior_positions;
480-
std::set<PosePrior::CoordinateSystem> coordinate_systems;
481-
for (const auto& pose_prior : pose_priors_) {
482-
coordinate_systems.insert(pose_prior.coordinate_system);
483-
if (pose_prior.coordinate_system != PosePrior::CoordinateSystem::WGS84) {
484-
prior_is_gps = false;
485-
} else {
486-
gps_prior_positions.push_back(pose_prior.position);
487-
}
488-
}
489-
490-
THROW_CHECK_LE(coordinate_systems.size(), 1)
491-
<< "Inconsistent coordinate systems defined in pose priors";
492-
493-
// If GPS priors are available, convert them to Cartesian ENU coordinates.
494-
if (prior_is_gps && !gps_prior_positions.empty()) {
495-
// GPS reference to be used for EllipsoidToENU conversion.
496-
const double ref_lat = gps_prior_positions[0][0];
497-
const double ref_lon = gps_prior_positions[0][1];
498-
const double ref_alt = gps_prior_positions[0][2];
499-
500-
const GPSTransform gps_transform(GPSTransform::Ellipsoid::WGS84);
501-
const std::vector<Eigen::Vector3d> v_xyz_prior =
502-
gps_transform.EllipsoidToENU(
503-
gps_prior_positions, ref_lat, ref_lon, ref_alt);
504-
505-
auto xyz_prior_it = v_xyz_prior.begin();
506-
for (auto& pose_prior : pose_priors_) {
507-
pose_prior.position = *xyz_prior_it;
508-
pose_prior.coordinate_system = PosePrior::CoordinateSystem::CARTESIAN;
509-
++xyz_prior_it;
510-
}
511-
}
512-
}
513-
514476
} // namespace colmap

‎src/colmap/scene/database_cache.h‎

Lines changed: 0 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -101,8 +101,6 @@ class DatabaseCache {
101101
const class Image* FindImageWithName(const std::string& name) const;
102102

103103
private:
104-
void ConvertPosePriorsToENU();
105-
106104
NodeHashMap<rig_t, class Rig> rigs_;
107105
NodeHashMap<camera_t, struct Camera> cameras_;
108106
NodeHashMap<frame_t, class Frame> frames_;

‎src/colmap/scene/database_cache_test.cc‎

Lines changed: 38 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -457,5 +457,43 @@ TEST(DatabaseCache, CreateFromCacheSkipsUndefinedGeometries) {
457457
0);
458458
}
459459

460+
TEST(DatabaseCache, ConvertMixedGravityAndGPSPosePriorsToENU) {
461+
auto database = Database::Open(kInMemorySqliteDatabasePath);
462+
const Camera camera = Camera::CreateFromModelId(
463+
kInvalidCameraId, SimplePinholeCameraModel::model_id, 1, 1, 1);
464+
const camera_t camera_id = database->WriteCamera(camera);
465+
466+
Image image1;
467+
image1.SetName("image1");
468+
image1.SetCameraId(camera_id);
469+
const image_t image_id1 = database->WriteImage(image1);
470+
Image image2;
471+
image2.SetName("image2");
472+
image2.SetCameraId(camera_id);
473+
const image_t image_id2 = database->WriteImage(image2);
474+
475+
PosePrior gravity_prior;
476+
gravity_prior.corr_data_id = data_t(camera.SensorId(), image_id1);
477+
gravity_prior.gravity = Eigen::Vector3d(0, 1, 0);
478+
database->WritePosePrior(gravity_prior);
479+
480+
PosePrior gps_prior;
481+
gps_prior.corr_data_id = data_t(camera.SensorId(), image_id2);
482+
gps_prior.position = Eigen::Vector3d(47.3769, 8.5417, 408.0);
483+
gps_prior.coordinate_system = PosePrior::CoordinateSystem::WGS84;
484+
database->WritePosePrior(gps_prior);
485+
486+
DatabaseCache::Options options;
487+
options.load_all_images = true;
488+
options.convert_pose_priors_to_enu = true;
489+
const auto cache = DatabaseCache::Create(*database, options);
490+
ASSERT_EQ(cache->PosePriors().size(), 2);
491+
EXPECT_FALSE(cache->PosePriors()[0].HasPosition());
492+
EXPECT_EQ(cache->PosePriors()[0].gravity, gravity_prior.gravity);
493+
EXPECT_EQ(cache->PosePriors()[1].coordinate_system,
494+
PosePrior::CoordinateSystem::CARTESIAN);
495+
EXPECT_EQ(cache->PosePriors()[1].position, Eigen::Vector3d::Zero());
496+
}
497+
460498
} // namespace
461499
} // namespace colmap

0 commit comments

Comments
 (0)