From 6f507686c2c81b4ba87d8ad986796edd2a058497 Mon Sep 17 00:00:00 2001 From: Carl Chatfield Date: Mon, 27 Jul 2020 12:35:56 +0200 Subject: [PATCH 1/5] [sfm] Optimize RemoveOutliers_AngleError - Use greedy hueristic to avoid O(N^2) code in vast majority of cases - Use Eigen matrix operations to speed up O(N^2) operations - Add omp parallel --- src/aliceVision/camera/IntrinsicBase.hpp | 26 +++++-- src/aliceVision/sfm/sfmFilters.cpp | 95 ++++++++++++++++++------ 2 files changed, 94 insertions(+), 27 deletions(-) diff --git a/src/aliceVision/camera/IntrinsicBase.hpp b/src/aliceVision/camera/IntrinsicBase.hpp index 15b2823910..200ce39797 100644 --- a/src/aliceVision/camera/IntrinsicBase.hpp +++ b/src/aliceVision/camera/IntrinsicBase.hpp @@ -463,6 +463,24 @@ class IntrinsicBase std::string _serialNumber; }; +/** + * @brief Return the angle (degree) between two bearing vector rays + * @param[in] pose Extrinsic pose + * @param[in] intrinsic Intrinsic camera paremeters + * @param[in] x Point in image + * @return The unit vector in 3D space pointing out from the camera to the point + */ +inline Vec3 applyIntrinsicExtrinsic(const geometry::Pose3& pose, + const IntrinsicBase* intrinsic, + const Vec2& x) { + + // x = (u, v, 1.0) // image coordinates + // X = R.t() * K.inv() * x + C // Camera world point + // getting the ray: + // ray = X - C = R.t() * K.inv() * x + return (pose.rotation().transpose() * intrinsic->toUnitSphere(intrinsic->removeDistortion(intrinsic->ima2cam(x)))).normalized(); +} + /** * @brief Return the angle (degree) between two bearing vector rays * @param[in] ray1 First bearing vector ray @@ -493,12 +511,8 @@ inline double angleBetweenRays(const geometry::Pose3& pose1, const Vec2& x1, const Vec2& x2) { - // x = (u, v, 1.0) // image coordinates - // X = R.t() * K.inv() * x + C // Camera world point - // getting the ray: - // ray = X - C = R.t() * K.inv() * x - const Vec3 ray1 = (pose1.rotation().transpose() * intrinsic1->toUnitSphere(intrinsic1->removeDistortion(intrinsic1->ima2cam(x1)))).normalized(); - const Vec3 ray2 = (pose2.rotation().transpose() * intrinsic2->toUnitSphere(intrinsic2->removeDistortion(intrinsic2->ima2cam(x2)))).normalized(); + const Vec3 ray1 = applyIntrinsicExtrinsic(pose1, intrinsic1, x1); + const Vec3 ray2 = applyIntrinsicExtrinsic(pose2, intrinsic2, x2); return angleBetweenRays(ray1, ray2); } diff --git a/src/aliceVision/sfm/sfmFilters.cpp b/src/aliceVision/sfm/sfmFilters.cpp index a552d7cf98..149c869895 100644 --- a/src/aliceVision/sfm/sfmFilters.cpp +++ b/src/aliceVision/sfm/sfmFilters.cpp @@ -63,41 +63,94 @@ IndexT RemoveOutliers_PixelResidualError(sfmData::SfMData& sfmData, IndexT RemoveOutliers_AngleError(sfmData::SfMData& sfmData, const double dMinAcceptedAngle) { + // note that smallest accepted angle => largest accepted cos(angle) + const double dMaxAcceptedCosAngle = std::cos(degreeToRadian(dMinAcceptedAngle)); IndexT removedTrack_count = 0; sfmData::Landmarks::iterator iterTracks = sfmData.structure.begin(); - while(iterTracks != sfmData.structure.end()) + typedef std::vector LandmarksKeysVec; + LandmarksKeysVec v_keys; v_keys.reserve(sfmData.structure.size()); + for (const sfmData::Landmarks::value_type &kv : sfmData.structure) v_keys.push_back(kv.first); + + std::vector toErase; + + #pragma omp parallel for + for (auto it = v_keys.begin(); it < v_keys.end(); it ++) { - sfmData::Observations & observations = iterTracks->second.observations; - double max_angle = 0.0; - for(sfmData::Observations::const_iterator itObs1 = observations.begin(); itObs1 != observations.end(); ++itObs1) + const sfmData::Observations &observations = sfmData.structure[*it].observations; + + // create matrix for observation directions from camera to point + Eigen::Matrix viewDirections(3, observations.size()); + Eigen::Matrix::Index i; + sfmData::Observations::const_iterator itObs; + + // Greedy algorithm almost always finds an acceptable angle in 1-5 iterations (if it exists). + // It works by greedily chasing the first larger view angle found from the current greedy index. + // View angles have a spartial distribution, so greedily jumping over larger and larger angles + // forces the greedy index towards the outside of the distribution. + double dGreedyCos = 1.1; + Eigen::Matrix::Index greedyI = 0; + + + // fill matrix, optimistically checking each new entry against col(greedyI) + for(itObs = observations.begin(), i = 0; itObs != observations.end(); ++itObs, ++i) { - const sfmData::View * view1 = sfmData.views.at(itObs1->first).get(); - const geometry::Pose3 pose1 = sfmData.getPose(*view1).getTransform(); - const camera::IntrinsicBase * intrinsic1 = sfmData.intrinsics.at(view1->getIntrinsicId()).get(); + const sfmData::View * view = sfmData.views.at(itObs->first).get(); + const geometry::Pose3 pose = sfmData.getPose(*view).getTransform(); + const camera::IntrinsicBase * intrinsic = sfmData.intrinsics.at(view->getIntrinsicId()).get(); - sfmData::Observations::const_iterator itObs2 = itObs1; - ++itObs2; + viewDirections.col(i) = applyIntrinsicExtrinsic(pose, intrinsic, itObs->second.x); - for(; itObs2 != observations.end(); ++itObs2) + double dCosAngle = viewDirections.col(i).transpose() * viewDirections.col(greedyI); + if (dCosAngle < dMaxAcceptedCosAngle) + { + break; + } + else if (dCosAngle < dGreedyCos) { - const sfmData::View * view2 = sfmData.views.at(itObs2->first).get(); - const geometry::Pose3 pose2 = sfmData.getPose(*view2).getTransform(); - const camera::IntrinsicBase * intrinsic2 = sfmData.intrinsics.at(view2->getIntrinsicId()).get(); + dGreedyCos = dCosAngle; + greedyI = i; + } + } - const double angle = angleBetweenRays(pose1, intrinsic1, pose2, intrinsic2, itObs1->second.x, itObs2->second.x); - max_angle = std::max(angle, max_angle); + // early exit, acceptable angle found + if (itObs != observations.end()) + { + continue; + } + + // Switch to O(n^2) exhaustive search. + // Although this is an O(n^2) loop, in practice it will almost always break very early. + // + // - Default value of dMinAcceptedAngle is 2 degrees. Any larger angle breaks. + // - For landmarks with small number of views, n^2 is negligible. + // - For landmarks with large number of views, backwards iteration means + // all view directions as considered as early as possible, + // making it difficult for a small angle to hide between views. + // + for(i = viewDirections.cols() - 1; i > 0; i -= 1) + { + // Compute and find minimum cosAngle between viewDirections[i] and all viewDirections[0:i]. + // Single statement can allow Eigen optimizations + double dMinCosAngle = (viewDirections.col(i).transpose() * viewDirections.leftCols(i)).minCoeff(); + if (dMinCosAngle < dMaxAcceptedCosAngle) { + break; } } - if (max_angle < dMinAcceptedAngle) + + // acceptable angle not found + if (i == 0) { - iterTracks = sfmData.structure.erase(iterTracks); - ++removedTrack_count; + #pragma omp critical + toErase.push_back(*it); } - else - ++iterTracks; } - return removedTrack_count; + + for (IndexT key : toErase) { + sfmData.structure.erase(key); + } + + return toErase.size(); } bool eraseUnstablePoses(sfmData::SfMData& sfmData, const IndexT min_points_per_pose, std::set* outRemovedViewsId) From 5b552700b5f37c12105dc5d5495d6b82af47ae6b Mon Sep 17 00:00:00 2001 From: Carl Chatfield Date: Thu, 30 Jul 2020 22:26:29 +0200 Subject: [PATCH 2/5] spelling --- src/aliceVision/camera/IntrinsicBase.hpp | 3 ++- src/aliceVision/sfm/sfmFilters.cpp | 2 +- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/src/aliceVision/camera/IntrinsicBase.hpp b/src/aliceVision/camera/IntrinsicBase.hpp index 200ce39797..cf232ae350 100644 --- a/src/aliceVision/camera/IntrinsicBase.hpp +++ b/src/aliceVision/camera/IntrinsicBase.hpp @@ -464,7 +464,8 @@ class IntrinsicBase }; /** - * @brief Return the angle (degree) between two bearing vector rays + * @brief Apply intrinsic and extrinsic parameters to unit vector + * from the cameras focus to a point on the camera plane * @param[in] pose Extrinsic pose * @param[in] intrinsic Intrinsic camera paremeters * @param[in] x Point in image diff --git a/src/aliceVision/sfm/sfmFilters.cpp b/src/aliceVision/sfm/sfmFilters.cpp index 149c869895..87076bce26 100644 --- a/src/aliceVision/sfm/sfmFilters.cpp +++ b/src/aliceVision/sfm/sfmFilters.cpp @@ -86,7 +86,7 @@ IndexT RemoveOutliers_AngleError(sfmData::SfMData& sfmData, const double dMinAcc // Greedy algorithm almost always finds an acceptable angle in 1-5 iterations (if it exists). // It works by greedily chasing the first larger view angle found from the current greedy index. - // View angles have a spartial distribution, so greedily jumping over larger and larger angles + // View angles have a sparial distribution, so greedily jumping over larger and larger angles // forces the greedy index towards the outside of the distribution. double dGreedyCos = 1.1; Eigen::Matrix::Index greedyI = 0; From 2e75e12b9a726b194805eb175dbf346384d6efc9 Mon Sep 17 00:00:00 2001 From: Fabien Castan Date: Mon, 24 Aug 2020 10:44:35 +0200 Subject: [PATCH 3/5] [sfm] minor code style updates Co-authored-by: Simone Gasparini --- src/aliceVision/sfm/sfmFilters.cpp | 19 ++++++++++--------- 1 file changed, 10 insertions(+), 9 deletions(-) diff --git a/src/aliceVision/sfm/sfmFilters.cpp b/src/aliceVision/sfm/sfmFilters.cpp index 87076bce26..52f6cf0347 100644 --- a/src/aliceVision/sfm/sfmFilters.cpp +++ b/src/aliceVision/sfm/sfmFilters.cpp @@ -68,11 +68,11 @@ IndexT RemoveOutliers_AngleError(sfmData::SfMData& sfmData, const double dMinAcc IndexT removedTrack_count = 0; sfmData::Landmarks::iterator iterTracks = sfmData.structure.begin(); - typedef std::vector LandmarksKeysVec; + using LandmarksKeysVec = std::vector; LandmarksKeysVec v_keys; v_keys.reserve(sfmData.structure.size()); - for (const sfmData::Landmarks::value_type &kv : sfmData.structure) v_keys.push_back(kv.first); + std::transform(sfmData.structure.cbegin(), sfmData.structure.cend(), std::back_inserter(v_keys), stl::RetrieveKey()); - std::vector toErase; + LandmarksKeysVec toErase; #pragma omp parallel for for (auto it = v_keys.begin(); it < v_keys.end(); it ++) @@ -80,16 +80,16 @@ IndexT RemoveOutliers_AngleError(sfmData::SfMData& sfmData, const double dMinAcc const sfmData::Observations &observations = sfmData.structure[*it].observations; // create matrix for observation directions from camera to point - Eigen::Matrix viewDirections(3, observations.size()); - Eigen::Matrix::Index i; + Mat3X viewDirections(3, observations.size()); + Mat3X::Index i; sfmData::Observations::const_iterator itObs; // Greedy algorithm almost always finds an acceptable angle in 1-5 iterations (if it exists). // It works by greedily chasing the first larger view angle found from the current greedy index. - // View angles have a sparial distribution, so greedily jumping over larger and larger angles + // View angles have a spatial distribution, so greedily jumping over larger and larger angles // forces the greedy index towards the outside of the distribution. double dGreedyCos = 1.1; - Eigen::Matrix::Index greedyI = 0; + Mat3X::Index greedyI = 0; // fill matrix, optimistically checking each new entry against col(greedyI) @@ -132,7 +132,7 @@ IndexT RemoveOutliers_AngleError(sfmData::SfMData& sfmData, const double dMinAcc { // Compute and find minimum cosAngle between viewDirections[i] and all viewDirections[0:i]. // Single statement can allow Eigen optimizations - double dMinCosAngle = (viewDirections.col(i).transpose() * viewDirections.leftCols(i)).minCoeff(); + const double dMinCosAngle = (viewDirections.col(i).transpose() * viewDirections.leftCols(i)).minCoeff(); if (dMinCosAngle < dMaxAcceptedCosAngle) { break; } @@ -146,7 +146,8 @@ IndexT RemoveOutliers_AngleError(sfmData::SfMData& sfmData, const double dMinAcc } } - for (IndexT key : toErase) { + for (IndexT key : toErase) + { sfmData.structure.erase(key); } From 7fed8b79786f3bb456db0ba3f121124922241a74 Mon Sep 17 00:00:00 2001 From: Fabien Castan Date: Mon, 24 Aug 2020 22:22:10 +0200 Subject: [PATCH 4/5] [sfm] sfmFilters: RemoveOutliers_AngleError fix for omp with MSVC --- src/aliceVision/sfm/sfmFilters.cpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/src/aliceVision/sfm/sfmFilters.cpp b/src/aliceVision/sfm/sfmFilters.cpp index 52f6cf0347..765c2ada02 100644 --- a/src/aliceVision/sfm/sfmFilters.cpp +++ b/src/aliceVision/sfm/sfmFilters.cpp @@ -65,8 +65,6 @@ IndexT RemoveOutliers_AngleError(sfmData::SfMData& sfmData, const double dMinAcc { // note that smallest accepted angle => largest accepted cos(angle) const double dMaxAcceptedCosAngle = std::cos(degreeToRadian(dMinAcceptedAngle)); - IndexT removedTrack_count = 0; - sfmData::Landmarks::iterator iterTracks = sfmData.structure.begin(); using LandmarksKeysVec = std::vector; LandmarksKeysVec v_keys; v_keys.reserve(sfmData.structure.size()); @@ -75,9 +73,9 @@ IndexT RemoveOutliers_AngleError(sfmData::SfMData& sfmData, const double dMinAcc LandmarksKeysVec toErase; #pragma omp parallel for - for (auto it = v_keys.begin(); it < v_keys.end(); it ++) + for (int landmarkIndex = 0; landmarkIndex < v_keys.size(); ++landmarkIndex) { - const sfmData::Observations &observations = sfmData.structure[*it].observations; + const sfmData::Observations &observations = sfmData.structure.at(v_keys[landmarkIndex]).observations; // create matrix for observation directions from camera to point Mat3X viewDirections(3, observations.size()); From cd49a5dbf9df4076498a699af31fd1f3116c459e Mon Sep 17 00:00:00 2001 From: Fabien Castan Date: Mon, 24 Aug 2020 23:05:45 +0200 Subject: [PATCH 5/5] [sfm] sfmFilters: code build fix --- src/aliceVision/sfm/sfmFilters.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/aliceVision/sfm/sfmFilters.cpp b/src/aliceVision/sfm/sfmFilters.cpp index 765c2ada02..f0743371bc 100644 --- a/src/aliceVision/sfm/sfmFilters.cpp +++ b/src/aliceVision/sfm/sfmFilters.cpp @@ -140,7 +140,7 @@ IndexT RemoveOutliers_AngleError(sfmData::SfMData& sfmData, const double dMinAcc if (i == 0) { #pragma omp critical - toErase.push_back(*it); + toErase.push_back(v_keys[landmarkIndex]); } }