Merge pull request #1729 from alicevision/dev/fixSfm

Fix SfM when using undistortions

src/aliceVision/camera/DistortionFisheye.cpp

@@ -119,7 +119,7 @@ Vec2 DistortionFisheye::removeDistortion(const Vec2& p) const
     if (theta_dist > eps)
     {
         double theta = theta_dist;
-        for (int j = 0; j < 10; ++j)
+        for (int j = 0; j < 20; ++j)
         {
             const double theta2 = theta * theta;
             const double theta4 = theta2 * theta2;

src/aliceVision/camera/IntrinsicBase.hpp

@@ -125,7 +125,6 @@ class IntrinsicBase
      * @brief Get the derivative of a projection of a 3D point into the camera plane
      * @param[in] pose The pose
      * @param[in] pt3D The 3D point
-     * @param[in] applyDistortion If true, apply the distortion if there is any
      * @return The projection jacobian with respect to the pose
      */
     virtual Eigen::Matrix<double, 2, 16> getDerivativeProjectWrtPose(const Eigen::Matrix4d& pose, const Vec4& pt3D) const = 0;
@@ -134,7 +133,6 @@ class IntrinsicBase
      * @brief Get the derivative of a projection of a 3D point into the camera plane
      * @param[in] pose The pose
      * @param[in] pt3D The 3D point
-     * @param[in] applyDistortion If true, apply the distortion if there is any
      * @return The projection jacobian with respect to the pose
      */
     virtual Eigen::Matrix<double, 2, 16> getDerivativeProjectWrtPoseLeft(const Eigen::Matrix4d& pose, const Vec4& pt3D) const = 0;
@@ -143,7 +141,6 @@ class IntrinsicBase
      * @brief Get the derivative of a projection of a 3D point into the camera plane
      * @param[in] pose The pose
      * @param[in] pt3D The 3D point
-     * @param[in] applyDistortion If true, apply the distortion if there is any
      * @return The projection jacobian with respect to the point
      */
     virtual Eigen::Matrix<double, 2, 4> getDerivativeProjectWrtPoint(const Eigen::Matrix4d& pose, const Vec4& pt3D) const = 0;
@@ -152,7 +149,6 @@ class IntrinsicBase
      * @brief Get the derivative of a projection of a 3D point into the camera plane
      * @param[in] pose The pose
      * @param[in] pt3D The 3D point
-     * @param[in] applyDistortion If true, apply the distortion if there is any
      * @return The projection jacobian with respect to the point
      */
     virtual Eigen::Matrix<double, 2, 3> getDerivativeProjectWrtPoint3(const Eigen::Matrix4d& pose, const Vec4& pt3D) const = 0;
@@ -161,7 +157,6 @@ class IntrinsicBase
      * @brief Get the derivative of a projection of a 3D point into the camera plane
      * @param[in] pose The pose
      * @param[in] pt3D The 3D point
-     * @param[in] applyDistortion If true, apply the distortion if there is any
      * @return The projection jacobian with respect to the params
      */
     virtual Eigen::Matrix<double, 2, Eigen::Dynamic> getDerivativeProjectWrtParams(const Eigen::Matrix4d& pos, const Vec4& pt3D) const = 0;
@@ -171,12 +166,15 @@ class IntrinsicBase
      * @param[in] pose The pose
      * @param[in] X The 3D projected point
      * @param[in] x The image observation
+     * @param[in] applyDistortion If true, apply the distortion if there is any
      * @return residual between the 3D projected point and the image observation
      */
-    inline Vec2 residual(const geometry::Pose3& pose, const Vec4& X, const Vec2& x) const
+    inline Vec2 residual(const geometry::Pose3& pose, const Vec4& X, const Vec2& x, bool applyDistortion = true) const
     {
-        const Vec2 proj = this->project(pose, X);
-        return x - proj;
+        // We will compare to an undistorted point, so always ignore the distortion when computing coordinates
+        const Vec2 proj = this->project(pose, X, false);
+
+        return ((applyDistortion)?this->getUndistortedPixel(x):x) - proj;
     }
 
     /**

src/aliceVision/multiview/resection/ResectionSphericalKernel.hpp

@@ -25,11 +25,11 @@ class ResectionSphericalKernel
                     const std::vector<Eigen::Vector3d> & structure, 
                     const std::vector<Eigen::Vector2d> & observations) 
     : _camera(camera), 
-    _structure(structure), 
-    _observations(observations)
+    _structure(structure)
     {
-        for (const auto & pt : _observations)
+        for (const auto & pt : observations)
         {
+            _observations.push_back(_camera->getUndistortedPixel(pt));
             _liftedObservations.push_back(_camera->toUnitSphere(_camera->removeDistortion(_camera->ima2cam(pt))));
         }
 
@@ -120,7 +120,7 @@ class ResectionSphericalKernel
             const Vec4 X = _structure[idx].homogeneous();
             const Vec2 x = _observations[idx];
 
-            const Vec2 residual = _camera->residual(geometry::Pose3(model), X, x);
+            const Vec2 residual = _camera->residual(geometry::Pose3(model), X, x, false);
 
             errors[idx] = residual.norm();
         }

src/aliceVision/multiview/triangulation/TriangulationSphericalKernel.hpp

@@ -29,15 +29,16 @@ class TriangulationSphericalKernel : public robustEstimation::IRansacKernel<robu
         const std::vector<Eigen::Matrix4d>& poses, 
         std::vector<std::shared_ptr<camera::IntrinsicBase>> & intrinsics
     )
-  : _observations(observations)
-  , _poses(poses)
+  : _poses(poses)
   , _intrinsics(intrinsics)
   {
     for (int id = 0; id < observations.size(); id++)
     {
         //Lift all points onto the metric unit sphere
-        const Vec2 & obs = _observations[id];
+        const Vec2 & obs = observations[id];
         std::shared_ptr<camera::IntrinsicBase> camera = _intrinsics[id];
+
+        _observations.push_back(camera->getUndistortedPixel(obs));
         _lifted.push_back(camera->toUnitSphere(camera->removeDistortion(camera->ima2cam(obs))));
     }
   }
@@ -146,7 +147,7 @@ class TriangulationSphericalKernel : public robustEstimation::IRansacKernel<robu
         X = X / X(3);
     }
 
-    Vec2 residual = _intrinsics[sample]->residual(_poses[sample], X, _observations[sample]);
+    Vec2 residual = _intrinsics[sample]->residual(_poses[sample], X, _observations[sample], false);
 
     return residual.norm();
   }
@@ -215,7 +216,7 @@ class TriangulationSphericalKernel : public robustEstimation::IRansacKernel<robu
 
 private:
   std::vector<Vec3> _lifted;
-  const std::vector<Vec2> _observations;
+  std::vector<Vec2> _observations;
   const std::vector<Eigen::Matrix4d> _poses;
   const std::vector<std::shared_ptr<camera::IntrinsicBase>> _intrinsics;
   multiview::TriangulateNViewsSphericalSolver _solver;

src/aliceVision/sfm/pipeline/expanding/ExpansionPolicyLegacy.cpp

@@ -188,7 +188,7 @@ double ExpansionPolicyLegacy::computeScore(const track::TracksMap & tracksMap,
         }
 
         //The higher the level, the higher the weight per cell
-        double w = pow(2.0, shiftLevel);
+        double w = pow(2.0, countLevels - (shiftLevel + 1));
         sum += w * double(size);
     }
 

src/aliceVision/sfm/pipeline/expanding/SfmTriangulation.cpp

@@ -139,6 +139,7 @@ bool SfmTriangulation::processTrack(
 
     if (observations.size() <= 0)
     {
+        return false;
     }
     else 
     {

src/software/pipeline/main_sfmBootstraping.cpp

@@ -294,8 +294,9 @@ int aliceVision_main(int argc, char** argv)
     ALICEVISION_LOG_INFO("Give a score to all pairs");
     int count = 0;
 
-    double bestScore = 0.0;
+    double bestScore = std::numeric_limits<double>::lowest();
     sfm::ReconstructedPair bestPair;
+    bestPair.reference = UndefinedIndexT;
     std::vector<std::size_t> bestUsedTracks;
 
     for (const sfm::ReconstructedPair & pair: reconstructedPairs)
@@ -308,9 +309,10 @@ int aliceVision_main(int argc, char** argv)
             continue;
         }
 
+        //If the angle is too small, then dramatically reduce its chances
         if (radianToDegree(angle) < minAngle)
         {
-            continue;
+            angle = -1.0 / angle;
         }
 
         const sfmData::View & vref = sfmData.getView(pair.reference);
@@ -332,6 +334,12 @@ int aliceVision_main(int argc, char** argv)
         }
     }
 
+    if (bestPair.reference == UndefinedIndexT)
+    {
+        ALICEVISION_LOG_INFO("No valid pair");
+        return EXIT_FAILURE;
+    }
+
     if (!buildSfmData(sfmData, bestPair.reference, bestPair.next, bestPair.pose, tracksHandler.getAllTracks(), bestUsedTracks))
     {
         return EXIT_FAILURE;