Skip to content

Commit 402be69

Browse files
lpanaflpanaf
committed
f
1 parent 68585e8 commit 402be69

13 files changed

+61
-63
lines changed

‎glomap/controllers/global_mapper.cc‎

Lines changed: 19 additions & 17 deletions
Original file line numberDiff line numberDiff line change
@@ -17,26 +17,26 @@ namespace glomap {
1717

1818
// TODO: Rig normalizaiton has not be done
1919
bool GlobalMapper::Solve(const colmap::Database& database,
20-
ViewGraph& view_graph,
21-
std::unordered_map<rig_t, Rig>& rigs,
22-
std::unordered_map<camera_t, Camera>& cameras,
23-
std::unordered_map<frame_t, Frame>& frames,
24-
std::unordered_map<image_t, Image>& images,
25-
std::unordered_map<track_t, Track>& tracks) {
26-
// Check out the rig scales. If the some rigs are with known sensor_from_rig, then do not normalize scale
20+
ViewGraph& view_graph,
21+
std::unordered_map<rig_t, Rig>& rigs,
22+
std::unordered_map<camera_t, Camera>& cameras,
23+
std::unordered_map<frame_t, Frame>& frames,
24+
std::unordered_map<image_t, Image>& images,
25+
std::unordered_map<track_t, Track>& tracks) {
26+
// Check out the rig scales. If the some rigs are with known sensor_from_rig,
27+
// then do not normalize scale
2728
bool normalize_scale = true;
28-
for (auto &[rig_id, rig] : rigs) {
29+
for (auto& [rig_id, rig] : rigs) {
2930
auto sensors = rig.Sensors();
30-
for (auto &[sensor_id, sensor_from_rig]: sensors) {
31+
for (auto& [sensor_id, sensor_from_rig] : sensors) {
3132
if (sensor_from_rig.has_value()) {
3233
normalize_scale = false;
3334
break;
3435
}
3536
}
36-
if (!normalize_scale)
37-
break;
37+
if (!normalize_scale) break;
3838
}
39-
39+
4040
// 0. Preprocessing
4141
if (!options_.skip_preprocessing) {
4242
std::cout << "-------------------------------------" << std::endl;
@@ -185,7 +185,8 @@ bool GlobalMapper::Solve(const colmap::Database& database,
185185
// TODO: determine the logic for reconstruction normalization
186186
// Normalize the structure
187187
// If the camera rig is used, the structure do not need to be normalized
188-
NormalizeReconstruction(rigs, cameras, frames, images, tracks, !normalize_scale);
188+
NormalizeReconstruction(
189+
rigs, cameras, frames, images, tracks, !normalize_scale);
189190
normalize_scale = false;
190191

191192
run_timer.PrintSeconds();
@@ -204,8 +205,7 @@ bool GlobalMapper::Solve(const colmap::Database& database,
204205
for (int ite = 0; ite < options_.num_iteration_bundle_adjustment; ite++) {
205206
BundleAdjuster ba_engine(options_.opt_ba);
206207

207-
BundleAdjusterOptions& ba_engine_options_inner =
208-
ba_engine.GetOptions();
208+
BundleAdjusterOptions& ba_engine_options_inner = ba_engine.GetOptions();
209209

210210
// Staged bundle adjustment
211211
// 6.1. First stage: optimize positions only
@@ -233,7 +233,8 @@ bool GlobalMapper::Solve(const colmap::Database& database,
233233

234234
// TODO: determine the logic for reconstruction normalization
235235
// Normalize the structure
236-
NormalizeReconstruction(rigs, cameras, frames, images, tracks, !normalize_scale);
236+
NormalizeReconstruction(
237+
rigs, cameras, frames, images, tracks, !normalize_scale);
237238
normalize_scale = false;
238239

239240
// 6.3. Filter tracks based on the estimation
@@ -325,7 +326,8 @@ bool GlobalMapper::Solve(const colmap::Database& database,
325326
}
326327

327328
// Normalize the structure
328-
NormalizeReconstruction(rigs, cameras, frames, images, tracks, !normalize_scale);
329+
NormalizeReconstruction(
330+
rigs, cameras, frames, images, tracks, !normalize_scale);
329331
normalize_scale = false;
330332

331333
// Filter tracks based on the estimation

‎glomap/controllers/global_mapper.h‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -1,11 +1,11 @@
11
#pragma once
22
#include "glomap/controllers/track_establishment.h"
33
#include "glomap/controllers/track_retriangulation.h"
4-
#include "glomap/estimators/relpose_estimation.h"
5-
#include "glomap/estimators/view_graph_calibration.h"
64
#include "glomap/estimators/bundle_adjustment.h"
75
#include "glomap/estimators/global_positioning.h"
86
#include "glomap/estimators/global_rotation_averaging.h"
7+
#include "glomap/estimators/relpose_estimation.h"
8+
#include "glomap/estimators/view_graph_calibration.h"
99
#include "glomap/types.h"
1010

1111
#include <colmap/scene/database.h>

‎glomap/controllers/global_mapper_test.cc‎

Lines changed: 6 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,5 @@
11
#include "glomap/controllers/global_mapper.h"
2+
23
#include "glomap/io/colmap_io.h"
34
#include "glomap/types.h"
45

@@ -97,7 +98,8 @@ TEST(GlobalMapper, WithoutNoiseWithNonTrivialKnownRig) {
9798
synthetic_dataset_options.num_frames_per_rig = 7;
9899
synthetic_dataset_options.num_points3D = 50;
99100
synthetic_dataset_options.point2D_stddev = 0;
100-
synthetic_dataset_options.sensor_from_rig_translation_stddev = 0.1; // No noise
101+
synthetic_dataset_options.sensor_from_rig_translation_stddev =
102+
0.1; // No noise
101103
synthetic_dataset_options.sensor_from_rig_rotation_stddev = 5.; // No noise
102104
colmap::SynthesizeDataset(
103105
synthetic_dataset_options, &gt_reconstruction, &database);
@@ -136,9 +138,10 @@ TEST(GlobalMapper, WithoutNoiseWithNonTrivialUnknownRig) {
136138
synthetic_dataset_options.num_frames_per_rig = 7;
137139
synthetic_dataset_options.num_points3D = 50;
138140
synthetic_dataset_options.point2D_stddev = 0;
139-
synthetic_dataset_options.sensor_from_rig_translation_stddev = 0.1; // No noise
141+
synthetic_dataset_options.sensor_from_rig_translation_stddev =
142+
0.1; // No noise
140143
synthetic_dataset_options.sensor_from_rig_rotation_stddev = 5.; // No noise
141-
144+
142145
colmap::SynthesizeDataset(
143146
synthetic_dataset_options, &gt_reconstruction, &database);
144147

‎glomap/estimators/bundle_adjustment.cc‎

Lines changed: 4 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -9,10 +9,10 @@
99
namespace glomap {
1010

1111
bool BundleAdjuster::Solve(std::unordered_map<rig_t, Rig>& rigs,
12-
std::unordered_map<camera_t, Camera>& cameras,
13-
std::unordered_map<frame_t, Frame>& frames,
14-
std::unordered_map<image_t, Image>& images,
15-
std::unordered_map<track_t, Track>& tracks) {
12+
std::unordered_map<camera_t, Camera>& cameras,
13+
std::unordered_map<frame_t, Frame>& frames,
14+
std::unordered_map<image_t, Image>& images,
15+
std::unordered_map<track_t, Track>& tracks) {
1616
// Check if the input data is valid
1717
if (images.empty()) {
1818
LOG(ERROR) << "Number of images = " << images.size();
@@ -102,7 +102,6 @@ bool BundleAdjuster::Solve(std::unordered_map<rig_t, Rig>& rigs,
102102
else
103103
LOG(INFO) << summary.BriefReport();
104104

105-
106105
return summary.IsSolutionUsable();
107106
}
108107

@@ -217,7 +216,6 @@ void BundleAdjuster::AddCamerasAndPointsToParameterGroups(
217216
}
218217
}
219218

220-
221219
// Add camera parameters to group 1.
222220
for (auto& [camera_id, camera] : cameras) {
223221
if (problem_->HasParameterBlock(camera.params.data()))

‎glomap/estimators/bundle_adjustment.h‎

Lines changed: 1 addition & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -37,8 +37,7 @@ struct BundleAdjusterOptions : public OptimizationBaseOptions {
3737
};
3838
class BundleAdjuster {
3939
public:
40-
BundleAdjuster(const BundleAdjusterOptions& options)
41-
: options_(options) {}
40+
BundleAdjuster(const BundleAdjusterOptions& options) : options_(options) {}
4241

4342
// Returns true if the optimization was a success, false if there was a
4443
// failure.

‎glomap/estimators/global_positioning.cc‎

Lines changed: 16 additions & 17 deletions
Original file line numberDiff line numberDiff line change
@@ -20,18 +20,17 @@ Eigen::Vector3d RandVector3d(std::mt19937& random_generator,
2020

2121
} // namespace
2222

23-
GlobalPositioner::GlobalPositioner(
24-
const GlobalPositionerOptions& options)
23+
GlobalPositioner::GlobalPositioner(const GlobalPositionerOptions& options)
2524
: options_(options) {
2625
random_generator_.seed(options_.seed);
2726
}
2827

2928
bool GlobalPositioner::Solve(const ViewGraph& view_graph,
30-
std::unordered_map<rig_t, Rig>& rigs,
31-
std::unordered_map<camera_t, Camera>& cameras,
32-
std::unordered_map<frame_t, Frame>& frames,
33-
std::unordered_map<image_t, Image>& images,
34-
std::unordered_map<track_t, Track>& tracks) {
29+
std::unordered_map<rig_t, Rig>& rigs,
30+
std::unordered_map<camera_t, Camera>& cameras,
31+
std::unordered_map<frame_t, Frame>& frames,
32+
std::unordered_map<image_t, Image>& images,
33+
std::unordered_map<track_t, Track>& tracks) {
3534
if (rigs.size() > 1) {
3635
LOG(ERROR) << "Number of camera rigs = " << rigs.size();
3736
}
@@ -60,7 +59,8 @@ bool GlobalPositioner::Solve(const ViewGraph& view_graph,
6059
InitializeRandomPositions(view_graph, frames, images, tracks);
6160

6261
// Add the camera to camera constraints to the problem.
63-
// TODO: support the relative constraints with trivial frames to a non trivial frame
62+
// TODO: support the relative constraints with trivial frames to a non trivial
63+
// frame
6464
if (options_.constraint_type != GlobalPositionerOptions::ONLY_POINTS) {
6565
AddCameraToCameraConstraints(view_graph, images);
6666
}
@@ -114,7 +114,6 @@ void GlobalPositioner::SetupProblem(
114114
return sum + track.second.observations.size();
115115
}));
116116

117-
118117
// Initialize the rig scales to be 1.0.
119118
for (const auto& [rig_id, rig] : rigs) {
120119
rig_scales_.emplace(rig_id, 1.0);
@@ -167,11 +166,12 @@ void GlobalPositioner::InitializeRandomPositions(
167166

168167
void GlobalPositioner::AddCameraToCameraConstraints(
169168
const ViewGraph& view_graph, std::unordered_map<image_t, Image>& images) {
170-
// For cam to cam constraint, only support the trivial frames now
171-
for (const auto & [image_id, image] : images) {
169+
// For cam to cam constraint, only support the trivial frames now
170+
for (const auto& [image_id, image] : images) {
172171
if (!image.is_registered) continue;
173172
if (!image.HasTrivialFrame()) {
174-
LOG(ERROR) << "Now, only trivial frames are supported for the camera to camera constraints";
173+
LOG(ERROR) << "Now, only trivial frames are supported for the camera to "
174+
"camera constraints";
175175
}
176176
}
177177

@@ -207,7 +207,6 @@ void GlobalPositioner::AddCameraToCameraConstraints(
207207
VLOG(2) << problem_->NumResidualBlocks()
208208
<< " camera to camera constraints were added to the position "
209209
"estimation problem.";
210-
211210
}
212211

213212
void GlobalPositioner::AddPointToCameraConstraints(
@@ -358,8 +357,8 @@ void GlobalPositioner::AddTrackToProblem(
358357
// the rig is not needed, as it would natrually be consistent with the
359358
// global one
360359
ceres::CostFunction* cost_function =
361-
RigUnknownBATAPairwiseDirectionError::Create(translation,
362-
image.frame_ptr->RigFromWorld().rotation);
360+
RigUnknownBATAPairwiseDirectionError::Create(
361+
translation, image.frame_ptr->RigFromWorld().rotation);
363362

364363
problem_->AddResidualBlock(
365364
cost_function,
@@ -577,7 +576,8 @@ void GlobalPositioner::ConvertResults(
577576
std::map<sensor_t, std::optional<Rigid3d>>& sensors = rig.Sensors();
578577
for (auto& [sensor_id, cam_from_rig] : sensors) {
579578
if (cam_from_rig.has_value()) {
580-
if (problem_->HasParameterBlock(rig.SensorFromRig(sensor_id).translation.data())) {
579+
if (problem_->HasParameterBlock(
580+
rig.SensorFromRig(sensor_id).translation.data())) {
581581
cam_from_rig->translation =
582582
-(cam_from_rig->rotation * cam_from_rig->translation);
583583
} else {
@@ -588,7 +588,6 @@ void GlobalPositioner::ConvertResults(
588588
}
589589
}
590590
}
591-
592591
}
593592

594593
} // namespace glomap

‎glomap/estimators/global_positioning.h‎

Lines changed: 1 addition & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -82,8 +82,7 @@ class GlobalPositioner {
8282

8383
// Creates camera to camera constraints from relative translations. (3D)
8484
void AddCameraToCameraConstraints(const ViewGraph& view_graph,
85-
std::unordered_map<image_t, Image>&
86-
images);
85+
std::unordered_map<image_t, Image>& images);
8786

8887
// Add tracks to the problem
8988
void AddPointToCameraConstraints(

‎glomap/estimators/global_rotation_averaging.cc‎

Lines changed: 4 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -551,10 +551,9 @@ bool RotationEstimator::SolveL1Regression(
551551
return true;
552552
}
553553

554-
bool RotationEstimator::SolveIRLS(
555-
const ViewGraph& view_graph,
556-
std::unordered_map<frame_t, Frame>& frames,
557-
std::unordered_map<image_t, Image>& images) {
554+
bool RotationEstimator::SolveIRLS(const ViewGraph& view_graph,
555+
std::unordered_map<frame_t, Frame>& frames,
556+
std::unordered_map<image_t, Image>& images) {
558557
// TODO: Determine what is the best solver for this part
559558
Eigen::CholmodSupernodalLLT<Eigen::SparseMatrix<double>> llt;
560559

@@ -598,8 +597,7 @@ bool RotationEstimator::SolveIRLS(
598597
if (options_.weight_type == RotationEstimatorOptions::GEMAN_MCCLURE) {
599598
double tmp = err_squared + sigma * sigma;
600599
w = sigma * sigma / (tmp * tmp);
601-
} else if (options_.weight_type ==
602-
RotationEstimatorOptions::HALF_NORM) {
600+
} else if (options_.weight_type == RotationEstimatorOptions::HALF_NORM) {
603601
w = std::pow(err_squared, (0.5 - 2) / 2);
604602
}
605603

@@ -815,7 +813,6 @@ void RotationEstimator::ConvertResults(
815813
image_id_to_idx_[image_id_begin], 3))),
816814
Eigen::Vector3d::Zero()));
817815
}
818-
819816
}
820817

821818
// add the estimated

‎glomap/estimators/rotation_initializer.cc‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -98,7 +98,8 @@ bool ConvertRotationsFromImageToRig(
9898
if (!image.is_registered) continue;
9999

100100
if (image_id == frame_to_ref_image_id[frame_id]) {
101-
rig_from_world_rotations.push_back(cam_from_worlds.at(image_id).rotation);
101+
rig_from_world_rotations.push_back(
102+
cam_from_worlds.at(image_id).rotation);
102103
} else {
103104
auto cam_from_rig_opt =
104105
rigs[camera_id_to_rig_id[image.camera_id]].MaybeSensorFromRig(

‎glomap/estimators/rotation_initializer.h‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -10,5 +10,5 @@ bool ConvertRotationsFromImageToRig(
1010
const std::unordered_map<image_t, Image>& images,
1111
std::unordered_map<rig_t, Rig>& rigs,
1212
std::unordered_map<frame_t, Frame>& frames);
13-
13+
1414
} // namespace glomap

0 commit comments

Comments
 (0)