Merge pull request #841 from 0xfaded/carl-angle-error-speedup

[sfm] Performance improvements: optimize RemoveOutliers_AngleError

src/aliceVision/camera/IntrinsicBase.hpp

@@ -463,6 +463,25 @@ class IntrinsicBase
   std::string _serialNumber;
 };
 
+/**
+ * @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
+ * @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 +512,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);
 }
 

src/aliceVision/sfm/sfmFilters.cpp

@@ -63,41 +63,93 @@ IndexT RemoveOutliers_PixelResidualError(sfmData::SfMData& sfmData,
 
 IndexT RemoveOutliers_AngleError(sfmData::SfMData& sfmData, const double dMinAcceptedAngle)
 {
-  IndexT removedTrack_count = 0;
-  sfmData::Landmarks::iterator iterTracks = sfmData.structure.begin();
+  // note that smallest accepted angle => largest accepted cos(angle)
+  const double dMaxAcceptedCosAngle = std::cos(degreeToRadian(dMinAcceptedAngle));
 
-  while(iterTracks != sfmData.structure.end())
+  using LandmarksKeysVec = std::vector<sfmData::Landmarks::key_type>;
+  LandmarksKeysVec v_keys; v_keys.reserve(sfmData.structure.size());
+  std::transform(sfmData.structure.cbegin(), sfmData.structure.cend(), std::back_inserter(v_keys), stl::RetrieveKey());
+
+  LandmarksKeysVec toErase;
+
+  #pragma omp parallel for
+  for (int landmarkIndex = 0; landmarkIndex < v_keys.size(); ++landmarkIndex)
   {
-    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.at(v_keys[landmarkIndex]).observations;
+
+    // create matrix for observation directions from camera to point
+    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 spatial distribution, so greedily jumping over larger and larger angles
+    // forces the greedy index towards the outside of the distribution.
+    double dGreedyCos = 1.1;
+    Mat3X::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
+      const 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(v_keys[landmarkIndex]);
     }
-    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<IndexT>* outRemovedViewsId)