Merge pull request #1098 from alicevision/dev/focalMillimeter

Change focal length in sfmData file formats

.github/workflows/continuous-integration.yml

@@ -89,7 +89,7 @@ jobs:
           git clone --branch master https://github.com/alicevision/SfM_quality_evaluation.git
           cd SfM_quality_evaluation/
           # checkout a specific commit to ensure repeatability
-          git checkout 40ad6a089969cb9426a0b9879c28fbeb08c1a24c
+          git checkout 130d12a0f59ebe1695dafdc42dfe109fe08407ea
           export LD_LIBRARY_PATH=$PWD/../../../AV_install/lib64:${DEPS_INSTALL_DIR}/lib64:${DEPS_INSTALL_DIR}/lib:${LD_LIBRARY_PATH}
           echo "ldd aliceVision_cameraInit"
           ldd $PWD/../../../AV_install/bin/aliceVision_cameraInit

src/aliceVision/calibration/distortionEstimation.cpp

@@ -46,8 +46,8 @@ class CostLine : public ceres::CostFunction
         const int distortionSize = _camera->getDistortionParams().size();
 
         //Read parameters and update camera
-        _camera->setScale(parameter_scale[0], parameter_scale[1]);
-        _camera->setOffset(parameter_center[0], parameter_center[1]);
+        _camera->setScale({parameter_scale[0], parameter_scale[1]});
+        _camera->setOffset({parameter_center[0], parameter_center[1]});
         std::vector<double> cameraDistortionParams = _camera->getDistortionParams();
 
         for (int idParam = 0; idParam < distortionSize; idParam++)
@@ -151,8 +151,8 @@ class CostPoint : public ceres::CostFunction
         const int distortionSize = _camera->getDistortionParams().size();
 
         //Read parameters and update camera
-        _camera->setScale(parameter_scale[0], parameter_scale[1]);
-        _camera->setOffset(parameter_center[0], parameter_center[1]);
+        _camera->setScale({parameter_scale[0], parameter_scale[1]});
+        _camera->setOffset({parameter_center[0], parameter_center[1]});
         std::vector<double> cameraDistortionParams = _camera->getDistortionParams();
 
         for (int idParam = 0; idParam < distortionSize; idParam++)

src/aliceVision/camera/Distortion.hpp

@@ -14,6 +14,12 @@ class Distortion
 
     virtual Distortion* clone() const = 0;
 
