diff --git a/glomap/estimators/global_positioning.cc b/glomap/estimators/global_positioning.cc index e6936746..0b94dd0d 100644 --- a/glomap/estimators/global_positioning.cc +++ b/glomap/estimators/global_positioning.cc @@ -42,8 +42,8 @@ bool GlobalPositioner::Solve(const ViewGraph& view_graph, LOG(INFO) << "Setting up the global positioner problem"; - // Initialize the problem - Reset(); + // Setup the problem. + SetupProblem(view_graph, tracks); // Initialize camera translations to be random. // Also, convert the camera pose translation to be the camera center. @@ -81,11 +81,24 @@ bool GlobalPositioner::Solve(const ViewGraph& view_graph, return summary.IsSolutionUsable(); } -void GlobalPositioner::Reset() { +void GlobalPositioner::SetupProblem( + const ViewGraph& view_graph, + const std::unordered_map& tracks) { ceres::Problem::Options problem_options; problem_options.loss_function_ownership = ceres::DO_NOT_TAKE_OWNERSHIP; problem_ = std::make_unique(problem_options); + // Allocate enough memory for the scales. One for each residual. + // Due to possibly invalid image pairs or tracks, the actual number of + // residuals may be smaller. scales_.clear(); + scales_.reserve( + view_graph.image_pairs.size() + + std::accumulate(tracks.begin(), + tracks.end(), + 0, + [](int sum, const std::pair& track) { + return sum + track.second.observations.size(); + })); } void GlobalPositioner::InitializeRandomPositions( @@ -145,8 +158,9 @@ void GlobalPositioner::AddCameraToCameraConstraints( images.find(image_id2) == images.end()) continue; - track_t counter = scales_.size(); - scales_.insert(std::make_pair(counter, 1)); + CHECK_GT(scales_.capacity(), scales_.size()) + << "Not enough capacity was reserved for the scales."; + double& scale = scales_.emplace_back(1); Eigen::Vector3d translation = -(images[image_id2].cam_from_world.rotation.inverse() * @@ -158,9 +172,9 @@ void GlobalPositioner::AddCameraToCameraConstraints( options_.loss_function.get(), images[image_id1].cam_from_world.translation.data(), images[image_id2].cam_from_world.translation.data(), - &(scales_[counter])); + &scale); - problem_->SetParameterLowerBound(&(scales_[counter]), 0, 1e-5); + problem_->SetParameterLowerBound(&scale, 0, 1e-5); } if (options_.verbose) @@ -246,14 +260,14 @@ void GlobalPositioner::AddTrackToProblem( ceres::CostFunction* cost_function = BATAPairwiseDirectionError::Create(translation); - track_t counter = scales_.size(); - if (options_.generate_scales || !tracks[track_id].is_initialized) { - scales_.insert(std::make_pair(counter, 1)); - } else { - Eigen::Vector3d trans_calc = + CHECK_GT(scales_.capacity(), scales_.size()) + << "Not enough capacity was reserved for the scales."; + double& scale = scales_.emplace_back(1); + if (!options_.generate_scales && tracks[track_id].is_initialized) { + const Eigen::Vector3d trans_calc = tracks[track_id].xyz - image.cam_from_world.translation; - double scale = translation.dot(trans_calc) / trans_calc.squaredNorm(); - scales_.insert(std::make_pair(counter, std::max(scale, 1e-5))); + scale = std::max(1e-5, + translation.dot(trans_calc) / trans_calc.squaredNorm()); } // For calibrated and uncalibrated cameras, use different loss functions @@ -263,14 +277,14 @@ void GlobalPositioner::AddTrackToProblem( loss_function_ptcam_calibrated_.get(), image.cam_from_world.translation.data(), tracks[track_id].xyz.data(), - &(scales_[counter])) + &scale) : problem_->AddResidualBlock(cost_function, loss_function_ptcam_uncalibrated_.get(), image.cam_from_world.translation.data(), tracks[track_id].xyz.data(), - &(scales_[counter])); + &scale); - problem_->SetParameterLowerBound(&(scales_[counter]), 0, 1e-5); + problem_->SetParameterLowerBound(&scale, 0, 1e-5); } } @@ -284,8 +298,8 @@ void GlobalPositioner::AddCamerasAndPointsToParameterGroups( options_.solver_options.linear_solver_ordering.get(); // Add scale parameters to group 0 (large and independent) - for (auto& [i, scale] : scales_) { - parameter_ordering->AddElementToGroup(&(scales_[i]), 0); + for (double& scale : scales_) { + parameter_ordering->AddElementToGroup(&scale, 0); } // Add point parameters to group 1. @@ -332,8 +346,8 @@ void GlobalPositioner::ParameterizeVariables( // If do not optimize the scales, set the scales to be constant if (!options_.optimize_scales) { - for (auto& [i, scale] : scales_) { - problem_->SetParameterBlockConstant(&(scales_[i])); + for (double& scale : scales_) { + problem_->SetParameterBlockConstant(&scale); } } @@ -358,4 +372,4 @@ void GlobalPositioner::ConvertResults( } } -} // namespace glomap \ No newline at end of file +} // namespace glomap diff --git a/glomap/estimators/global_positioning.h b/glomap/estimators/global_positioning.h index 24260dc2..6a9b3236 100644 --- a/glomap/estimators/global_positioning.h +++ b/glomap/estimators/global_positioning.h @@ -61,8 +61,8 @@ class GlobalPositioner { GlobalPositionerOptions& GetOptions() { return options_; } protected: - // Reset the problem - void Reset(); + void SetupProblem(const ViewGraph& view_graph, + const std::unordered_map& tracks); // Initialize all cameras to be random. void InitializeRandomPositions(const ViewGraph& view_graph, @@ -98,17 +98,17 @@ class GlobalPositioner { // center Convert the results back to camera poses void ConvertResults(std::unordered_map& images); - // Data members GlobalPositionerOptions options_; std::mt19937 random_generator_; std::unique_ptr problem_; - // loss functions for reweighted terms + // Loss functions for reweighted terms. std::shared_ptr loss_function_ptcam_uncalibrated_; std::shared_ptr loss_function_ptcam_calibrated_; - std::unordered_map scales_; + // Auxiliary scale variables. + std::vector scales_; }; } // namespace glomap