[camera] remove Vec3 version of residual()

alicevision · Aug 17, 2021 · 65df712 · 65df712
1 parent e0be1fb
commit 65df712
Show file tree

Hide file tree

Showing 11 changed files with 20 additions and 33 deletions.
diff --git a/src/aliceVision/camera/IntrinsicBase.hpp b/src/aliceVision/camera/IntrinsicBase.hpp
@@ -194,19 +194,6 @@ class IntrinsicBase
    */
   virtual Eigen::Matrix<double, 2, Eigen::Dynamic> getDerivativeProjectWrtParams(const geometry::Pose3& pose, const Vec4& pt3D) const = 0;
 
-  /**
-   * @brief Compute the residual between the 3D projected point X and an image observation x
-   * @param[in] pose The pose
-   * @param[in] X The 3D projected point
-   * @param[in] x The image observation
-   * @return residual
-   */
-  inline Vec2 residual(const geometry::Pose3& pose, const Vec3& X, const Vec2& x) const
-  {
-    const Vec2 proj = this->project(pose, X.homogeneous());
-    return x - proj;
-  }
-
    /**
    * @brief Compute the residual between the 3D projected point X and an image observation x
    * @param[in] pose The pose
@@ -234,7 +221,7 @@ class IntrinsicBase
     Mat2X residuals = Mat2X::Zero(2, numPts);
     for(std::size_t i = 0; i < numPts; ++i)
     {
-      residuals.col(i) = residual(pose, (const Vec3&)X.col(i), x.col(i));
+      residuals.col(i) = residual(pose, ((const Vec3&)X.col(i)).homogeneous(), x.col(i));
     }
     return residuals;
   }

diff --git a/src/aliceVision/sfm/BundleAdjustmentSymbolicCeres.cpp b/src/aliceVision/sfm/BundleAdjustmentSymbolicCeres.cpp
@@ -624,8 +624,8 @@ void BundleAdjustmentSymbolicCeres::addIntrinsicsToProblem(const sfmData::SfMDat
       lockFocal = true;
     }
 
-    bool optional_center = ((refineOptions & REFINE_INTRINSICS_OPTICALCENTER_IF_ENOUGH_DATA) && (usageCount > _minNbImagesToRefineOpticalCenter));
-    if ((refineOptions & REFINE_INTRINSICS_OPTICALCENTER_ALWAYS) || optional_center)
+    if((refineOptions & REFINE_INTRINSICS_OPTICALCENTER_ALWAYS) ||
+       ((refineOptions & REFINE_INTRINSICS_OPTICALCENTER_IF_ENOUGH_DATA) && _minNbImagesToRefineOpticalCenter > 0 && usageCount >= _minNbImagesToRefineOpticalCenter))
     {
       // refine optical center within 10% of the image size.
       assert(intrinsicBlock.size() >= 4);

diff --git a/src/aliceVision/sfm/bundleAdjustment_test.cpp b/src/aliceVision/sfm/bundleAdjustment_test.cpp
@@ -258,7 +258,7 @@ double RMSE(const SfMData & sfm_data)
       const View* view = sfm_data.getViews().find(itObs->first)->second.get();
       const Pose3 pose = sfm_data.getPose(*view).getTransform();
       const std::shared_ptr<IntrinsicBase> intrinsic = sfm_data.getIntrinsics().find(view->getIntrinsicId())->second;
-      const Vec2 residual = intrinsic->residual(pose, iterTracks->second.X, itObs->second.x);
+      const Vec2 residual = intrinsic->residual(pose, iterTracks->second.X.homogeneous(), itObs->second.x);
       vec.push_back( residual(0) );
       vec.push_back( residual(1) );
     }

diff --git a/src/aliceVision/sfm/generateReport.cpp b/src/aliceVision/sfm/generateReport.cpp
@@ -38,7 +38,7 @@ bool generateSfMReport(const sfmData::SfMData& sfmData,
       const geometry::Pose3 pose = sfmData.getPose(*view).getTransform();
       const camera::IntrinsicBase * intrinsic = sfmData.getIntrinsics().at(view->getIntrinsicId()).get();
       // Use absolute values
-      const Vec2 residual = intrinsic->residual(pose, iterTracks->second.X, itObs->second.x).array().abs();
+      const Vec2 residual = intrinsic->residual(pose, iterTracks->second.X.homogeneous(), itObs->second.x).array().abs();
       residuals_per_view[itObs->first].push_back(residual(0));
       residuals_per_view[itObs->first].push_back(residual(1));
       ++residualCount;

diff --git a/src/aliceVision/sfm/pipeline/RigSequence.cpp b/src/aliceVision/sfm/pipeline/RigSequence.cpp
@@ -67,7 +67,7 @@ double computeCameraScore(const SfMData& sfmData, const track::TracksPerView& tr
 
     if(itObs != landmark.observations.end())
     {
-      const Vec2 residual = intrinsic->residual(pose, landmark.X, itObs->second.x);
+      const Vec2 residual = intrinsic->residual(pose, landmark.X.homogeneous(), itObs->second.x);
       score += std::min(1.0 / residual.norm(), 4.0);
     }
   }

diff --git a/src/aliceVision/sfm/pipeline/sequential/ReconstructionEngine_sequentialSfM.cpp b/src/aliceVision/sfm/pipeline/sequential/ReconstructionEngine_sequentialSfM.cpp
@@ -1575,7 +1575,7 @@ void ReconstructionEngine_sequentialSfM::updateScene(const IndexT viewIndex, con
   {
     const Vec3 X = resectionData.pt3D.col(i);
     const Vec2 x = resectionData.pt2D.col(i);
-    const Vec2 residual = resectionData.optionalIntrinsic->residual(resectionData.pose, X, x);
+    const Vec2 residual = resectionData.optionalIntrinsic->residual(resectionData.pose, X.homogeneous(), x);
     if (residual.norm() < resectionData.error_max &&
         resectionData.pose.depth(X) > 0)
     {
@@ -1748,8 +1748,8 @@ void ReconstructionEngine_sequentialSfM::triangulate_multiViewsLORANSAC(SfMData&
       if (angleBetweenRays(poseI, camI.get(), poseJ, camJ.get(), xI, xJ) < _params.minAngleForTriangulation ||
           poseI.depth(X_euclidean) < 0 || 
           poseJ.depth(X_euclidean) < 0 || 
-          camI->residual(poseI, X_euclidean, xI).norm() > acThresholdI || 
-          camJ->residual(poseJ, X_euclidean, xJ).norm() > acThresholdJ)
+          camI->residual(poseI, X_euclidean.homogeneous(), xI).norm() > acThresholdI || 
+          camJ->residual(poseJ, X_euclidean.homogeneous(), xJ).norm() > acThresholdJ)
         isValidTrack = false;
     }
     else 
@@ -1919,7 +1919,7 @@ void ReconstructionEngine_sequentialSfM::triangulate_2Views(SfMData& scene, cons
             Landmark& landmark = scene.structure.at(trackId);
             if (landmark.observations.count(I) == 0)
             {
-              const Vec2 residual = camI->residual(poseI, landmark.X, xI);
+              const Vec2 residual = camI->residual(poseI, landmark.X.homogeneous(), xI);
               // TODO: scale in residual
               const auto& acThresholdIt = _map_ACThreshold.find(I);
               // TODO assert(acThresholdIt != _map_ACThreshold.end());
@@ -1933,7 +1933,7 @@ void ReconstructionEngine_sequentialSfM::triangulate_2Views(SfMData& scene, cons
             }
             if (landmark.observations.count(J) == 0)
             {
-              const Vec2 residual = camJ->residual(poseJ, landmark.X, xJ);
+              const Vec2 residual = camJ->residual(poseJ, landmark.X.homogeneous(), xJ);
               const auto& acThresholdIt = _map_ACThreshold.find(J);
               // TODO assert(acThresholdIt != _map_ACThreshold.end());
               const double acThreshold = (acThresholdIt != _map_ACThreshold.end()) ? acThresholdIt->second : 4.0;
@@ -1967,8 +1967,8 @@ void ReconstructionEngine_sequentialSfM::triangulate_2Views(SfMData& scene, cons
           //  - Check positive depth
           //  - Check residual values
           const double angle = angleBetweenRays(poseI, camI.get(), poseJ, camJ.get(), xI, xJ);
-          const Vec2 residualI = camI->residual(poseI, X_euclidean, xI);
-          const Vec2 residualJ = camJ->residual(poseJ, X_euclidean, xJ);
+          const Vec2 residualI = camI->residual(poseI, X_euclidean.homogeneous(), xI);
+          const Vec2 residualJ = camJ->residual(poseJ, X_euclidean.homogeneous(), xJ);
 
           // TODO assert(acThresholdIt != _map_ACThreshold.end());
 

diff --git a/src/aliceVision/sfm/sfmFilters.cpp b/src/aliceVision/sfm/sfmFilters.cpp
@@ -36,7 +36,7 @@ IndexT RemoveOutliers_PixelResidualError(sfmData::SfMData& sfmData,
       const geometry::Pose3 pose = sfmData.getPose(*view).getTransform();
       const camera::IntrinsicBase * intrinsic = sfmData.intrinsics.at(view->getIntrinsicId()).get();
 
-      Vec2 residual = intrinsic->residual(pose, iterTracks->second.X, itObs->second.x);
+      Vec2 residual = intrinsic->residual(pose, iterTracks->second.X.homogeneous(), itObs->second.x);
       if(featureConstraint == EFeatureConstraint::SCALE && itObs->second.scale > 0.0)
       {
           // Apply the scale of the feature to get a residual value

diff --git a/src/aliceVision/sfm/sfmStatistics.cpp b/src/aliceVision/sfm/sfmStatistics.cpp
@@ -46,7 +46,7 @@ void computeResidualsHistogram(const sfmData::SfMData& sfmData, BoxStats<double>
       const sfmData::View& view = sfmData.getView(obs.first);
       const aliceVision::geometry::Pose3 pose = sfmData.getPose(view).getTransform();
       const std::shared_ptr<aliceVision::camera::IntrinsicBase> intrinsic = sfmData.getIntrinsics().find(view.getIntrinsicId())->second;
-      const Vec2 residual = intrinsic->residual(pose, track.second.X, obs.second.x);
+      const Vec2 residual = intrinsic->residual(pose, track.second.X.homogeneous(), obs.second.x);
       vec_residuals.push_back(residual.norm());
       //ALICEVISION_LOG_INFO("[AliceVision] sfmtstatistics::computeResidualsHistogram track: " << track.first << ", residual: " << residual.norm());
     }
@@ -329,7 +329,7 @@ void computeResidualsPerView(const sfmData::SfMData& sfmData, int& nbViews, std:
         const sfmData::View& view = sfmData.getView(obs.first);
         const aliceVision::geometry::Pose3 pose = sfmData.getPose(view).getTransform();
         const std::shared_ptr<aliceVision::camera::IntrinsicBase> intrinsic = sfmData.getIntrinsics().find(view.getIntrinsicId())->second;
-        const Vec2 residual = intrinsic->residual(pose, landmark.second.X, obs.second.x);
+        const Vec2 residual = intrinsic->residual(pose, landmark.second.X.homogeneous(), obs.second.x);
         residualsPerView[obs.first].push_back(residual.norm());
       }
     }

diff --git a/src/aliceVision/sfm/sfmTriangulation.cpp b/src/aliceVision/sfm/sfmTriangulation.cpp
@@ -219,7 +219,7 @@ bool StructureComputation_robust::robust_triangulation(const sfmData::SfMData& s
       const sfmData::View * view = sfmData.views.at(itObs.first).get();
       const IntrinsicBase * intrinsic = sfmData.getIntrinsics().at(view->getIntrinsicId()).get();
       const Pose3 pose = sfmData.getPose(*view).getTransform();
-      const Vec2 residual = intrinsic->residual(pose, current_model, itObs.second.x);
+      const Vec2 residual = intrinsic->residual(pose, current_model.homogeneous(), itObs.second.x);
       const double residual_d = residual.norm();
 
       if (residual_d < dThresholdPixel)

diff --git a/src/aliceVision/sfm/utils/statistics.cpp b/src/aliceVision/sfm/utils/statistics.cpp
@@ -26,7 +26,7 @@ double RMSE(const sfmData::SfMData& sfmData)
       const sfmData::View* view = sfmData.getViews().find(itObs->first)->second.get();
       const geometry::Pose3 pose = sfmData.getPose(*view).getTransform();
       const std::shared_ptr<camera::IntrinsicBase> intrinsic = sfmData.getIntrinsics().at(view->getIntrinsicId());
-      const Vec2 residual = intrinsic->residual(pose, iterTracks->second.X, itObs->second.x);
+      const Vec2 residual = intrinsic->residual(pose, iterTracks->second.X.homogeneous(), itObs->second.x);
       vec.push_back(residual(0));
       vec.push_back(residual(1));
     }

diff --git a/src/samples/robustEssential/main_robustEssential.cpp b/src/samples/robustEssential/main_robustEssential.cpp
@@ -214,8 +214,8 @@ int main() {
       if (pose0.depth(X) < 0 && pose1.depth(X) < 0)
         continue;
 
-      const Vec2 residual0 = intrinsic0.residual(pose0, X, LL.coords().cast<double>());
-      const Vec2 residual1 = intrinsic1.residual(pose1, X, RR.coords().cast<double>());
+      const Vec2 residual0 = intrinsic0.residual(pose0, X.homogeneous(), LL.coords().cast<double>());
+      const Vec2 residual1 = intrinsic1.residual(pose1, X.homogeneous(), RR.coords().cast<double>());
       vec_residuals.push_back(fabs(residual0(0)));
       vec_residuals.push_back(fabs(residual0(1)));
       vec_residuals.push_back(fabs(residual1(0)));