[Trifocal+P2Pt] New filter post-resection group badj

src/openMVG/sfm/base/sfm_data_filters.cpp

@@ -50,26 +50,21 @@ IndexT RemoveOutliers_PixelResidualError
   {
     Observations & obs = iterTracks->second.obs;
     Observations::iterator itObs = obs.begin();
-//    LandmarkInfo *li;
-//    if (sfm_data.is_oriented())
-//      li = &sfm_data.info.at(iterTracks->first); // [lit.first] but const
     while (itObs != obs.end())
     {
       const View * view = sfm_data.views.at(itObs->first).get();
       const geometry::Pose3 pose = sfm_data.GetPoseOrDie(view);
       const cameras::IntrinsicBase * intrinsic = sfm_data.intrinsics.at(view->id_intrinsic).get();
       const Vec2 residual = intrinsic->residual(pose(iterTracks->second.X), itObs->second.x);
+
       if (residual.norm() > dThresholdPixel)
       {
         ++outlier_count;
-//        if (sfm_data.is_oriented())
-//          li->obs_info.erase(itObs->first);
         itObs = obs.erase(itObs);
       }
       else
         ++itObs;
     }
-    // assert (!sfm_data.is_oriented() || (sfm_data.is_oriented() && obs.size() == li->obs_info.size()));
     if (obs.empty() || obs.size() < minTrackLength)
       iterTracks = sfm_data.structure.erase(iterTracks);
     else
@@ -78,6 +73,53 @@ IndexT RemoveOutliers_PixelResidualError
   return outlier_count;
 }
 
+
+// Remove tracks that have a high reprojection error in any view
+// Return the number of removed tracks
+IndexT RemoveOutliers_TangentOrientationResidualError
+(
+  SfM_Data & sfm_data,
+  const double dThresholdTangentOrientationAngle,
+  const unsigned int minTrackLength
+)
+{
+  IndexT outlier_count = 0;
+  Landmarks::iterator iterTracks = sfm_data.structure.begin();
+  while (iterTracks != sfm_data.structure.end())
+  {
+    Observations &obs = iterTracks->second.obs;
+    LandmarkInfo &li = sfm_data.info.at(iterTracks->first); // [lit.first] but const
+    ObservationsInfo &info_obs = li.obs_info;
+
+    Observations::iterator itObs = obs.begin();
+    while (itObs != obs.end()) {
+      const View *view = sfm_data.views.at(itObs->first).get();
+      const geometry::Pose3 pose = sfm_data.GetPoseOrDie(view);
+      const cameras::IntrinsicBase *intrinsic = sfm_data.intrinsics.at(view->id_intrinsic).get();
+
+      // project tangent, measure error
+      static constexpr bool ignore_distortion = true; // We ignore distortion for now
+      Vec3 Xcam = pose(iterTracks->second.X);
+      double angular_error = intrinsic->residual_orientation(
+            pose.apply_to_orientation(li.T),
+            info_obs[itObs->first].t, Xcam/Xcam(2), ignore_distortion); // range 0,pi
+      if (angular_error > dThresholdTangentOrientationAngle
+          && angular_error + dThresholdTangentOrientationAngle <= M_PI) {
+        ++outlier_count;
+        info_obs.erase(itObs->first);
+        itObs = obs.erase(itObs);
+      } else
+        ++itObs;
+    }
+    if (obs.empty() || obs.size() < minTrackLength)
+      iterTracks = sfm_data.structure.erase(iterTracks);
+    else
+      ++iterTracks;
+  }
+  return outlier_count;
+}
+
+
 // Remove tracks that have a small angle (tracks with tiny angle leads to instable 3D points)
 // Return the number of removed tracks
 IndexT RemoveOutliers_AngleError

src/openMVG/sfm/base/sfm_data_filters.hpp

@@ -59,6 +59,15 @@ IndexT RemoveOutliers_AngleError
   const double dMinAcceptedAngle
 );
 
