diff --git a/glomap/estimators/cost_function.h b/glomap/estimators/cost_function.h index d0a8ce48..64c3e466 100644 --- a/glomap/estimators/cost_function.h +++ b/glomap/estimators/cost_function.h @@ -14,7 +14,7 @@ namespace glomap { // from two positions such that t_ij - scale * (c_j - c_i) is minimized. struct BATAPairwiseDirectionError { BATAPairwiseDirectionError(const Eigen::Vector3d& translation_obs) - : translation_obs_(translation_obs){}; + : translation_obs_(translation_obs) {} // The error is given by the position error described above. template @@ -22,19 +22,11 @@ struct BATAPairwiseDirectionError { const T* position2, const T* scale, T* residuals) const { - Eigen::Matrix translation; - translation[0] = position2[0] - position1[0]; - translation[1] = position2[1] - position1[1]; - translation[2] = position2[2] - position1[2]; - - Eigen::Matrix residual_vec; - - residual_vec = translation_obs_.cast() - scale[0] * translation; - - residuals[0] = residual_vec(0); - residuals[1] = residual_vec(1); - residuals[2] = residual_vec(2); - + Eigen::Map> residuals_vec(residuals); + residuals_vec = + translation_obs_.cast() - + scale[0] * (Eigen::Map>(position2) - + Eigen::Map>(position1)); return true; } diff --git a/glomap/estimators/global_positioning.cc b/glomap/estimators/global_positioning.cc index 0b94dd0d..5590903e 100644 --- a/glomap/estimators/global_positioning.cc +++ b/glomap/estimators/global_positioning.cc @@ -155,14 +155,15 @@ void GlobalPositioner::AddCameraToCameraConstraints( const image_t image_id1 = image_pair.image_id1; const image_t image_id2 = image_pair.image_id2; if (images.find(image_id1) == images.end() || - images.find(image_id2) == images.end()) + images.find(image_id2) == images.end()) { continue; + } CHECK_GT(scales_.capacity(), scales_.size()) << "Not enough capacity was reserved for the scales."; double& scale = scales_.emplace_back(1); - Eigen::Vector3d translation = + const Eigen::Vector3d translation = -(images[image_id2].cam_from_world.rotation.inverse() * image_pair.cam2_from_cam1.translation); ceres::CostFunction* cost_function = @@ -244,7 +245,7 @@ void GlobalPositioner::AddPointToCameraConstraints( } void GlobalPositioner::AddTrackToProblem( - const track_t& track_id, + track_t track_id, std::unordered_map& cameras, std::unordered_map& images, std::unordered_map& tracks) { @@ -255,8 +256,19 @@ void GlobalPositioner::AddTrackToProblem( Image& image = images[observation.first]; if (!image.is_registered) continue; - Eigen::Vector3d translation = image.cam_from_world.rotation.inverse() * - image.features_undist[observation.second]; + const Eigen::Vector3d& feature_undist = + image.features_undist[observation.second]; + if (feature_undist.array().isNaN().any()) { + LOG(WARNING) + << "Ignoring feature because it failed to undistort: track_id=" + << track_id << ", image_id=" << observation.first + << ", feature_id=" << observation.second; + continue; + } + + const Eigen::Vector3d translation = + image.cam_from_world.rotation.inverse() * + image.features_undist[observation.second]; ceres::CostFunction* cost_function = BATAPairwiseDirectionError::Create(translation); @@ -272,17 +284,19 @@ void GlobalPositioner::AddTrackToProblem( // For calibrated and uncalibrated cameras, use different loss functions // Down weight the uncalibrated cameras - (cameras[image.camera_id].has_prior_focal_length) - ? problem_->AddResidualBlock(cost_function, - loss_function_ptcam_calibrated_.get(), - image.cam_from_world.translation.data(), - tracks[track_id].xyz.data(), - &scale) - : problem_->AddResidualBlock(cost_function, - loss_function_ptcam_uncalibrated_.get(), - image.cam_from_world.translation.data(), - tracks[track_id].xyz.data(), - &scale); + if (cameras[image.camera_id].has_prior_focal_length) { + problem_->AddResidualBlock(cost_function, + loss_function_ptcam_calibrated_.get(), + image.cam_from_world.translation.data(), + tracks[track_id].xyz.data(), + &scale); + } else { + problem_->AddResidualBlock(cost_function, + loss_function_ptcam_uncalibrated_.get(), + image.cam_from_world.translation.data(), + tracks[track_id].xyz.data(), + &scale); + } problem_->SetParameterLowerBound(&scale, 0, 1e-5); } diff --git a/glomap/estimators/global_positioning.h b/glomap/estimators/global_positioning.h index 6a9b3236..0eb1185c 100644 --- a/glomap/estimators/global_positioning.h +++ b/glomap/estimators/global_positioning.h @@ -80,7 +80,7 @@ class GlobalPositioner { std::unordered_map& tracks); // Add a single track to the problem - void AddTrackToProblem(const track_t& track_id, + void AddTrackToProblem(track_t track_id, std::unordered_map& cameras, std::unordered_map& images, std::unordered_map& tracks); diff --git a/glomap/estimators/relpose_estimation.cc b/glomap/estimators/relpose_estimation.cc index 89bdab3a..c53f572a 100644 --- a/glomap/estimators/relpose_estimation.cc +++ b/glomap/estimators/relpose_estimation.cc @@ -47,15 +47,22 @@ void EstimateRelativePoses(ViewGraph& view_graph, inliers.clear(); poselib::CameraPose pose_rel_calc; - poselib::estimate_relative_pose( - points2D_1, - points2D_2, - ColmapCameraToPoseLibCamera(cameras[image1.camera_id]), - ColmapCameraToPoseLibCamera(cameras[image2.camera_id]), - options.ransac_options, - options.bundle_options, - &pose_rel_calc, - &inliers); + try { + poselib::estimate_relative_pose( + points2D_1, + points2D_2, + ColmapCameraToPoseLibCamera(cameras[image1.camera_id]), + ColmapCameraToPoseLibCamera(cameras[image2.camera_id]), + options.ransac_options, + options.bundle_options, + &pose_rel_calc, + &inliers); + } catch (const std::exception& e) { + LOG(ERROR) << "Error in relative pose estimation: " << e.what(); + image_pair.is_valid = false; + continue; + } + // Convert the relative pose to the glomap format for (int i = 0; i < 4; i++) { image_pair.cam2_from_cam1.rotation.coeffs()[i] = diff --git a/glomap/processors/track_filter.cc b/glomap/processors/track_filter.cc index 226af020..c3a78f7d 100644 --- a/glomap/processors/track_filter.cc +++ b/glomap/processors/track_filter.cc @@ -37,7 +37,7 @@ int TrackFilter::FilterTracksByReprojection( // If the reprojection error is smaller than the threshold, then keep it if (reprojection_error < max_reprojection_error) { - observation_new.emplace_back(std::make_pair(image_id, feature_id)); + observation_new.emplace_back(image_id, feature_id); } } if (observation_new.size() != track.observations.size()) { diff --git a/glomap/scene/image.h b/glomap/scene/image.h index a191371a..1497efae 100644 --- a/glomap/scene/image.h +++ b/glomap/scene/image.h @@ -47,11 +47,10 @@ struct Image { // Gravity information GravityInfo gravity_info; - // Features + // Distorted feature points in pixels. std::vector features; - std::vector - features_undist; // store the normalized features, can be obtained by - // calling UndistortImages + // Normalized feature rays, can be obtained by calling UndistortImages. + std::vector features_undist; // Methods inline Eigen::Vector3d Center() const;