Merge pull request #1615 from alicevision/fix/undistortion

fix sfm with undistortion

Author: cbentejac

src/aliceVision/sfm/bundle/BundleAdjustmentCeres.cpp

@@ -204,6 +204,11 @@ ceres::CostFunction* createCostFunctionFromIntrinsics(const IntrinsicBase* intri
         if (undistortion)
         {
             obsUndistorted.x = undistortion->undistort(observation.x);
+
+            if (intrinsicDistortionPtr->getDistortion() != nullptr)
+            {
+                throw std::runtime_error("Distortion should not be there when undistortion exists");
+            }
         }
     }
 
@@ -224,7 +229,7 @@ ceres::CostFunction* createCostFunctionFromIntrinsics(const IntrinsicBase* intri
             return new ceres::AutoDiffCostFunction<ResidualErrorFunctor_Pinhole3DEClassicLD, 2, 9, 6, 3>(
               new ResidualErrorFunctor_Pinhole3DEClassicLD(w, h, obsUndistorted));
         case EINTRINSIC::PINHOLE_CAMERA_3DEANAMORPHIC4:
-            return new ceres::AutoDiffCostFunction<ResidualErrorFunctor_Pinhole, 2, 18, 6, 3>(new ResidualErrorFunctor_Pinhole(w, h, obsUndistorted));
+            return new ceres::AutoDiffCostFunction<ResidualErrorFunctor_Pinhole, 2, 4, 6, 3>(new ResidualErrorFunctor_Pinhole(w, h, obsUndistorted));
         case EINTRINSIC::PINHOLE_CAMERA_BROWN:
             return new ceres::AutoDiffCostFunction<ResidualErrorFunctor_PinholeBrownT2, 2, 9, 6, 3>(
               new ResidualErrorFunctor_PinholeBrownT2(w, h, obsUndistorted));

src/aliceVision/sfmDataIO/AlembicImporter.cpp

@@ -621,11 +621,15 @@ bool readCamera(const Version& abcVersion,
             {
                 distortion->setParameters(distortionParams);
             }
+
+
             std::shared_ptr<camera::Undistortion> undistortion = intrinsicCasted->getUndistortion();
             if (undistortion)
             {
                 undistortion->setParameters(undistortionParams);
                 undistortion->setOffset(undistortionOffset);
+                //If undistortion exists, distortion does not
+                intrinsicCasted->setDistortionObject(nullptr);
             }
         }
 

src/aliceVision/sfmDataIO/jsonIO.cpp

@@ -342,22 +342,6 @@ void loadIntrinsic(const Version& version, IndexT& intrinsicId, std::shared_ptr<
 
         intrinsicWithDistoEnabled->setDistortionInitializationMode(distortionInitializationMode);
 
-        std::shared_ptr<camera::Distortion> distortionObject = intrinsicWithDistoEnabled->getDistortion();
-        if (distortionObject)
-        {
-            std::vector<double> distortionParams;
-            for (bpt::ptree::value_type& paramNode : intrinsicTree.get_child("distortionParams"))
-            {
-                distortionParams.emplace_back(paramNode.second.get_value<double>());
-            }
-
-            // ensure that we have the right number of params
-            if (distortionParams.size() == distortionObject->getParameters().size())
-            {
-                distortionObject->setParameters(distortionParams);
-            }
-        }
-
         std::shared_ptr<camera::Undistortion> undistortionObject = intrinsicWithDistoEnabled->getUndistortion();
         if (undistortionObject)
         {
@@ -375,6 +359,25 @@ void loadIntrinsic(const Version& version, IndexT& intrinsicId, std::shared_ptr<
                 loadMatrix("undistortionOffset", offset, intrinsicTree);
                 undistortionObject->setOffset(offset);
             }
+
+            //If undistortion exists, distortion does not
+            intrinsicWithDistoEnabled->setDistortionObject(nullptr);
+        }
+
+        std::shared_ptr<camera::Distortion> distortionObject = intrinsicWithDistoEnabled->getDistortion();
+        if (distortionObject)
+        {
+            std::vector<double> distortionParams;
+            for (bpt::ptree::value_type& paramNode : intrinsicTree.get_child("distortionParams"))
+            {
+                distortionParams.emplace_back(paramNode.second.get_value<double>());
+            }
+
+            // ensure that we have the right number of params
+            if (distortionParams.size() == distortionObject->getParameters().size())
+            {
+                distortionObject->setParameters(distortionParams);
+            }
         }
     }