[all] replace camera optical center with camera offset from optical c…

…enter
alicevision · Aug 12, 2021 · 714d52f · 714d52f
1 parent 73fef34
commit 714d52f
Show file tree

Hide file tree

Showing 22 changed files with 192 additions and 163 deletions.
diff --git a/src/aliceVision/calibration/distortioncalibration_test.cpp b/src/aliceVision/calibration/distortioncalibration_test.cpp
@@ -25,7 +25,7 @@ using namespace aliceVision;
 //-----------------
 BOOST_AUTO_TEST_CASE(distortionCalibration_calibrate_classicld)
 {
-  const camera::Pinhole3DEClassicLD cam(1000, 1000, 1000, 1000, 500, 500, -0.34768564335290314, 1.5809150001711287, -0.17204522667665839, -0.15541950225726325, 1.1240093674337683);
+  const camera::Pinhole3DEClassicLD cam(1000, 1000, 1000, 1000, 0, 0, -0.34768564335290314, 1.5809150001711287, -0.17204522667665839, -0.15541950225726325, 1.1240093674337683);
 
   //Create points
   std::vector<calibration::PointPair> pts;
@@ -48,7 +48,7 @@ BOOST_AUTO_TEST_CASE(distortionCalibration_calibrate_classicld)
 
   //Calibrate
   calibration::Statistics st;
-  std::shared_ptr<camera::Pinhole> estimatedCam = std::make_shared<camera::Pinhole3DEClassicLD>(1000, 1000, 1000, 1000, 500, 500, 0, M_PI_2, 0, 0, 0);
+  std::shared_ptr<camera::Pinhole> estimatedCam = std::make_shared<camera::Pinhole3DEClassicLD>(1000, 1000, 1000, 1000, 0, 0, 0, M_PI_2, 0, 0, 0);
 
   std::vector<bool> lockedDistortions = {false, false, false, false, false};
   BOOST_CHECK(calibration::estimate(estimatedCam, st, pts, true, false, lockedDistortions));
@@ -75,7 +75,7 @@ BOOST_AUTO_TEST_CASE(distortionCalibration_calibrate_classicld)
 //-----------------
 BOOST_AUTO_TEST_CASE(distortionCalibration_calibrate_radial4)
 {
-  const camera::Pinhole3DERadial4 cam(1000, 1000, 1000, 1000, 500, 500, -0.4839495643487452, 1.0301284234642258, 0.014928332802185664, -0.0007797104872758904, -0.038994206396183909, 8.0474385001183646e-05);
+  const camera::Pinhole3DERadial4 cam(1000, 1000, 1000, 1000, 0, 0, -0.4839495643487452, 1.0301284234642258, 0.014928332802185664, -0.0007797104872758904, -0.038994206396183909, 8.0474385001183646e-05);
 
   //Create points
   std::vector<calibration::PointPair> pts;
@@ -98,7 +98,7 @@ BOOST_AUTO_TEST_CASE(distortionCalibration_calibrate_radial4)
 
   //Calibrate
   calibration::Statistics st;
-  std::shared_ptr<camera::Pinhole> estimatedCam = std::make_shared<camera::Pinhole3DERadial4>(1000, 1000, 1000, 1000, 500, 500, 0, 0, 0, 0, 0);
+  std::shared_ptr<camera::Pinhole> estimatedCam = std::make_shared<camera::Pinhole3DERadial4>(1000, 1000, 1000, 1000, 0, 0, 0, 0, 0, 0, 0);
 
   std::vector<bool> lockedDistortions = {false, false, false, false, false, false};
   BOOST_CHECK(calibration::estimate(estimatedCam, st, pts, true, false, lockedDistortions));
@@ -127,7 +127,7 @@ BOOST_AUTO_TEST_CASE(distortionCalibration_calibrate_radial4)
 //-----------------
 BOOST_AUTO_TEST_CASE(distortionCalibration_calibrate_lines_classicld)
 {
-  const camera::Pinhole3DEClassicLD cam(1000, 1000, 1000, 1000, 500, 500, -0.34768564335290314, 1.5809150001711287, -0.17204522667665839, -0.15541950225726325, 1.1240093674337683);
+  const camera::Pinhole3DEClassicLD cam(1000, 1000, 1000, 1000, 0, 0, -0.34768564335290314, 1.5809150001711287, -0.17204522667665839, -0.15541950225726325, 1.1240093674337683);
 
   //Create points
   std::vector<calibration::PointPair> pts;
@@ -180,7 +180,7 @@ BOOST_AUTO_TEST_CASE(distortionCalibration_calibrate_lines_classicld)
 
   //Calibrate
   calibration::Statistics st;
-  std::shared_ptr<camera::Pinhole> estimatedCam = std::make_shared<camera::Pinhole3DEClassicLD>(1000, 1000, 1000, 1000, 500, 500, 0, M_PI_2, 0, 0, 0);
+  std::shared_ptr<camera::Pinhole> estimatedCam = std::make_shared<camera::Pinhole3DEClassicLD>(1000, 1000, 1000, 1000, 0, 0, 0, M_PI_2, 0, 0, 0);
 
   {
     std::vector<bool> lockedDistortions = {true, true, true, true, true};
@@ -218,7 +218,7 @@ BOOST_AUTO_TEST_CASE(distortionCalibration_calibrate_lines_classicld)
 //-----------------
 BOOST_AUTO_TEST_CASE(distortionCalibration_calibrate_lines_radial4)
 {
-  const camera::Pinhole3DERadial4 cam(1000, 1000, 1000, 1000, 500, 500, -0.4839495643487452, 1.0301284234642258, 0.014928332802185664, -0.0007797104872758904, -0.038994206396183909, 8.0474385001183646e-05);
+  const camera::Pinhole3DERadial4 cam(1000, 1000, 1000, 1000, 0, 0, -0.4839495643487452, 1.0301284234642258, 0.014928332802185664, -0.0007797104872758904, -0.038994206396183909, 8.0474385001183646e-05);
 
   //Create points
   std::vector<calibration::PointPair> pts;
@@ -271,7 +271,7 @@ BOOST_AUTO_TEST_CASE(distortionCalibration_calibrate_lines_radial4)
 
   //Calibrate
   calibration::Statistics st;
-  std::shared_ptr<camera::Pinhole> estimatedCam = std::make_shared<camera::Pinhole3DERadial4>(1000, 1000, 1000, 1000, 500, 500, 0, 0, 0, 0, 0, 0);
+  std::shared_ptr<camera::Pinhole> estimatedCam = std::make_shared<camera::Pinhole3DERadial4>(1000, 1000, 1000, 1000, 0, 0, 0, 0, 0, 0, 0, 0);
 
   {
     std::vector<bool> lockedDistortions = {true, true, true, true, true, true};

diff --git a/src/aliceVision/camera/Equidistant.hpp b/src/aliceVision/camera/Equidistant.hpp
@@ -389,7 +389,7 @@ class EquiDistant : public IntrinsicsScaleOffsetDisto
   // Transform a point from the camera plane to the image plane
   Vec2 cam2ima(const Vec2& p) const override
   {
-    return _circleRadius * p  + _offset;
+    return _circleRadius * p  + getPrincipalPoint();
   }
 
   Eigen::Matrix2d getDerivativeCam2ImaWrtPoint() const override
@@ -400,7 +400,7 @@ class EquiDistant : public IntrinsicsScaleOffsetDisto
   // Transform a point from the image plane to the camera plane
   Vec2 ima2cam(const Vec2& p) const override
   {
-    return (p - _offset) / _circleRadius;
+    return (p - getPrincipalPoint()) / _circleRadius;
   }
 
   Eigen::Matrix2d getDerivativeIma2CamWrtPoint() const override

diff --git a/src/aliceVision/camera/IntrinsicsScaleOffset.hpp b/src/aliceVision/camera/IntrinsicsScaleOffset.hpp
@@ -52,10 +52,19 @@ class IntrinsicsScaleOffset: public IntrinsicBase
     return _offset; 
   }
 
+  inline const Vec2 getPrincipalPoint() const 
+  {
+    Vec2 ret = _offset;
+    ret(0) += double(_w) * 0.5;
+    ret(1) += double(_h) * 0.5;
+
+    return ret;
+  }
+
   // Transform a point from the camera plane to the image plane
   Vec2 cam2ima(const Vec2& p) const override
   {
-    return p.cwiseProduct(_scale) + _offset;
+    return p.cwiseProduct(_scale) + getPrincipalPoint();
   }
 
   virtual Eigen::Matrix2d getDerivativeCam2ImaWrtScale(const Vec2& p) const
@@ -88,8 +97,10 @@ class IntrinsicsScaleOffset: public IntrinsicBase
   {
     Vec2 np;
 
-    np(0) = (p(0) - _offset(0)) / _scale(0);
-    np(1) = (p(1) - _offset(1)) / _scale(1);
+    Vec2 pp = getPrincipalPoint();
+
+    np(0) = (p(0) - pp(0)) / _scale(0);
+    np(1) = (p(1) - pp(1)) / _scale(1);
 
     return np;
   }
@@ -98,8 +109,10 @@ class IntrinsicsScaleOffset: public IntrinsicBase
   {
       Eigen::Matrix2d M = Eigen::Matrix2d::Zero();
 
-      M(0, 0) = -(p(0) - _offset(0)) / (_scale(0) * _scale(0));
-      M(1, 1) = -(p(1) - _offset(1)) / (_scale(1) * _scale(1));
+      Vec2 pp = getPrincipalPoint();
+
+      M(0, 0) = -(p(0) - pp(0)) / (_scale(0) * _scale(0));
+      M(1, 1) = -(p(1) - pp(1)) / (_scale(1) * _scale(1));
 
       return M;
   }

diff --git a/src/aliceVision/camera/Pinhole.hpp b/src/aliceVision/camera/Pinhole.hpp
@@ -59,11 +59,6 @@ class Pinhole : public IntrinsicsScaleOffsetDisto
     return _scale(1); 
   }
 
-  Vec2 getPrincipalPoint() const 
-  { 
-    return _offset; 
-  }
-
   bool isValid() const override
   {
     return getFocalLengthPixX() > 0 && getFocalLengthPixY() > 0 && IntrinsicBase::isValid(); 
@@ -77,8 +72,11 @@ class Pinhole : public IntrinsicsScaleOffsetDisto
   Mat3 K() const
   {
     Mat3 K;
-    K  << _scale(0), 0.0, _offset(0),
-          0.0, _scale(1), _offset(1),
+
+    Vec2 pp = getPrincipalPoint();
+
+    K  << _scale(0), 0.0, pp(0),
+          0.0, _scale(1), pp(1),
           0.0, 0.0, 1.0;
     return K;
   }
@@ -87,16 +85,16 @@ class Pinhole : public IntrinsicsScaleOffsetDisto
   {
     _scale(0) = focalLengthPix;
     _scale(1) = focalLengthPix;
-    _offset(0) = ppx;
-    _offset(1) = ppy;
+    _offset(0) = ppx - double(_w) * 0.5;
+    _offset(1) = ppy - double(_h) * 0.5;
   }
 
   void setK(const Mat3 & K)
   {
     _scale(0) = K(0, 0);
     _scale(1) = K(1, 1);
-    _offset(0) = K(0, 2);
-    _offset(1) = K(1, 2);
+    _offset(0) = K(0, 2) - double(_w) * 0.5;
+    _offset(1) = K(1, 2) - double(_h) * 0.5;
   }
 
   Vec2 project(const geometry::Pose3& pose, const Vec3& pt, bool applyDistortion = true) const override
@@ -247,13 +245,8 @@ class Pinhole : public IntrinsicsScaleOffsetDisto
   Mat34 getProjectiveEquivalent(const geometry::Pose3 & pose) const
   {
     Mat34 P;
-    Mat3 K = Eigen::Matrix3d::Identity();
-    K(0, 0) = _scale(0);
-    K(1, 1) = _scale(1);
-    K(0, 2) = _offset(0);
-    K(1, 2) = _offset(1);
 
-    P_from_KRt(K, pose.rotation(), pose.translation(), &P);
+    P_from_KRt(K(), pose.rotation(), pose.translation(), &P);
     return P;
   }
 

diff --git a/src/aliceVision/camera/pinhole3DE_test.cpp b/src/aliceVision/camera/pinhole3DE_test.cpp
@@ -26,7 +26,7 @@ using namespace aliceVision::camera;
 //-----------------
 BOOST_AUTO_TEST_CASE(cameraPinhole3DE_disto_undisto_Radial4)
 {
-  const Pinhole3DERadial4 cam(1000, 1000, 1000, 1000, 500, 500,
+  const Pinhole3DERadial4 cam(1000, 1000, 1000, 1000, 0, 0,
     -0.4839495643487452,
     1.0301284234642258,
     0.014928332802185664,
@@ -70,7 +70,7 @@ BOOST_AUTO_TEST_CASE(cameraPinhole3DE_disto_undisto_Radial4)
 //-----------------
 BOOST_AUTO_TEST_CASE(cameraPinhole3DE_disto_undisto_ClassicLD)
 {
-  const Pinhole3DEClassicLD cam(1000, 1000, 1000, 1000, 500, 500,
+  const Pinhole3DEClassicLD cam(1000, 1000, 1000, 1000, 0, 0,
     -0.34768564335290314,
     1.5809150001711287,
     -0.17204522667665839,

diff --git a/src/aliceVision/camera/pinholeBrown_test.cpp b/src/aliceVision/camera/pinholeBrown_test.cpp
@@ -27,7 +27,7 @@ using namespace aliceVision::camera;
 //-----------------
 BOOST_AUTO_TEST_CASE(cameraPinholeBrown_disto_undisto_T2)
 {
-  const PinholeBrownT2 cam(1000, 1000, 1000, 1000, 500, 500,
+  const PinholeBrownT2 cam(1000, 1000, 1000, 1000, 0, 0,
   // K1, K2, K3, T1, T2
   -0.054, 0.014, 0.006, 0.001, -0.001);
 

diff --git a/src/aliceVision/camera/pinholeFisheye1_test.cpp b/src/aliceVision/camera/pinholeFisheye1_test.cpp
@@ -26,7 +26,7 @@ using namespace aliceVision::camera;
 //-----------------
 BOOST_AUTO_TEST_CASE(cameraPinholeFisheye_disto_undisto_Fisheye1)
 {
-  const PinholeFisheye1 cam(1000, 1000, 1000, 1000, 500, 500,
+  const PinholeFisheye1 cam(1000, 1000, 1000, 1000, 0, 0,
                                     0.1); // K1
 
   const double epsilon = 1e-4;

diff --git a/src/aliceVision/camera/pinholeFisheye_test.cpp b/src/aliceVision/camera/pinholeFisheye_test.cpp
@@ -27,7 +27,7 @@ using namespace aliceVision::camera;
 //-----------------
 BOOST_AUTO_TEST_CASE(cameraPinholeFisheye_disto_undisto_Fisheye)
 {
-  const PinholeFisheye cam(1000, 1000, 1000, 1000, 500, 500,
+  const PinholeFisheye cam(1000, 1000, 1000, 1000, 0, 0,
                                     -0.054, 0.014, 0.006, 0.011); // K1, K2, K3, K4
 
   const double epsilon = 1e-4;

diff --git a/src/aliceVision/camera/pinholeRadial_test.cpp b/src/aliceVision/camera/pinholeRadial_test.cpp
@@ -27,7 +27,7 @@ using namespace aliceVision::camera;
 //-----------------
 BOOST_AUTO_TEST_CASE(cameraPinholeRadial_disto_undisto_K1)
 {
-  const PinholeRadialK1 cam(1000, 1000, 1000, 1000, 500, 500,
+  const PinholeRadialK1 cam(1000, 1000, 1000, 1000, 0, 0,
     // K1
     0.1);
 
@@ -67,7 +67,7 @@ BOOST_AUTO_TEST_CASE(cameraPinholeRadial_disto_undisto_K1)
 //-----------------
 BOOST_AUTO_TEST_CASE(cameraPinholeRadial_disto_undisto_K3)
 {
-  const PinholeRadialK3 cam(1000, 1000, 1000, 500, 500,
+  const PinholeRadialK3 cam(1000, 1000, 1000, 1000, 0, 0,
     // K1, K2, K3
     -0.245539, 0.255195, 0.163773);
 

diff --git a/src/aliceVision/image/image_test.cpp b/src/aliceVision/image/image_test.cpp
@@ -26,12 +26,12 @@ BOOST_AUTO_TEST_CASE(Image_Basis)
   imaGray(2,2) = 2;
   imaGray(5,0) = 2;
 
-  cout << imaGray << endl << endl;
+  //cout << imaGray << endl << endl;
   //-- Get raw ptr to image data :
   const unsigned char * ptr = imaGray.data();
   ((unsigned char*)ptr)[0] = 2;
   fill(((unsigned char*)ptr+9*10),((unsigned char*)ptr+10*10),2);
-  cout << "After" << endl << imaGray;
+  //cout << "After" << endl << imaGray;
 
   // Construction by re-copy
   Image<unsigned char> imageGray2(imaGray);

diff --git a/src/aliceVision/multiview/resection/resectionLORansac_test.cpp b/src/aliceVision/multiview/resection/resectionLORansac_test.cpp
@@ -227,7 +227,7 @@ BOOST_AUTO_TEST_CASE(P3P_Ransac_noisyFromImagePoints)
     refinePoseAsItShouldbe(pts3D,
                            pts2Dnorm,
                            inliers,
-                           new camera::Pinhole(WIDTH, HEIGHT, 1, 1, 0, 0),
+                           new camera::Pinhole(0, 0, 1, 1, 0, 0),
                            pose,
                            true,
                            false );

diff --git a/src/aliceVision/sfm/BundleAdjustment.hpp b/src/aliceVision/sfm/BundleAdjustment.hpp
@@ -106,11 +106,11 @@ class BundleAdjustment
     REFINE_TRANSLATION = 2,                               //< refine pose translations
     REFINE_STRUCTURE = 4,                                 //< refine structure (i.e. 3D points)
     REFINE_INTRINSICS_FOCAL = 8,                          //< refine the focal length
-    REFINE_INTRINSICS_OPTICALCENTER_ALWAYS = 16,          //< refine the optical center
-    REFINE_INTRINSICS_OPTICALCENTER_IF_ENOUGH_DATA = 32,  //< refine the optical center only if we have a minimum number of cameras
+    REFINE_INTRINSICS_OPTICALOFFSET_ALWAYS = 16,          //< refine the optical offset from the center
+    REFINE_INTRINSICS_OPTICALOFFSET_IF_ENOUGH_DATA = 32,  //< refine the optical offset only if we have a minimum number of cameras
     REFINE_INTRINSICS_DISTORTION = 64,                    //< refine the distortion parameters
     /// Refine all intrinsics parameters
-    REFINE_INTRINSICS_ALL = REFINE_INTRINSICS_FOCAL | REFINE_INTRINSICS_OPTICALCENTER_IF_ENOUGH_DATA | REFINE_INTRINSICS_DISTORTION,
+    REFINE_INTRINSICS_ALL = REFINE_INTRINSICS_FOCAL | REFINE_INTRINSICS_OPTICALOFFSET_IF_ENOUGH_DATA | REFINE_INTRINSICS_DISTORTION,
     /// Refine all parameters
     REFINE_ALL = REFINE_ROTATION | REFINE_TRANSLATION | REFINE_INTRINSICS_ALL | REFINE_STRUCTURE,
   };