Skip to content

Commit 8d637c6

Browse files
ahojnnesahojnnes
committed
d
1 parent 0ab5f85 commit 8d637c6

File tree

1 file changed

+41
-42
lines changed

1 file changed

+41
-42
lines changed

‎glomap/controllers/rotation_averager.cc‎

Lines changed: 41 additions & 42 deletions
Original file line numberDiff line numberDiff line change
@@ -16,18 +16,14 @@ bool SolveRotationAveraging(ViewGraph& view_graph,
1616

1717
ViewGraph view_graph_grav;
1818
image_pair_t total_pairs = 0;
19-
image_pair_t grav_pairs = 0;
2019
if (solve_1dof_system) {
2120
// Prepare two sets: ones all with gravity, and one does not have gravity.
2221
// Solve them separately first, then solve them in a single system
2322
for (const auto& [pair_id, image_pair] : view_graph.image_pairs) {
2423
if (!image_pair.is_valid) continue;
2524

26-
image_t image_id1 = image_pair.image_id1;
27-
image_t image_id2 = image_pair.image_id2;
28-
29-
Image& image1 = images[image_id1];
30-
Image& image2 = images[image_id2];
25+
const Image& image1 = images[image_pair.image_id1];
26+
const Image& image2 = images[image_pair.image_id2];
3127

3228
if (!image1.IsRegistered() || !image2.IsRegistered()) continue;
3329

@@ -36,23 +32,28 @@ bool SolveRotationAveraging(ViewGraph& view_graph,
3632
if (image1.HasGravity() && image2.HasGravity()) {
3733
view_graph_grav.image_pairs.emplace(
3834
pair_id,
39-
ImagePair(image_id1, image_id2, image_pair.cam2_from_cam1));
40-
grav_pairs++;
35+
ImagePair(image_pair.image_id1,
36+
image_pair.image_id2,
37+
image_pair.cam2_from_cam1));
4138
}
4239
}
4340
}
4441

42+
const size_t grav_pairs = view_graph_grav.image_pairs.size();
43+
44+
LOG(INFO) << "Total image pairs: " << total_pairs
45+
<< ", gravity image pairs: " << grav_pairs;
46+
4547
// If there is no image pairs with gravity or most image pairs are with
4648
// gravity, then just run the 3dof version
47-
const bool status = (grav_pairs == 0) || (grav_pairs > total_pairs * 0.95);
48-
solve_1dof_system = solve_1dof_system && (!status);
49+
const bool status = grav_pairs == 0 || grav_pairs > total_pairs * 0.95;
50+
solve_1dof_system = solve_1dof_system && !status;
4951

5052
if (solve_1dof_system) {
5153
// Run the 1dof optimization
5254
LOG(INFO) << "Solving subset 1DoF rotation averaging problem in the mixed "
5355
"prior system";
54-
int num_img_grv =
55-
view_graph_grav.KeepLargestConnectedComponents(frames, images);
56+
view_graph_grav.KeepLargestConnectedComponents(frames, images);
5657
RotationEstimator rotation_estimator_grav(options);
5758
if (!rotation_estimator_grav.EstimateRotations(
5859
view_graph_grav, rigs, frames, images)) {
@@ -61,31 +62,30 @@ bool SolveRotationAveraging(ViewGraph& view_graph,
6162
view_graph.KeepLargestConnectedComponents(frames, images);
6263
}
6364

64-
// By default, run trivial rotation averaging for rigged cameras if some
65-
// cam_from_rig are not estimated Check if there are rigs with non-trivial
66-
// cam_from_rig
67-
std::unordered_set<camera_t> camera_without_rig;
65+
// By default, run trivial rotation averaging for cameras with unknown
66+
// cam_from_rig.
67+
std::unordered_set<camera_t> unknown_cams_from_rig;
6868
rig_t max_rig_id = 0;
6969
for (const auto& [rig_id, rig] : rigs) {
7070
max_rig_id = std::max(max_rig_id, rig_id);
7171
for (const auto& [sensor_id, sensor] : rig.NonRefSensors()) {
7272
if (sensor_id.type != SensorType::CAMERA) continue;
7373
if (!rig.MaybeSensorFromRig(sensor_id).has_value()) {
74-
camera_without_rig.insert(sensor_id.id);
74+
unknown_cams_from_rig.insert(sensor_id.id);
7575
}
7676
}
7777
}
7878

7979
bool status_ra = false;
8080
// If the trivial rotation averaging is enabled, run it
81-
if (camera_without_rig.size() > 0 && !options.skip_initialization) {
81+
if (!unknown_cams_from_rig.empty() && !options.skip_initialization) {
8282
LOG(INFO) << "Running trivial rotation averaging for rigged cameras";
8383
// Create a rig for each camera
8484
std::unordered_map<rig_t, Rig> rigs_trivial;
8585
std::unordered_map<frame_t, Frame> frames_trivial;
8686
std::unordered_map<image_t, Image> images_trivial;
8787

88-
// For cameras without rigs, create a trivial rig
88+
// For cameras with known cam_from_rig, create rigs with only those sensors.
8989
std::unordered_map<camera_t, rig_t> camera_id_to_rig_id;
9090
for (const auto& [rig_id, rig] : rigs) {
9191
Rig rig_trivial;
@@ -103,8 +103,8 @@ bool SolveRotationAveraging(ViewGraph& view_graph,
103103
rigs_trivial[rig_trivial.RigId()] = rig_trivial;
104104
}
105105

106-
// Then, for each camera without rig, create a trivial rig
107-
for (const auto& camera_id : camera_without_rig) {
106+
// For each camera with unknown cam_from_rig, create a separate trivial rig.
107+
for (const auto& camera_id : unknown_cams_from_rig) {
108108
Rig rig_trivial;
109109
rig_trivial.SetRigId(++max_rig_id);
110110
rig_trivial.AddRefSensor(sensor_t(SensorType::CAMERA, camera_id));
@@ -113,8 +113,8 @@ bool SolveRotationAveraging(ViewGraph& view_graph,
113113
}
114114

115115
frame_t max_frame_id = 0;
116-
for (const auto& [frame_id, frame] : frames) {
117-
if (frame_id == colmap::kInvalidFrameId) continue;
116+
for (const auto& [frame_id, _] : frames) {
117+
THROW_CHECK_NE(frame_id, colmap::kInvalidFrameId);
118118
max_frame_id = std::max(max_frame_id, frame_id);
119119
}
120120
max_frame_id++;
@@ -129,26 +129,25 @@ bool SolveRotationAveraging(ViewGraph& view_graph,
129129
: nullptr);
130130
frames_trivial[frame_id] = frame_trivial;
131131

132-
for (const auto& data_id : frame.DataIds()) {
133-
image_t image_id = data_id.id;
134-
if (images.find(image_id) == images.end()) continue;
135-
const auto& image = images.at(image_id);
132+
for (const auto& data_id : frame.ImageIds()) {
133+
const auto& image = images.at(data_id.id);
136134
if (!image.IsRegistered()) continue;
137-
images_trivial.insert(std::make_pair(
138-
image_id, Image(image_id, image.camera_id, image.file_name)));
139-
140-
if (camera_without_rig.find(images_trivial[image_id].camera_id) ==
141-
camera_without_rig.end()) {
142-
// images_trivial_to_frame_id[image_id] = frame_id;
143-
144-
frames_trivial[frame_id].AddDataId(images_trivial[image_id].DataId());
145-
images_trivial[image_id].frame_id = frame_id;
146-
images_trivial[image_id].frame_ptr = &frames_trivial[frame_id];
135+
auto& image_trivial =
136+
images_trivial
137+
.emplace(data_id.id,
138+
Image(data_id.id, image.camera_id, image.file_name))
139+
.first->second;
140+
141+
if (unknown_cams_from_rig.find(image_trivial.camera_id) ==
142+
unknown_cams_from_rig.end()) {
143+
frames_trivial[frame_id].AddDataId(image_trivial.DataId());
144+
image_trivial.frame_id = frame_id;
145+
image_trivial.frame_ptr = &frames_trivial[frame_id];
147146
} else {
148147
// If the camera is not in any rig, then create a trivial frame
149148
// for it
150149
CreateFrameForImage(Rigid3d(),
151-
images_trivial[image_id],
150+
image_trivial,
152151
rigs_trivial,
153152
frames_trivial,
154153
camera_id_to_rig_id[image.camera_id],
@@ -167,13 +166,13 @@ bool SolveRotationAveraging(ViewGraph& view_graph,
167166
view_graph, rigs_trivial, frames_trivial, images_trivial);
168167

169168
// Collect the results
170-
std::unordered_map<image_t, Rigid3d> cam_from_worlds;
169+
std::unordered_map<image_t, Rigid3d> cams_from_world;
171170
for (const auto& [image_id, image] : images_trivial) {
172171
if (!image.IsRegistered()) continue;
173-
cam_from_worlds[image_id] = image.CamFromWorld();
172+
cams_from_world[image_id] = image.CamFromWorld();
174173
}
175174

176-
ConvertRotationsFromImageToRig(cam_from_worlds, images, rigs, frames);
175+
ConvertRotationsFromImageToRig(cams_from_world, images, rigs, frames);
177176

178177
RotationEstimatorOptions options_ra = options;
179178
options_ra.skip_initialization = true;
@@ -186,7 +185,7 @@ bool SolveRotationAveraging(ViewGraph& view_graph,
186185
// For cases where there are some cameras without known cam_from_rig
187186
// transformation, we need to run the rotation averaging with the
188187
// skip_initialization flag set to false for convergence
189-
if (camera_without_rig.size() > 0) {
188+
if (unknown_cams_from_rig.size() > 0) {
190189
options_ra.skip_initialization = false;
191190
}
192191

0 commit comments

Comments
 (0)