+// Remove tracks that have a large reprojection angle of tangent/feature orientation
+// Return the number of removed tracks
+IndexT RemoveOutliers_TangentOrientationResidualError
+(
+  SfM_Data & sfm_data,
+  const double dThresholdTangentOrientationAngle,
+  const unsigned int minTrackLength
+);
+
 /// Erase pose with insufficient track observations
 bool eraseMissingPoses
 (

src/openMVG/sfm/pipelines/sequential/sequential_SfM.cpp

@@ -1197,6 +1197,29 @@ bool SequentialSfMReconstructionEngine::MakeInitialSeedReconstruction()
   return true;
 }
 
+/**
+ * @brief Discard tracks with too large residual error
+ *
+ * Remove observation/tracks that have:
+ *  - too large residual error
+ *  - too small angular value
+ *
+ * @return True if more than 'count' outliers have been removed.
+ */
+bool SequentialSfMReconstructionEngine::badTrackRejector(double dPrecision, size_t count)
+{
+  const size_t nbOutliers_residualErr = RemoveOutliers_PixelResidualError(sfm_data_, dPrecision, 2);
+  const size_t nbOutliers_angleErr = RemoveOutliers_AngleError(sfm_data_, 2.0);
+
+  size_t nbOutliers_orientationErr = 0;
+  if (UseOrientedConstraint()) {
+    ReconstructAllTangents();
+    nbOutliers_orientationErr = RemoveOutliers_TangentOrientationResidualError(sfm_data_, 0.34 /* 20deg rad */, 2);
+  }
+
+  return (nbOutliers_residualErr + nbOutliers_angleErr + nbOutliers_orientationErr) > count;
+}
+
 
 } // namespace sfm
 } // namespace openMVG

src/openMVG/sfm/pipelines/sequential/sequential_SfM.hpp

@@ -83,6 +83,9 @@ class SequentialSfMReconstructionEngine : public SequentialSfMReconstructionEngi
   // consistency checks for oriented datastructures that run parallel to
   // the usual landmarks/observations 
   bool ConsistencyCheckOriented() const;
+
+  /// Discard track with too large residual error
+  bool badTrackRejector(double dPrecision, size_t count = 0);
 };
 
 } // namespace sfm

src/openMVG/sfm/pipelines/sequential/sequential_SfM_base.cpp

@@ -742,23 +742,6 @@ struct sort_pair_second {
   }
 };
 
-/**
- * @brief Discard tracks with too large residual error
- *
- * Remove observation/tracks that have:
- *  - too large residual error
- *  - too small angular value
- *
- * @return True if more than 'count' outliers have been removed.
- */
-bool SequentialSfMReconstructionEngineBase::badTrackRejector(double dPrecision, size_t count)
-{
-  const size_t nbOutliers_residualErr = RemoveOutliers_PixelResidualError(sfm_data_, dPrecision, 2);
-  const size_t nbOutliers_angleErr = RemoveOutliers_AngleError(sfm_data_, 2.0);
-  // TODO: orientation
-
-  return (nbOutliers_residualErr + nbOutliers_angleErr) > count;
-}
 
 void SequentialSfMReconstructionEngineBase::FinalStatistics()
 {

src/openMVG/sfm/pipelines/sequential/sequential_SfM_base.hpp

@@ -111,9 +111,6 @@ class SequentialSfMReconstructionEngineBase : public ReconstructionEngine
   /// Bundle adjustment to refine Structure; Motion and Intrinsics
   bool BundleAdjustment();
 
-  /// Discard track with too large residual error
-  bool badTrackRejector(double dPrecision, size_t count = 0);
-
   bool using_initial_triple() { return std::get<2>(initial_triplet_) != 0; }
 
   //----

src/openMVG_Samples/multiview_trifocal/README.md

@@ -218,3 +218,6 @@ Adding code in this engine could be simpler since you have to inherit and write
 5. Bootstrap GlobalSfM
 Since your solver is using less points than some other solver, we could perhaps have better results (more stable, and less outliers triplets)
 
+# Export matches/tracks
+
+