+    // not virtual as child classes do not hold any data
+    bool operator==(const Distortion& other) const
+    {
+        return _distortionParams == other._distortionParams;
+    }
+
     std::vector<double>& getParameters()
     {
         return _distortionParams;

src/aliceVision/camera/Equidistant.hpp

@@ -32,17 +32,17 @@ class EquiDistant : public IntrinsicsScaleOffsetDisto
 public:
   EquiDistant() = default;
 
-  EquiDistant(unsigned int w, unsigned int h, double focalLengthPix, double ppx, double ppy,
+  EquiDistant(unsigned int w, unsigned int h, double focalLengthPix, double offsetX, double offsetY,
               std::shared_ptr<Distortion> distortion = nullptr)
-      : IntrinsicsScaleOffsetDisto(w, h, focalLengthPix, focalLengthPix, ppx, ppy, distortion)
+      : IntrinsicsScaleOffsetDisto(w, h, focalLengthPix, focalLengthPix, offsetX, offsetY, distortion)
       , _circleRadius(std::min(w, h) * 0.5)
       , _circleCenter(w / 2.0, h / 2.0)
   {
   }
 
-  EquiDistant(unsigned int w, unsigned int h, double focalLengthPix, double ppx, double ppy, double circleRadiusPix,
+  EquiDistant(unsigned int w, unsigned int h, double focalLengthPix, double offsetX, double offsetY, double circleRadiusPix,
               std::shared_ptr<Distortion> distortion = nullptr)
-      : IntrinsicsScaleOffsetDisto(w, h, focalLengthPix, focalLengthPix, ppx, ppy, distortion)
+      : IntrinsicsScaleOffsetDisto(w, h, focalLengthPix, focalLengthPix, offsetX, offsetY, distortion)
       , _circleRadius(circleRadiusPix)
       , _circleCenter(w / 2.0, h / 2.0)
   {

src/aliceVision/camera/EquidistantRadial.hpp

@@ -27,9 +27,9 @@ class EquiDistantRadialK3 : public EquiDistant
 public:
   EquiDistantRadialK3() = default;
 
-  explicit EquiDistantRadialK3(int w, int h, double focalLengthPix, double ppx, double ppy,
+  explicit EquiDistantRadialK3(int w, int h, double focalLengthPix, double offsetX, double offsetY,
                                double radius = 0.0, double k1 = 0.0, double k2 = 0.0, double k3 = 0.0)
-    : EquiDistant(w, h, focalLengthPix, ppx, ppy, (radius != 0.0 ? radius : std::min(w, h) * 0.5), std::shared_ptr<Distortion>(new DistortionRadialK3PT(k1, k2, k3)))
+    : EquiDistant(w, h, focalLengthPix, offsetX, offsetY, (radius != 0.0 ? radius : std::min(w, h) * 0.5), std::shared_ptr<Distortion>(new DistortionRadialK3PT(k1, k2, k3)))
   {
   }
 

src/aliceVision/camera/IntrinsicBase.hpp

@@ -26,8 +26,8 @@ namespace camera {
 class IntrinsicBase
 {
 public:
-
-  explicit IntrinsicBase(unsigned int width = 0, unsigned int height = 0, const std::string& serialNumber = "")
+  IntrinsicBase() = default;
+  explicit IntrinsicBase(unsigned int width, unsigned int height, const std::string& serialNumber = "")
     : _w(width)
     , _h(height)
     , _serialNumber(serialNumber)
@@ -103,27 +103,26 @@ class IntrinsicBase
    * @param[in] other
    * @return True if equals
    */
-  inline bool operator==(const IntrinsicBase& other) const
+  virtual bool operator==(const IntrinsicBase& other) const
   {
       if(getParams().size() != other.getParams().size())
       {
           return false;
       }
       for(int i = 0; i < getParams().size(); i++)
       {
-          const double diff = getParams()[i] - other.getParams()[i];
-          if(std::abs(diff) > 1e-8)
+          if(!isSimilar(getParams()[i], other.getParams()[i]))
           {
               return false;
           }
       }
-    return _w == other._w &&
-           _h == other._h &&
-           _sensorWidth == other._sensorWidth &&
-           _sensorHeight == other._sensorHeight &&
-           _serialNumber == other._serialNumber &&
-           _initializationMode == other._initializationMode &&
-           getType() == other.getType();
+      return _w == other._w &&
+             _h == other._h &&
+             _sensorWidth == other._sensorWidth &&
+             _sensorHeight == other._sensorHeight &&
+             _serialNumber == other._serialNumber &&
+             _initializationMode == other._initializationMode &&
+             getType() == other.getType();
   }
 
   inline bool operator!=(const IntrinsicBase& other) const

src/aliceVision/camera/IntrinsicsScaleOffset.hpp

@@ -35,18 +35,26 @@ class IntrinsicsScaleOffset: public IntrinsicBase
     *this = other;
   }
 
-  void setScale(double x, double y)
+  bool operator==(const IntrinsicBase& otherBase) const override
   {
-    _scale(0) = x;
-    _scale(1) = y;
+      if(!IntrinsicBase::operator==(otherBase))
+          return false;
+      if(typeid(*this) != typeid(otherBase))
+          return false;
+      const IntrinsicsScaleOffset& other = static_cast<const IntrinsicsScaleOffset&>(otherBase);
+      return _scale.isApprox(other._scale) && _offset.isApprox(other._offset);
+  }
+
+  void setScale(const Vec2& scale)
+  {
+    _scale = scale;
   }
 
   inline Vec2 getScale() const { return _scale; }
 
-  void setOffset(double offset_x, double offset_y)
+  void setOffset(const Vec2& offset)
   {
-    _offset(0) = offset_x;
-    _offset(1) = offset_y;
+    _offset = offset;
   }
 
   inline Vec2 getOffset() const 
@@ -209,7 +217,7 @@ class IntrinsicsScaleOffset: public IntrinsicBase
   /**
    * @brief Set initial Scale (for constraining minimization)
    */
-  inline void setInitialScale(double initialScale)
+  inline void setInitialScale(const Vec2 & initialScale)
   {
     _initialScale = initialScale;
   }
@@ -218,15 +226,30 @@ class IntrinsicsScaleOffset: public IntrinsicBase
    * @brief Get the intrinsic initial scale
    * @return The intrinsic initial scale
    */
-  inline double initialScale() const
+  inline Vec2 getInitialScale() const
   {
     return _initialScale;
   }
 
+  /**
+   * @brief lock the ratio between fx and fy
+   * @param lock is the ratio locked
+   */
+  void setRatioLocked(bool lock) 
+  {
+    _ratioLocked = lock;
+  }
+
+  bool isRatioLocked() const
+  {
+    return _ratioLocked;
+  }
+
 protected:
   Vec2 _scale{1.0, 1.0};
   Vec2 _offset{0.0, 0.0};
-  double _initialScale{-1};
+  Vec2 _initialScale{-1.0, -1.0};
+  bool _ratioLocked{true};
 };
 
 } // namespace camera

src/aliceVision/camera/IntrinsicsScaleOffsetDisto.hpp

@@ -32,9 +32,22 @@ class IntrinsicsScaleOffsetDisto : public IntrinsicsScaleOffset
     *this = dynamic_cast<const IntrinsicsScaleOffsetDisto&>(other);
   }
 
+  bool operator==(const IntrinsicBase& otherBase) const override
+  {
+      if(!IntrinsicsScaleOffset::operator==(otherBase))
+          return false;
+      if(typeid(*this) != typeid(otherBase))
+          return false;
+      const IntrinsicsScaleOffsetDisto& other = static_cast<const IntrinsicsScaleOffsetDisto&>(otherBase);
+
+      if(_pDistortion != nullptr && other._pDistortion != nullptr)
+          return (*_pDistortion) == (*other._pDistortion);
+      return _pDistortion == other._pDistortion;
+  }
+
   bool hasDistortion() const override
   {
-    return !(_pDistortion == nullptr);
+    return _pDistortion != nullptr;
   }
 
   Vec2 addDistortion(const Vec2& p) const override

src/aliceVision/camera/Pinhole.hpp

@@ -32,8 +32,9 @@ class Pinhole : public IntrinsicsScaleOffsetDisto
   {
   }
 
-  Pinhole(unsigned int w, unsigned int h, double focalLengthPixX, double focalLengthPixY, double ppx, double ppy, std::shared_ptr<Distortion> distortion = nullptr)
-  : IntrinsicsScaleOffsetDisto(w,h, focalLengthPixX, focalLengthPixY, ppx, ppy, distortion)
+  Pinhole(unsigned int w, unsigned int h, double focalLengthPixX, double focalLengthPixY, double offsetX, double offsetY,
+          std::shared_ptr<Distortion> distortion = nullptr)
+      : IntrinsicsScaleOffsetDisto(w, h, focalLengthPixX, focalLengthPixY, offsetX, offsetY, distortion)
   {
   }
 

src/aliceVision/camera/Pinhole3DE.hpp

@@ -21,8 +21,8 @@ class Pinhole3DERadial4 : public Pinhole
 {
   public:
 
-  explicit Pinhole3DERadial4(int w = 0, int h = 0, double focalLengthPixX = 0.0, double focalLengthPixY = 0.0, double ppx = 0, double ppy = 0, double c2 = 0.0, double c4 = 0.0, double u1 = 0.0, double v1 = 0.0, double u2 = 0.0, double v2 = 0.0)
-  : Pinhole(w, h, focalLengthPixX, focalLengthPixY, ppx, ppy, std::shared_ptr<Distortion>(new Distortion3DERadial4(c2, c4, u1, v1, u2, v2)))
+  explicit Pinhole3DERadial4(int w = 0, int h = 0, double focalLengthPixX = 0.0, double focalLengthPixY = 0.0, double offsetX = 0, double offsetY = 0, double c2 = 0.0, double c4 = 0.0, double u1 = 0.0, double v1 = 0.0, double u2 = 0.0, double v2 = 0.0)
+  : Pinhole(w, h, focalLengthPixX, focalLengthPixY, offsetX, offsetY, std::shared_ptr<Distortion>(new Distortion3DERadial4(c2, c4, u1, v1, u2, v2)))
   {
   }
 
@@ -39,8 +39,8 @@ class Pinhole3DEAnamorphic4 : public Pinhole
 {
   public:
 
-  explicit Pinhole3DEAnamorphic4(int w = 0, int h = 0, double focalLengthPixX = 0.0, double focalLengthPixY = 0.0, double ppx = 0, double ppy = 0, double cxx = 0.0, double cxy = 0.0, double cyx = 0.0, double cyy = 0.0)
-  : Pinhole(w, h, focalLengthPixX, focalLengthPixY, ppx, ppy, std::shared_ptr<Distortion>(new Distortion3DEAnamorphic4(cxx, cxy, cyx, cyy)))
+  explicit Pinhole3DEAnamorphic4(int w = 0, int h = 0, double focalLengthPixX = 0.0, double focalLengthPixY = 0.0, double offsetX = 0, double offsetY = 0, double cxx = 0.0, double cxy = 0.0, double cyx = 0.0, double cyy = 0.0)
+  : Pinhole(w, h, focalLengthPixX, focalLengthPixY, offsetX, offsetY, std::shared_ptr<Distortion>(new Distortion3DEAnamorphic4(cxx, cxy, cyx, cyy)))
   {
   }
 
@@ -57,8 +57,8 @@ class Pinhole3DEClassicLD : public Pinhole
 {
   public:
 
-  explicit Pinhole3DEClassicLD(int w = 0, int h = 0, double focalLengthPixX = 0.0, double focalLengthPixY = 0.0, double ppx = 0, double ppy = 0, double delta = 0.0, double epsilon = 1.0, double mux = 0.0, double muy = 0.0, double q = 0.0)
-  : Pinhole(w, h, focalLengthPixX, focalLengthPixY, ppx, ppy, std::shared_ptr<Distortion>(new Distortion3DEClassicLD(delta, epsilon, mux, muy, q)))
+  explicit Pinhole3DEClassicLD(int w = 0, int h = 0, double focalLengthPixX = 0.0, double focalLengthPixY = 0.0, double offsetX = 0, double offsetY = 0, double delta = 0.0, double epsilon = 1.0, double mux = 0.0, double muy = 0.0, double q = 0.0)
+  : Pinhole(w, h, focalLengthPixX, focalLengthPixY, offsetX, offsetY, std::shared_ptr<Distortion>(new Distortion3DEClassicLD(delta, epsilon, mux, muy, q)))
   {
   }
 

src/aliceVision/camera/PinholeBrown.hpp

@@ -23,8 +23,8 @@ class PinholeBrownT2 : public Pinhole
 {
     public:
 
-    explicit PinholeBrownT2(int w = 0, int h = 0, double focalLengthPixX = 0.0, double focalLengthPixY = 0.0, double ppx = 0, double ppy = 0, double k1 = 0.0, double k2 = 0.0, double k3 = 0.0, double t1 = 0.0, double t2 = 0.0)
-    : Pinhole(w, h, focalLengthPixX, focalLengthPixY, ppx, ppy, std::shared_ptr<Distortion>(new DistortionBrown(k1, k2, k3, t1, t2)))
+    explicit PinholeBrownT2(int w = 0, int h = 0, double focalLengthPixX = 0.0, double focalLengthPixY = 0.0, double offsetX = 0, double offsetY = 0, double k1 = 0.0, double k2 = 0.0, double k3 = 0.0, double t1 = 0.0, double t2 = 0.0)
+    : Pinhole(w, h, focalLengthPixX, focalLengthPixY, offsetX, offsetY, std::shared_ptr<Distortion>(new DistortionBrown(k1, k2, k3, t1, t2)))
     {
     }
 

src/aliceVision/camera/PinholeFisheye.hpp

@@ -26,8 +26,8 @@ class PinholeFisheye : public Pinhole
 {
   public:
 
-  explicit PinholeFisheye(int w = 0, int h = 0, double focalLengthPixX = 0.0, double focalLengthPixY = 0.0, double ppx = 0, double ppy = 0, double k1 = 0.0, double k2 = 0.0, double k3 = 0.0, double k4 = 0.0)
-  :Pinhole(w, h, focalLengthPixX, focalLengthPixY, ppx, ppy, std::shared_ptr<Distortion>(new DistortionFisheye(k1, k2, k3, k4)))
+  explicit PinholeFisheye(int w = 0, int h = 0, double focalLengthPixX = 0.0, double focalLengthPixY = 0.0, double offsetX = 0, double offsetY = 0, double k1 = 0.0, double k2 = 0.0, double k3 = 0.0, double k4 = 0.0)
+  :Pinhole(w, h, focalLengthPixX, focalLengthPixY, offsetX, offsetY, std::shared_ptr<Distortion>(new DistortionFisheye(k1, k2, k3, k4)))
   {
   }
 

src/aliceVision/camera/PinholeFisheye1.hpp

@@ -27,8 +27,8 @@ class PinholeFisheye1 : public Pinhole
 {
 public:
 
-  explicit PinholeFisheye1(int w = 0, int h = 0, double focalLengthPixX = 0.0, double focalLengthPixY = 0.0, double ppx = 0, double ppy = 0, double k1 = 0.0)
-  :Pinhole(w, h, focalLengthPixX, focalLengthPixY, ppx, ppy, std::shared_ptr<Distortion>(new DistortionFisheye1(k1)))
+  explicit PinholeFisheye1(int w = 0, int h = 0, double focalLengthPixX = 0.0, double focalLengthPixY = 0.0, double offsetX = 0, double offsetY = 0, double k1 = 0.0)
+  :Pinhole(w, h, focalLengthPixX, focalLengthPixY, offsetX, offsetY, std::shared_ptr<Distortion>(new DistortionFisheye1(k1)))
   {
   }
 

src/aliceVision/camera/PinholeRadial.hpp

@@ -23,8 +23,10 @@ class PinholeRadialK1 : public Pinhole
 {
 public:
 
-  explicit PinholeRadialK1(int w = 0, int h = 0, double focalLengthPixX = 0.0, double focalLengthPixY = 0.0, double ppx = 0, double ppy = 0, double k1 = 0.0)
-  :Pinhole(w, h, focalLengthPixX, focalLengthPixY, ppx, ppy, std::shared_ptr<Distortion>(new DistortionRadialK1(k1)))
+  explicit PinholeRadialK1(int w = 0, int h = 0, double focalLengthPixX = 0.0, double focalLengthPixY = 0.0,
+                             double offsetX = 0, double offsetY = 0, double k1 = 0.0)
+        : Pinhole(w, h, focalLengthPixX, focalLengthPixY, offsetX, offsetY,
+                  std::shared_ptr<Distortion>(new DistortionRadialK1(k1)))
   {
   }
 
@@ -42,8 +44,11 @@ class PinholeRadialK3 : public Pinhole
 {
   public:
 
-  explicit PinholeRadialK3(int w = 0, int h = 0, double focalLengthPixX = 0.0, double focalLengthPixY = 0.0, double ppx = 0, double ppy = 0, double k1 = 0.0, double k2 = 0.0, double k3 = 0.0)
-  : Pinhole(w, h, focalLengthPixX, focalLengthPixY, ppx, ppy, std::shared_ptr<Distortion>(new DistortionRadialK3(k1, k2, k3)))
+  explicit PinholeRadialK3(int w = 0, int h = 0, double focalLengthPixX = 0.0, double focalLengthPixY = 0.0,
+                               double offsetX = 0, double offsetY = 0, double k1 = 0.0, double k2 = 0.0,
+                               double k3 = 0.0)
+          : Pinhole(w, h, focalLengthPixX, focalLengthPixY, offsetX, offsetY,
+                    std::shared_ptr<Distortion>(new DistortionRadialK3(k1, k2, k3)))
   {
   }
 

src/aliceVision/camera/camera.hpp

@@ -25,32 +25,32 @@ namespace camera {
 inline std::shared_ptr<IntrinsicBase> createIntrinsic(EINTRINSIC intrinsicType,
     unsigned int w = 0, unsigned int h = 0,
     double focalLengthPixX = 0.0, double focalLengthPixY = 0.0,
-    double ppx = 0.0, double ppy = 0.0)
+    double offsetX = 0.0, double offsetY = 0.0)
 {
   switch(intrinsicType)
   {
     case EINTRINSIC::PINHOLE_CAMERA:
-      return std::make_shared<Pinhole>(w, h, focalLengthPixX, focalLengthPixY, ppx, ppy);
+      return std::make_shared<Pinhole>(w, h, focalLengthPixX, focalLengthPixY, offsetX, offsetY);
     case EINTRINSIC::PINHOLE_CAMERA_RADIAL1:
-      return std::make_shared<PinholeRadialK1>(w, h, focalLengthPixX, focalLengthPixY, ppx, ppy);
+      return std::make_shared<PinholeRadialK1>(w, h, focalLengthPixX, focalLengthPixY, offsetX, offsetY);
     case EINTRINSIC::PINHOLE_CAMERA_RADIAL3:
-      return std::make_shared<PinholeRadialK3>(w, h, focalLengthPixX, focalLengthPixY, ppx, ppy);
+      return std::make_shared<PinholeRadialK3>(w, h, focalLengthPixX, focalLengthPixY, offsetX, offsetY);
     case EINTRINSIC::PINHOLE_CAMERA_3DERADIAL4:
-      return std::make_shared<Pinhole3DERadial4>(w, h, focalLengthPixX, focalLengthPixY, ppx, ppy);
+      return std::make_shared<Pinhole3DERadial4>(w, h, focalLengthPixX, focalLengthPixY, offsetX, offsetY);
     case EINTRINSIC::PINHOLE_CAMERA_BROWN:
-      return std::make_shared<PinholeBrownT2>(w, h, focalLengthPixX, focalLengthPixY, ppx, ppy);
+      return std::make_shared<PinholeBrownT2>(w, h, focalLengthPixX, focalLengthPixY, offsetX, offsetY);
     case EINTRINSIC::PINHOLE_CAMERA_FISHEYE:
-      return std::make_shared<PinholeFisheye>(w, h, focalLengthPixX, focalLengthPixY, ppx, ppy);
+      return std::make_shared<PinholeFisheye>(w, h, focalLengthPixX, focalLengthPixY, offsetX, offsetY);
     case EINTRINSIC::PINHOLE_CAMERA_FISHEYE1:
-      return std::make_shared<PinholeFisheye1>(w, h, focalLengthPixX, focalLengthPixY, ppx, ppy);
+      return std::make_shared<PinholeFisheye1>(w, h, focalLengthPixX, focalLengthPixY, offsetX, offsetY);
     case EINTRINSIC::PINHOLE_CAMERA_3DEANAMORPHIC4:
-      return std::make_shared<Pinhole3DEAnamorphic4>(w, h, focalLengthPixX, focalLengthPixY, ppx, ppy);
+      return std::make_shared<Pinhole3DEAnamorphic4>(w, h, focalLengthPixX, focalLengthPixY, offsetX, offsetY);
     case EINTRINSIC::PINHOLE_CAMERA_3DECLASSICLD:
-      return std::make_shared<Pinhole3DEClassicLD>(w, h, focalLengthPixX, focalLengthPixY, ppx, ppy);
+      return std::make_shared<Pinhole3DEClassicLD>(w, h, focalLengthPixX, focalLengthPixY, offsetX, offsetY);
     case EINTRINSIC::EQUIDISTANT_CAMERA:
-      return std::make_shared<EquiDistant>(w, h, focalLengthPixX, ppx, ppy);
+      return std::make_shared<EquiDistant>(w, h, focalLengthPixX, offsetX, offsetY);
     case EINTRINSIC::EQUIDISTANT_CAMERA_RADIAL3:
-      return std::make_shared<EquiDistantRadialK3>(w, h, focalLengthPixX, ppx, ppy);
+      return std::make_shared<EquiDistantRadialK3>(w, h, focalLengthPixX, offsetX, offsetY);
     case EINTRINSIC::UNKNOWN:
     case EINTRINSIC::VALID_PINHOLE:
     case EINTRINSIC::VALID_EQUIDISTANT:

src/aliceVision/camera/equidistant_test.cpp

@@ -29,13 +29,13 @@ BOOST_AUTO_TEST_CASE(cameraEquidistant_disto_undisto_Radial)
   const int w = 1000;
   const int h = 800;
   const double focal = 800.0;
-  const double ppx = w * 0.5;
-  const double ppy = h * 0.5;
+  const double offsetX = 0.0;
+  const double offsetY = 0.0;
   const double radius = 0.0; // defined automatically
   const double k1 = 0.3;
   const double k2 = 0.2;
   const double k3 = 0.1;
-  const EquiDistantRadialK3 cam(w, h, focal, ppx, ppy, radius, k1, k2, k3);
+  const EquiDistantRadialK3 cam(w, h, focal, offsetX, offsetY, radius, k1, k2, k3);
 
   const double epsilon = 1e-4;
   for (int i = 0; i < 10; ++i)