From 7f36e2e254683414c552e5edc7264fc0a3d2c9cb Mon Sep 17 00:00:00 2001
From: "Kilic, Deniz" <d.kilic@fz-juelich.de>
Date: Mon, 28 Feb 2022 14:35:59 +0100
Subject: [PATCH 01/12] Deleted debug output and old method

---
 src/extrCalibration.cpp | 240 ++++++++--------------------------------
 1 file changed, 48 insertions(+), 192 deletions(-)

diff --git a/src/extrCalibration.cpp b/src/extrCalibration.cpp
index 722b13885..5d346c33e 100644
--- a/src/extrCalibration.cpp
+++ b/src/extrCalibration.cpp
@@ -863,219 +863,75 @@ cv::Point2f ExtrCalibration::getImagePoint(cv::Point3f p3d)
  */
 cv::Point3f ExtrCalibration::get3DPoint(cv::Point2f p2d, double h)
 {
-    bool debug = false;
-
-    if(debug)
-    {
-        std::cout << "get3DPoint: Start Point2D: (" << p2d.x << ", " << p2d.y << ") h: " << h << std::endl;
-    }
-
     int bS = mMainWindow->getImage() ? mMainWindow->getImageBorderSize() : 0;
 
-    if(false && bS > 0)
-    {
-        p2d.x += bS;
-        p2d.y += bS;
-    }
-
     // Ergebnis 3D-Punkt
     cv::Point3f resultPoint, tmpPoint;
 
+    double rvec_array[3], translation_vector[3];
+    rvec_array[0] = mControlWidget->getCalibExtrRot1();
+    rvec_array[1] = mControlWidget->getCalibExtrRot2();
+    rvec_array[2] = mControlWidget->getCalibExtrRot3();
 
-    bool newMethod = true;
-    /////////////// Start new method
-    if(newMethod)
-    {
-        double rvec_array[3], translation_vector[3];
-        rvec_array[0] = mControlWidget->getCalibExtrRot1();
-        rvec_array[1] = mControlWidget->getCalibExtrRot2();
-        rvec_array[2] = mControlWidget->getCalibExtrRot3();
-
-        cv::Mat rvec(3, 1, CV_64F, rvec_array), rot_inv;
-        cv::Mat rot_mat(3, 3, CV_64F), e(3, 3, CV_64F);
-        // Transform the rotation vector into a rotation matrix with opencvs rodrigues method
-        Rodrigues(rvec, rot_mat);
-
-        translation_vector[0] = rot_mat.at<double>(0, 0) * mControlWidget->getCalibExtrTrans1() +
-                                rot_mat.at<double>(0, 1) * mControlWidget->getCalibExtrTrans2() +
-                                rot_mat.at<double>(0, 2) * mControlWidget->getCalibExtrTrans3();
-        translation_vector[1] = rot_mat.at<double>(1, 0) * mControlWidget->getCalibExtrTrans1() +
-                                rot_mat.at<double>(1, 1) * mControlWidget->getCalibExtrTrans2() +
-                                rot_mat.at<double>(1, 2) * mControlWidget->getCalibExtrTrans3();
-        translation_vector[2] = rot_mat.at<double>(2, 0) * mControlWidget->getCalibExtrTrans1() +
-                                rot_mat.at<double>(2, 1) * mControlWidget->getCalibExtrTrans2() +
-                                rot_mat.at<double>(2, 2) * mControlWidget->getCalibExtrTrans3();
-
-        // use inverse Matrix
-        rot_inv = rot_mat.inv(cv::DECOMP_LU);
-        e       = rot_inv * rot_mat;
-
-        if(debug)
-        {
-            debout << "\n-.- ESTIMATED ROTATION\n";
-            for(int p = 0; p < 3; p++)
-            {
-                printf(
-                    "%20.18f, %20.18f, %20.18f\n",
-                    rot_mat.at<double>(p, 0),
-                    rot_mat.at<double>(p, 1),
-                    rot_mat.at<double>(p, 2));
-            }
-
-            debout << "\n-.- ESTIMATED ROTATION^-1\n";
-            for(int p = 0; p < 3; p++)
-            {
-                printf(
-                    "%20.18f, %20.18f, %20.18f\n",
-                    rot_inv.at<double>(p, 0),
-                    rot_inv.at<double>(p, 1),
-                    rot_inv.at<double>(p, 2));
-            }
-
-            debout << "\n-.- ESTIMATED R^-1*R\n";
-            for(int p = 0; p < 3; p++)
-            {
-                printf("%20.18f, %20.18f, %20.18f\n", e.at<double>(p, 0), e.at<double>(p, 1), e.at<double>(p, 2));
-            }
-
-            debout << "\n-.- ESTIMATED TRANSLATION\n";
-            debout << mControlWidget->getCalibExtrTrans1() << " , " << mControlWidget->getCalibExtrTrans2() << " , "
-                   << mControlWidget->getCalibExtrTrans3() << "\n";
-            debout << translation_vector[0] << " , " << translation_vector[1] << " , " << translation_vector[2] << "\n";
-
-            debout << "Det(rot_mat): " << determinant(rot_mat) << std::endl;
-            debout << "Det(rot_inv): " << determinant(rot_inv) << std::endl;
-        }
-        double z = h + rot_inv.at<double>(2, 0) * translation_vector[0] +
-                   rot_inv.at<double>(2, 1) * translation_vector[1] + rot_inv.at<double>(2, 2) * translation_vector[2];
-        if(debug)
-        {
-            debout << "##### z: " << h << " + " << rot_inv.at<double>(2, 0) << "*" << translation_vector[0] << " + "
-                   << rot_inv.at<double>(2, 1) << "*" << translation_vector[1] << " + " << rot_inv.at<double>(2, 2)
-                   << "*" << translation_vector[2] << " = " << z << std::endl;
-        }
-        z /=
-            (rot_inv.at<double>(2, 0) * (p2d.x - (mControlWidget->getCalibCxValue() - bS)) /
-                 mControlWidget->getCalibFxValue() +
-             rot_inv.at<double>(2, 1) * (p2d.y - (mControlWidget->getCalibCyValue() - bS)) /
-                 mControlWidget->getCalibFyValue() +
-             rot_inv.at<double>(2, 2));
-        if(debug)
-        {
-            std::cout << "###### z: " << z << std::endl;
-        }
-
-        resultPoint.x = (p2d.x - (mControlWidget->getCalibCxValue() - bS));
-        resultPoint.y = (p2d.y - (mControlWidget->getCalibCyValue() - bS));
-        resultPoint.z = z;
-
-        if(debug)
-        {
-            std::cout << "###### (" << resultPoint.x << ", " << resultPoint.y << ", " << resultPoint.z << ")"
-                      << std::endl;
-        }
+    cv::Mat rvec(3, 1, CV_64F, rvec_array), rot_inv;
+    cv::Mat rot_mat(3, 3, CV_64F), e(3, 3, CV_64F);
+    // Transform the rotation vector into a rotation matrix with opencvs rodrigues method
+    Rodrigues(rvec, rot_mat);
 
-        resultPoint.x = resultPoint.x * z / mControlWidget->getCalibFxValue();
-        resultPoint.y = resultPoint.y * z / mControlWidget->getCalibFyValue();
+    translation_vector[0] = rot_mat.at<double>(0, 0) * mControlWidget->getCalibExtrTrans1() +
+                            rot_mat.at<double>(0, 1) * mControlWidget->getCalibExtrTrans2() +
+                            rot_mat.at<double>(0, 2) * mControlWidget->getCalibExtrTrans3();
+    translation_vector[1] = rot_mat.at<double>(1, 0) * mControlWidget->getCalibExtrTrans1() +
+                            rot_mat.at<double>(1, 1) * mControlWidget->getCalibExtrTrans2() +
+                            rot_mat.at<double>(1, 2) * mControlWidget->getCalibExtrTrans3();
+    translation_vector[2] = rot_mat.at<double>(2, 0) * mControlWidget->getCalibExtrTrans1() +
+                            rot_mat.at<double>(2, 1) * mControlWidget->getCalibExtrTrans2() +
+                            rot_mat.at<double>(2, 2) * mControlWidget->getCalibExtrTrans3();
 
-        if(debug)
-        {
-            std::cout << "###### After intern re-calibration: (" << resultPoint.x << ", " << resultPoint.y << ", "
-                      << resultPoint.z << ")" << std::endl;
-        }
+    // use inverse Matrix
+    rot_inv = rot_mat.inv(cv::DECOMP_LU);
 
-        tmpPoint.x = resultPoint.x - translation_vector[0];
-        tmpPoint.y = resultPoint.y - translation_vector[1];
-        tmpPoint.z = resultPoint.z - translation_vector[2];
 
-        if(debug)
-        {
-            std::cout << "###### After translation: (" << tmpPoint.x << ", " << tmpPoint.y << ", " << tmpPoint.z << ")"
-                      << std::endl;
-        }
+    double z = h + rot_inv.at<double>(2, 0) * translation_vector[0] + rot_inv.at<double>(2, 1) * translation_vector[1] +
+               rot_inv.at<double>(2, 2) * translation_vector[2];
 
-        resultPoint.x = rot_inv.at<double>(0, 0) * (tmpPoint.x) + rot_inv.at<double>(0, 1) * (tmpPoint.y) +
-                        rot_inv.at<double>(0, 2) * (tmpPoint.z);
-        resultPoint.y = rot_inv.at<double>(1, 0) * (tmpPoint.x) + rot_inv.at<double>(1, 1) * (tmpPoint.y) +
-                        rot_inv.at<double>(1, 2) * (tmpPoint.z);
-        resultPoint.z = rot_inv.at<double>(2, 0) * (tmpPoint.x) + rot_inv.at<double>(2, 1) * (tmpPoint.y) +
-                        rot_inv.at<double>(2, 2) * (tmpPoint.z);
+    z /=
+        (rot_inv.at<double>(2, 0) * (p2d.x - (mControlWidget->getCalibCxValue() - bS)) /
+             mControlWidget->getCalibFxValue() +
+         rot_inv.at<double>(2, 1) * (p2d.y - (mControlWidget->getCalibCyValue() - bS)) /
+             mControlWidget->getCalibFyValue() +
+         rot_inv.at<double>(2, 2));
 
-        if(debug)
-        {
-            std::cout << "#resultPoint: (" << resultPoint.x << ", " << resultPoint.y << ", " << resultPoint.z << ")"
-                      << std::endl;
-        }
-        if(debug)
-        {
-            std::cout << "Coord Translation: x: " << mControlWidget->getCalibCoord3DTransX()
-                      << ", y: " << mControlWidget->getCalibCoord3DTransY()
-                      << ", z: " << mControlWidget->getCalibCoord3DTransZ() << std::endl;
-        }
 
+    resultPoint.x = (p2d.x - (mControlWidget->getCalibCxValue() - bS));
+    resultPoint.y = (p2d.y - (mControlWidget->getCalibCyValue() - bS));
+    resultPoint.z = z;
 
-        // Coordinate Transformations
 
-        resultPoint.x -= mControlWidget->getCalibCoord3DTransX();
-        resultPoint.y -= mControlWidget->getCalibCoord3DTransY();
-        resultPoint.z -= mControlWidget->getCalibCoord3DTransZ();
+    resultPoint.x = resultPoint.x * z / mControlWidget->getCalibFxValue();
+    resultPoint.y = resultPoint.y * z / mControlWidget->getCalibFyValue();
 
-        resultPoint.x *= mControlWidget->getCalibCoord3DSwapX() ? -1 : 1;
-        resultPoint.y *= mControlWidget->getCalibCoord3DSwapY() ? -1 : 1;
-        resultPoint.z *= mControlWidget->getCalibCoord3DSwapZ() ? -1 : 1;
-    }
-    else //////////////// End new method
-    {
-        //////////////// Start old method
 
-        cv::Point3f camInWorld = transformRT(cv::Point3f(0, 0, 0));
+    tmpPoint.x = resultPoint.x - translation_vector[0];
+    tmpPoint.y = resultPoint.y - translation_vector[1];
+    tmpPoint.z = resultPoint.z - translation_vector[2];
 
-        // 3D-Punkt vor der Kamera mit Tiefe 5
-        CvPoint3D32f pointBeforeCam;
-        pointBeforeCam.x = (p2d.x - mControlWidget->cx->value()) / mControlWidget->fx->value() * 50;
-        pointBeforeCam.y = (p2d.y - mControlWidget->cy->value()) / mControlWidget->fy->value() * 50;
-        pointBeforeCam.z = 50;
-        if(debug)
-        {
-            std::cout << "Point before Camera: [" << pointBeforeCam.x << ", " << pointBeforeCam.y << ", "
-                      << pointBeforeCam.z << "]" << std::endl;
-        }
-        // 3D-Punkt vor Kamera in Weltkoordinaten
-        cv::Point3f pBCInWorld = transformRT(pointBeforeCam);
-        if(debug)
-        {
-            std::cout << "Point before Camera in World-Coordinatesystem: [" << pBCInWorld.x << ", " << pBCInWorld.y
-                      << ", " << pBCInWorld.z << "]" << std::endl;
-        }
-        if(debug)
-        {
-            std::cout << "Camera in World-Coordinatesystem: [" << camInWorld.x << ", " << camInWorld.y << ", "
-                      << camInWorld.z << "]" << std::endl;
-        }
-        // Berechnung des Richtungsvektors der Gerade von der Kamera durch den Pixel
-        // Als Sttzvektor der Geraden wird die Position der Kamera gewhlt
-        pBCInWorld.x -= camInWorld.x;
-        pBCInWorld.y -= camInWorld.y;
-        pBCInWorld.z -= camInWorld.z;
-        if(debug)
-        {
-            std::cout << "G:x = (" << camInWorld.x << " / " << camInWorld.y << " / " << camInWorld.z << ") + lambda ("
-                      << pBCInWorld.x << " / " << pBCInWorld.y << " / " << pBCInWorld.z << ")" << std::endl;
-        }
+    resultPoint.x = rot_inv.at<double>(0, 0) * (tmpPoint.x) + rot_inv.at<double>(0, 1) * (tmpPoint.y) +
+                    rot_inv.at<double>(0, 2) * (tmpPoint.z);
+    resultPoint.y = rot_inv.at<double>(1, 0) * (tmpPoint.x) + rot_inv.at<double>(1, 1) * (tmpPoint.y) +
+                    rot_inv.at<double>(1, 2) * (tmpPoint.z);
+    resultPoint.z = rot_inv.at<double>(2, 0) * (tmpPoint.x) + rot_inv.at<double>(2, 1) * (tmpPoint.y) +
+                    rot_inv.at<double>(2, 2) * (tmpPoint.z);
 
-        // Berechnung des Schnittpunktes: Hier lambda von der Geraden
-        double lambda = (h - camInWorld.z) / (pBCInWorld.z);
-        if(debug)
-        {
-            std::cout << "Lambda: " << lambda << std::endl;
-        }
 
-        // Lambda in Gerade einsetzen
-        resultPoint.x = (mControlWidget->getCalibCoord3DSwapX() ? -1 : 1) * (camInWorld.x + lambda * pBCInWorld.x);
-        resultPoint.y = (mControlWidget->getCalibCoord3DSwapY() ? -1 : 1) * (camInWorld.y + lambda * pBCInWorld.y);
-        resultPoint.z = (mControlWidget->getCalibCoord3DSwapZ() ? -1 : 1) * (camInWorld.z + lambda * pBCInWorld.z);
+    // Coordinate Transformations
+    resultPoint.x -= mControlWidget->getCalibCoord3DTransX();
+    resultPoint.y -= mControlWidget->getCalibCoord3DTransY();
+    resultPoint.z -= mControlWidget->getCalibCoord3DTransZ();
 
-    } //////////////// End old method
+    resultPoint.x *= mControlWidget->getCalibCoord3DSwapX() ? -1 : 1;
+    resultPoint.y *= mControlWidget->getCalibCoord3DSwapY() ? -1 : 1;
+    resultPoint.z *= mControlWidget->getCalibCoord3DSwapZ() ? -1 : 1;
 
     return resultPoint;
 }
-- 
GitLab


From 9b926239c17d5e2302d2d39920fae54669a92aec Mon Sep 17 00:00:00 2001
From: "Kilic, Deniz" <d.kilic@fz-juelich.de>
Date: Mon, 28 Feb 2022 14:43:31 +0100
Subject: [PATCH 02/12] Write rotation vector directly, without c-array

---
 src/extrCalibration.cpp | 12 +++++++-----
 1 file changed, 7 insertions(+), 5 deletions(-)

diff --git a/src/extrCalibration.cpp b/src/extrCalibration.cpp
index 5d346c33e..b697a48ea 100644
--- a/src/extrCalibration.cpp
+++ b/src/extrCalibration.cpp
@@ -868,12 +868,14 @@ cv::Point3f ExtrCalibration::get3DPoint(cv::Point2f p2d, double h)
     // Ergebnis 3D-Punkt
     cv::Point3f resultPoint, tmpPoint;
 
-    double rvec_array[3], translation_vector[3];
-    rvec_array[0] = mControlWidget->getCalibExtrRot1();
-    rvec_array[1] = mControlWidget->getCalibExtrRot2();
-    rvec_array[2] = mControlWidget->getCalibExtrRot3();
+    double        translation_vector[3];
+    const cv::Mat rvec =
+        (cv::Mat_<double>(3, 1) << mControlWidget->getCalibExtrRot1(),
+         mControlWidget->getCalibExtrRot2(),
+         mControlWidget->getCalibExtrRot3());
 
-    cv::Mat rvec(3, 1, CV_64F, rvec_array), rot_inv;
+
+    cv::Mat rot_inv;
     cv::Mat rot_mat(3, 3, CV_64F), e(3, 3, CV_64F);
     // Transform the rotation vector into a rotation matrix with opencvs rodrigues method
     Rodrigues(rvec, rot_mat);
-- 
GitLab


From fc73e6c14ef0813afa65fe45f16024ca9a909759 Mon Sep 17 00:00:00 2001
From: "Kilic, Deniz" <d.kilic@fz-juelich.de>
Date: Mon, 28 Feb 2022 14:51:42 +0100
Subject: [PATCH 03/12] Tranlation vector as cv::Vec3d

---
 src/extrCalibration.cpp | 38 ++++++++++++++++----------------------
 1 file changed, 16 insertions(+), 22 deletions(-)

diff --git a/src/extrCalibration.cpp b/src/extrCalibration.cpp
index b697a48ea..f854b765e 100644
--- a/src/extrCalibration.cpp
+++ b/src/extrCalibration.cpp
@@ -868,34 +868,28 @@ cv::Point3f ExtrCalibration::get3DPoint(cv::Point2f p2d, double h)
     // Ergebnis 3D-Punkt
     cv::Point3f resultPoint, tmpPoint;
 
-    double        translation_vector[3];
+
+    // Transform the rotation vector into a rotation matrix with opencvs rodrigues method
+    cv::Mat       rot_inv;
+    cv::Mat       rot_mat(3, 3, CV_64F);
     const cv::Mat rvec =
         (cv::Mat_<double>(3, 1) << mControlWidget->getCalibExtrRot1(),
          mControlWidget->getCalibExtrRot2(),
          mControlWidget->getCalibExtrRot3());
-
-
-    cv::Mat rot_inv;
-    cv::Mat rot_mat(3, 3, CV_64F), e(3, 3, CV_64F);
-    // Transform the rotation vector into a rotation matrix with opencvs rodrigues method
     Rodrigues(rvec, rot_mat);
+    rot_inv = rot_mat.inv(cv::DECOMP_LU);
 
-    translation_vector[0] = rot_mat.at<double>(0, 0) * mControlWidget->getCalibExtrTrans1() +
-                            rot_mat.at<double>(0, 1) * mControlWidget->getCalibExtrTrans2() +
-                            rot_mat.at<double>(0, 2) * mControlWidget->getCalibExtrTrans3();
-    translation_vector[1] = rot_mat.at<double>(1, 0) * mControlWidget->getCalibExtrTrans1() +
-                            rot_mat.at<double>(1, 1) * mControlWidget->getCalibExtrTrans2() +
-                            rot_mat.at<double>(1, 2) * mControlWidget->getCalibExtrTrans3();
-    translation_vector[2] = rot_mat.at<double>(2, 0) * mControlWidget->getCalibExtrTrans1() +
-                            rot_mat.at<double>(2, 1) * mControlWidget->getCalibExtrTrans2() +
-                            rot_mat.at<double>(2, 2) * mControlWidget->getCalibExtrTrans3();
 
-    // use inverse Matrix
-    rot_inv = rot_mat.inv(cv::DECOMP_LU);
+    // Create and rotate translation vector
+    cv::Vec3d translation{
+        mControlWidget->getCalibExtrTrans1(),
+        mControlWidget->getCalibExtrTrans2(),
+        mControlWidget->getCalibExtrTrans3()};
+    translation = cv::Mat(rot_mat * translation); // translation is applied after rotation, so rotate translation vector
 
 
-    double z = h + rot_inv.at<double>(2, 0) * translation_vector[0] + rot_inv.at<double>(2, 1) * translation_vector[1] +
-               rot_inv.at<double>(2, 2) * translation_vector[2];
+    double z = h + rot_inv.at<double>(2, 0) * translation[0] + rot_inv.at<double>(2, 1) * translation[1] +
+               rot_inv.at<double>(2, 2) * translation[2];
 
     z /=
         (rot_inv.at<double>(2, 0) * (p2d.x - (mControlWidget->getCalibCxValue() - bS)) /
@@ -914,9 +908,9 @@ cv::Point3f ExtrCalibration::get3DPoint(cv::Point2f p2d, double h)
     resultPoint.y = resultPoint.y * z / mControlWidget->getCalibFyValue();
 
 
-    tmpPoint.x = resultPoint.x - translation_vector[0];
-    tmpPoint.y = resultPoint.y - translation_vector[1];
-    tmpPoint.z = resultPoint.z - translation_vector[2];
+    tmpPoint.x = resultPoint.x - translation[0];
+    tmpPoint.y = resultPoint.y - translation[1];
+    tmpPoint.z = resultPoint.z - translation[2];
 
     resultPoint.x = rot_inv.at<double>(0, 0) * (tmpPoint.x) + rot_inv.at<double>(0, 1) * (tmpPoint.y) +
                     rot_inv.at<double>(0, 2) * (tmpPoint.z);
-- 
GitLab


From 142c99240be7b6fd0fee9e2dab022869e62c1574 Mon Sep 17 00:00:00 2001
From: "Kilic, Deniz" <d.kilic@fz-juelich.de>
Date: Mon, 28 Feb 2022 14:59:32 +0100
Subject: [PATCH 04/12] Restructure calculation of z

- more comments on what is done, hopefully more understandable
---
 src/extrCalibration.cpp | 34 ++++++++++++++++++++++++----------
 1 file changed, 24 insertions(+), 10 deletions(-)

diff --git a/src/extrCalibration.cpp b/src/extrCalibration.cpp
index f854b765e..72a283cb2 100644
--- a/src/extrCalibration.cpp
+++ b/src/extrCalibration.cpp
@@ -887,16 +887,30 @@ cv::Point3f ExtrCalibration::get3DPoint(cv::Point2f p2d, double h)
         mControlWidget->getCalibExtrTrans3()};
     translation = cv::Mat(rot_mat * translation); // translation is applied after rotation, so rotate translation vector
 
-
-    double z = h + rot_inv.at<double>(2, 0) * translation[0] + rot_inv.at<double>(2, 1) * translation[1] +
-               rot_inv.at<double>(2, 2) * translation[2];
-
-    z /=
-        (rot_inv.at<double>(2, 0) * (p2d.x - (mControlWidget->getCalibCxValue() - bS)) /
-             mControlWidget->getCalibFxValue() +
-         rot_inv.at<double>(2, 1) * (p2d.y - (mControlWidget->getCalibCyValue() - bS)) /
-             mControlWidget->getCalibFyValue() +
-         rot_inv.at<double>(2, 2));
+    // Subtract principal point and border, so we can assume pinhole camera
+    const cv::Vec2d centeredImagePoint{
+        p2d.x - (mControlWidget->getCalibCxValue() - bS), p2d.y - (mControlWidget->getCalibCyValue() - bS)};
+
+    // Calculate reprojection of pixel to camera coords according to pinhole camera
+    // x_3 = 1, because we divide by z in our equation
+    // C = R * W + T <=> C/z = (R * W + T)/z
+    // C := Point in Camera Coords
+    // W := Point in World Coords
+    // R,T := Rotation and Translation of Camera
+    const cv::Vec2d focalLength{mControlWidget->getCalibFxValue(), mControlWidget->getCalibFyValue()};
+    const cv::Vec2d pinholeProjectionXY = cv::Mat(centeredImagePoint.div(focalLength));
+    const cv::Vec3d pinholeProjectionXY1{pinholeProjectionXY[0], pinholeProjectionXY[1], 1};
+    const cv::Vec3d rotatedProj = cv::Mat(rot_inv * pinholeProjectionXY1);
+
+    // Evaluating 3rd row of C/z = (R * W + T)/z, to get z
+    double z = (h + cv::Vec3d(cv::Mat(rot_inv * translation))[2]) / rotatedProj[2];
+
+    //    z /=
+    //        (rot_inv.at<double>(2, 0) * (p2d.x - (mControlWidget->getCalibCxValue() - bS)) /
+    //             mControlWidget->getCalibFxValue() +
+    //         rot_inv.at<double>(2, 1) * (p2d.y - (mControlWidget->getCalibCyValue() - bS)) /
+    //             mControlWidget->getCalibFyValue() +
+    //         rot_inv.at<double>(2, 2));
 
 
     resultPoint.x = (p2d.x - (mControlWidget->getCalibCxValue() - bS));
-- 
GitLab


From 11ff031eb1491f4005aba79c4269f05a97431fef Mon Sep 17 00:00:00 2001
From: "Kilic, Deniz" <d.kilic@fz-juelich.de>
Date: Mon, 28 Feb 2022 15:45:26 +0100
Subject: [PATCH 05/12] Rotate translation vector later

simplifies expression for calculation z
---
 src/extrCalibration.cpp | 18 +++++++-----------
 1 file changed, 7 insertions(+), 11 deletions(-)

diff --git a/src/extrCalibration.cpp b/src/extrCalibration.cpp
index 72a283cb2..7037a946c 100644
--- a/src/extrCalibration.cpp
+++ b/src/extrCalibration.cpp
@@ -880,12 +880,11 @@ cv::Point3f ExtrCalibration::get3DPoint(cv::Point2f p2d, double h)
     rot_inv = rot_mat.inv(cv::DECOMP_LU);
 
 
-    // Create and rotate translation vector
+    // Create translation vector
     cv::Vec3d translation{
         mControlWidget->getCalibExtrTrans1(),
         mControlWidget->getCalibExtrTrans2(),
         mControlWidget->getCalibExtrTrans3()};
-    translation = cv::Mat(rot_mat * translation); // translation is applied after rotation, so rotate translation vector
 
     // Subtract principal point and border, so we can assume pinhole camera
     const cv::Vec2d centeredImagePoint{
@@ -903,25 +902,22 @@ cv::Point3f ExtrCalibration::get3DPoint(cv::Point2f p2d, double h)
     const cv::Vec3d rotatedProj = cv::Mat(rot_inv * pinholeProjectionXY1);
 
     // Evaluating 3rd row of C/z = (R * W + T)/z, to get z
-    double z = (h + cv::Vec3d(cv::Mat(rot_inv * translation))[2]) / rotatedProj[2];
-
-    //    z /=
-    //        (rot_inv.at<double>(2, 0) * (p2d.x - (mControlWidget->getCalibCxValue() - bS)) /
-    //             mControlWidget->getCalibFxValue() +
-    //         rot_inv.at<double>(2, 1) * (p2d.y - (mControlWidget->getCalibCyValue() - bS)) /
-    //             mControlWidget->getCalibFyValue() +
-    //         rot_inv.at<double>(2, 2));
+    // c_3 = h/z + t_3/z
+    double z = (h + translation[2]) / rotatedProj[2];
 
 
     resultPoint.x = (p2d.x - (mControlWidget->getCalibCxValue() - bS));
     resultPoint.y = (p2d.y - (mControlWidget->getCalibCyValue() - bS));
     resultPoint.z = z;
 
-
     resultPoint.x = resultPoint.x * z / mControlWidget->getCalibFxValue();
     resultPoint.y = resultPoint.y * z / mControlWidget->getCalibFyValue();
 
+    // const cv::Vec3f pointCameraCoords + z * pinholeProjectionXY1;
+    // resultPoint = cv::Point3f(pointCameraCoords);
 
+    translation = cv::Mat(
+        rot_mat * translation); // translation in formula is applied after rotation, so rotate translation vector
     tmpPoint.x = resultPoint.x - translation[0];
     tmpPoint.y = resultPoint.y - translation[1];
     tmpPoint.z = resultPoint.z - translation[2];
-- 
GitLab


From 602695ff62faa18ad8f00bc9671286454b48d2f2 Mon Sep 17 00:00:00 2001
From: "Kilic, Deniz" <d.kilic@fz-juelich.de>
Date: Mon, 28 Feb 2022 15:56:10 +0100
Subject: [PATCH 06/12] Change to Matx instead of Mat for rot

---
 src/extrCalibration.cpp | 34 ++++++++++++++--------------------
 1 file changed, 14 insertions(+), 20 deletions(-)

diff --git a/src/extrCalibration.cpp b/src/extrCalibration.cpp
index 7037a946c..3dec97816 100644
--- a/src/extrCalibration.cpp
+++ b/src/extrCalibration.cpp
@@ -868,17 +868,15 @@ cv::Point3f ExtrCalibration::get3DPoint(cv::Point2f p2d, double h)
     // Ergebnis 3D-Punkt
     cv::Point3f resultPoint, tmpPoint;
 
-
     // Transform the rotation vector into a rotation matrix with opencvs rodrigues method
-    cv::Mat       rot_inv;
-    cv::Mat       rot_mat(3, 3, CV_64F);
-    const cv::Mat rvec =
+    cv::Matx<double, 3, 3> rot_inv;
+    cv::Matx<double, 3, 3> rot_mat(3, 3, CV_64F);
+    const cv::Mat          rvec =
         (cv::Mat_<double>(3, 1) << mControlWidget->getCalibExtrRot1(),
          mControlWidget->getCalibExtrRot2(),
          mControlWidget->getCalibExtrRot3());
     Rodrigues(rvec, rot_mat);
-    rot_inv = rot_mat.inv(cv::DECOMP_LU);
-
+    rot_inv = rot_mat.inv(cv::DECOMP_LU, nullptr);
 
     // Create translation vector
     cv::Vec3d translation{
@@ -899,13 +897,12 @@ cv::Point3f ExtrCalibration::get3DPoint(cv::Point2f p2d, double h)
     const cv::Vec2d focalLength{mControlWidget->getCalibFxValue(), mControlWidget->getCalibFyValue()};
     const cv::Vec2d pinholeProjectionXY = cv::Mat(centeredImagePoint.div(focalLength));
     const cv::Vec3d pinholeProjectionXY1{pinholeProjectionXY[0], pinholeProjectionXY[1], 1};
-    const cv::Vec3d rotatedProj = cv::Mat(rot_inv * pinholeProjectionXY1);
+    const cv::Vec3d rotatedProj = rot_inv * pinholeProjectionXY1;
 
     // Evaluating 3rd row of C/z = (R * W + T)/z, to get z
     // c_3 = h/z + t_3/z
     double z = (h + translation[2]) / rotatedProj[2];
 
-
     resultPoint.x = (p2d.x - (mControlWidget->getCalibCxValue() - bS));
     resultPoint.y = (p2d.y - (mControlWidget->getCalibCyValue() - bS));
     resultPoint.z = z;
@@ -916,18 +913,15 @@ cv::Point3f ExtrCalibration::get3DPoint(cv::Point2f p2d, double h)
     // const cv::Vec3f pointCameraCoords + z * pinholeProjectionXY1;
     // resultPoint = cv::Point3f(pointCameraCoords);
 
-    translation = cv::Mat(
-        rot_mat * translation); // translation in formula is applied after rotation, so rotate translation vector
-    tmpPoint.x = resultPoint.x - translation[0];
-    tmpPoint.y = resultPoint.y - translation[1];
-    tmpPoint.z = resultPoint.z - translation[2];
-
-    resultPoint.x = rot_inv.at<double>(0, 0) * (tmpPoint.x) + rot_inv.at<double>(0, 1) * (tmpPoint.y) +
-                    rot_inv.at<double>(0, 2) * (tmpPoint.z);
-    resultPoint.y = rot_inv.at<double>(1, 0) * (tmpPoint.x) + rot_inv.at<double>(1, 1) * (tmpPoint.y) +
-                    rot_inv.at<double>(1, 2) * (tmpPoint.z);
-    resultPoint.z = rot_inv.at<double>(2, 0) * (tmpPoint.x) + rot_inv.at<double>(2, 1) * (tmpPoint.y) +
-                    rot_inv.at<double>(2, 2) * (tmpPoint.z);
+    // translation in formula is applied after rotation, so rotate translation vector
+    translation = rot_mat * translation;
+    tmpPoint.x  = resultPoint.x - translation[0];
+    tmpPoint.y  = resultPoint.y - translation[1];
+    tmpPoint.z  = resultPoint.z - translation[2];
+
+    resultPoint.x = rot_inv(0, 0) * (tmpPoint.x) + rot_inv(0, 1) * (tmpPoint.y) + rot_inv(0, 2) * (tmpPoint.z);
+    resultPoint.y = rot_inv(1, 0) * (tmpPoint.x) + rot_inv(1, 1) * (tmpPoint.y) + rot_inv(1, 2) * (tmpPoint.z);
+    resultPoint.z = rot_inv(2, 0) * (tmpPoint.x) + rot_inv(2, 1) * (tmpPoint.y) + rot_inv(2, 2) * (tmpPoint.z);
 
 
     // Coordinate Transformations
-- 
GitLab


From f196f1cee2f33cafd6a207c406d37ef76c7bb980 Mon Sep 17 00:00:00 2001
From: "Kilic, Deniz" <d.kilic@fz-juelich.de>
Date: Mon, 28 Feb 2022 16:07:41 +0100
Subject: [PATCH 07/12] Clarified comment

---
 src/extrCalibration.cpp | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/src/extrCalibration.cpp b/src/extrCalibration.cpp
index 3dec97816..cd10f4144 100644
--- a/src/extrCalibration.cpp
+++ b/src/extrCalibration.cpp
@@ -913,7 +913,8 @@ cv::Point3f ExtrCalibration::get3DPoint(cv::Point2f p2d, double h)
     // const cv::Vec3f pointCameraCoords + z * pinholeProjectionXY1;
     // resultPoint = cv::Point3f(pointCameraCoords);
 
-    // translation in formula is applied after rotation, so rotate translation vector
+    // T is from C = R * W + T, so in cam coords
+    // Need translation from cam to world, so rotate and substract instead of add
     translation = rot_mat * translation;
     tmpPoint.x  = resultPoint.x - translation[0];
     tmpPoint.y  = resultPoint.y - translation[1];
-- 
GitLab


From 0f74e0c05e788884e36cc8a05b40067b26ece9cf Mon Sep 17 00:00:00 2001
From: "Kilic, Deniz" <d.kilic@fz-juelich.de>
Date: Mon, 28 Feb 2022 16:10:34 +0100
Subject: [PATCH 08/12] More comments

---
 src/extrCalibration.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/extrCalibration.cpp b/src/extrCalibration.cpp
index cd10f4144..edd770711 100644
--- a/src/extrCalibration.cpp
+++ b/src/extrCalibration.cpp
@@ -914,7 +914,7 @@ cv::Point3f ExtrCalibration::get3DPoint(cv::Point2f p2d, double h)
     // resultPoint = cv::Point3f(pointCameraCoords);
 
     // T is from C = R * W + T, so in cam coords
-    // Need translation from cam to world, so rotate and substract instead of add
+    // So we now calc: R^-1 * (C - T); T needs to be given in World Coords, so we rotate
     translation = rot_mat * translation;
     tmpPoint.x  = resultPoint.x - translation[0];
     tmpPoint.y  = resultPoint.y - translation[1];
-- 
GitLab


From 1b497f8053c1b1a64ff869f970a9f59915cbbdcf Mon Sep 17 00:00:00 2001
From: "Kilic, Deniz" <d.kilic@fz-juelich.de>
Date: Mon, 28 Feb 2022 16:41:10 +0100
Subject: [PATCH 09/12] Take const parameter get3DPoint

---
 include/extrCalibration.h | 2 +-
 src/extrCalibration.cpp   | 5 +----
 2 files changed, 2 insertions(+), 5 deletions(-)

diff --git a/include/extrCalibration.h b/include/extrCalibration.h
index 495689185..7710f796c 100644
--- a/include/extrCalibration.h
+++ b/include/extrCalibration.h
@@ -179,7 +179,7 @@ public:
     void                            calibExtrParams();
     bool                            calcReprojectionError();
     virtual cv::Point2f             getImagePoint(cv::Point3f p3d);
-    cv::Point3f                     get3DPoint(cv::Point2f p2d, double h);
+    cv::Point3f                     get3DPoint(const cv::Point2f &p2d, double h);
     cv::Point3f                     transformRT(cv::Point3f p);
     bool                            isOutsideImage(cv::Point2f p2d);
     inline bool                     isOutsideImage(cv::Point3f p3d) { return isOutsideImage(getImagePoint(p3d)); }
diff --git a/src/extrCalibration.cpp b/src/extrCalibration.cpp
index edd770711..2060c80b4 100644
--- a/src/extrCalibration.cpp
+++ b/src/extrCalibration.cpp
@@ -861,7 +861,7 @@ cv::Point2f ExtrCalibration::getImagePoint(cv::Point3f p3d)
  * @param h height i.e. distance to xy-plane in cm
  * @return calculated 3D point in cm
  */
-cv::Point3f ExtrCalibration::get3DPoint(cv::Point2f p2d, double h)
+cv::Point3f ExtrCalibration::get3DPoint(const cv::Point2f &p2d, double h)
 {
     int bS = mMainWindow->getImage() ? mMainWindow->getImageBorderSize() : 0;
 
@@ -910,9 +910,6 @@ cv::Point3f ExtrCalibration::get3DPoint(cv::Point2f p2d, double h)
     resultPoint.x = resultPoint.x * z / mControlWidget->getCalibFxValue();
     resultPoint.y = resultPoint.y * z / mControlWidget->getCalibFyValue();
 
-    // const cv::Vec3f pointCameraCoords + z * pinholeProjectionXY1;
-    // resultPoint = cv::Point3f(pointCameraCoords);
-
     // T is from C = R * W + T, so in cam coords
     // So we now calc: R^-1 * (C - T); T needs to be given in World Coords, so we rotate
     translation = rot_mat * translation;
-- 
GitLab


From 51c5d8e01de5651ae3a25f1a7260c26b2dcf9074 Mon Sep 17 00:00:00 2001
From: Deniz Kilic <d.kilic@fz-juelich.de>
Date: Wed, 16 Mar 2022 11:48:46 +0100
Subject: [PATCH 10/12] Change comments to argue about lines in pinhole model

---
 src/extrCalibration.cpp | 46 +++++++++++++++++++++++++++++++----------
 1 file changed, 35 insertions(+), 11 deletions(-)

diff --git a/src/extrCalibration.cpp b/src/extrCalibration.cpp
index 2060c80b4..0303078dc 100644
--- a/src/extrCalibration.cpp
+++ b/src/extrCalibration.cpp
@@ -865,7 +865,6 @@ cv::Point3f ExtrCalibration::get3DPoint(const cv::Point2f &p2d, double h)
 {
     int bS = mMainWindow->getImage() ? mMainWindow->getImageBorderSize() : 0;
 
-    // Ergebnis 3D-Punkt
     cv::Point3f resultPoint, tmpPoint;
 
     // Transform the rotation vector into a rotation matrix with opencvs rodrigues method
@@ -888,21 +887,46 @@ cv::Point3f ExtrCalibration::get3DPoint(const cv::Point2f &p2d, double h)
     const cv::Vec2d centeredImagePoint{
         p2d.x - (mControlWidget->getCalibCxValue() - bS), p2d.y - (mControlWidget->getCalibCyValue() - bS)};
 
-    // Calculate reprojection of pixel to camera coords according to pinhole camera
-    // x_3 = 1, because we divide by z in our equation
-    // C = R * W + T <=> C/z = (R * W + T)/z
-    // C := Point in Camera Coords
-    // W := Point in World Coords
-    // R,T := Rotation and Translation of Camera
+
+    /* Basic Idea:
+     * All points projecting onto a point on the image plane lie on the same
+     * line (cf. pinhole camera model). We can determine this line in the form:
+     *
+     * g: x = lambda * v
+     *
+     * This line exists in camera coordinates. Let v be the projection with
+     * depth 1 (i.e. v_3 = 1). Then lambda is the depth of the resulting point.
+     * We'll continue to call lambda z instead, to show this.
+     * We now want to determine the depth at which the resulting point has height h
+     * in world coordinates. The transformation from cam to world is:
+     *
+     * W = R * C - T
+     * W   := Point in World Coords
+     * C   := Point in Cam Coords
+     * R,T := Rotation and Translation of Cam
+     *
+     * By putting in our x = z * v, we get:
+     *     W          = R * (z * v) - T
+     * <=> W          = z * Rv - T
+     * <=> W + T      = z * Rv
+     * <=> (W + T)/Rv = z
+     * We select the third row of this to solve for z. Finally g(z) is transformed
+     * into World Coords.
+     */
+
+    // calc Rv, (R = rot_inv)
+    // rotatedProj = Rv
     const cv::Vec2d focalLength{mControlWidget->getCalibFxValue(), mControlWidget->getCalibFyValue()};
     const cv::Vec2d pinholeProjectionXY = cv::Mat(centeredImagePoint.div(focalLength));
     const cv::Vec3d pinholeProjectionXY1{pinholeProjectionXY[0], pinholeProjectionXY[1], 1};
     const cv::Vec3d rotatedProj = rot_inv * pinholeProjectionXY1;
 
-    // Evaluating 3rd row of C/z = (R * W + T)/z, to get z
-    // c_3 = h/z + t_3/z
+    // determine z via formula from comment above; using 3rd row
     double z = (h + translation[2]) / rotatedProj[2];
 
+    // Evaluate line at depth z; calc point in camera coords
+    // written this way instead of z * pinholeProjectionXY1 (i.e. z * v) to not change test results due to floating
+    // point precision diff
     resultPoint.x = (p2d.x - (mControlWidget->getCalibCxValue() - bS));
     resultPoint.y = (p2d.y - (mControlWidget->getCalibCyValue() - bS));
     resultPoint.z = z;
@@ -910,8 +934,8 @@ cv::Point3f ExtrCalibration::get3DPoint(const cv::Point2f &p2d, double h)
     resultPoint.x = resultPoint.x * z / mControlWidget->getCalibFxValue();
     resultPoint.y = resultPoint.y * z / mControlWidget->getCalibFyValue();
 
-    // T is from C = R * W + T, so in cam coords
-    // So we now calc: R^-1 * (C - T); T needs to be given in World Coords, so we rotate
+    // We transform from cam coords to world coords with W = R * C - T
+    // we now calc: W = R * (C - R^-1*T), which is equivalent
     translation = rot_mat * translation;
     tmpPoint.x  = resultPoint.x - translation[0];
     tmpPoint.y  = resultPoint.y - translation[1];
-- 
GitLab


From 1b949f45d99cea7f3f3b58984d2f21cf6c0886ea Mon Sep 17 00:00:00 2001
From: Deniz Kilic <d.kilic@fz-juelich.de>
Date: Wed, 16 Mar 2022 11:54:51 +0100
Subject: [PATCH 11/12] Delete now unused function transformRT

---
 src/extrCalibration.cpp | 37 -------------------------------------
 1 file changed, 37 deletions(-)

diff --git a/src/extrCalibration.cpp b/src/extrCalibration.cpp
index 0303078dc..f91131760 100644
--- a/src/extrCalibration.cpp
+++ b/src/extrCalibration.cpp
@@ -957,44 +957,7 @@ cv::Point3f ExtrCalibration::get3DPoint(const cv::Point2f &p2d, double h)
 
     return resultPoint;
 }
-/**
- * Transformiert den angegebenen 3D-Punkt mit der Rotation und Translation
- * um Umrechnungen zwischen verschiedenen Koordinatensystemen zu ermglichen
- */
-cv::Point3f ExtrCalibration::transformRT(cv::Point3f p)
-{
-    // ToDo: use projectPoints();
-
-    double rvec_array[3], rotation_matrix[9];
-    rvec_array[0] = mControlWidget->getCalibExtrRot1();
-    rvec_array[1] = mControlWidget->getCalibExtrRot2();
-    rvec_array[2] = mControlWidget->getCalibExtrRot3();
 
-    cv::Mat rvec(3, 1, CV_64F, rvec_array);
-    cv::Mat rot_mat(3, 3, CV_64F);
-    // Transform the rotation vector into a rotation matrix with opencvs rodrigues method
-    Rodrigues(rvec, rot_mat);
-
-    rotation_matrix[0] = rot_mat.at<double>(0, 0);
-    rotation_matrix[1] = rot_mat.at<double>(0, 1);
-    rotation_matrix[2] = rot_mat.at<double>(0, 2);
-    rotation_matrix[3] = rot_mat.at<double>(1, 0);
-    rotation_matrix[4] = rot_mat.at<double>(1, 1);
-    rotation_matrix[5] = rot_mat.at<double>(1, 2);
-    rotation_matrix[6] = rot_mat.at<double>(2, 0);
-    rotation_matrix[7] = rot_mat.at<double>(2, 1);
-    rotation_matrix[8] = rot_mat.at<double>(2, 2);
-
-    cv::Point3f point3D;
-
-    point3D.x = rotation_matrix[0] * p.x + rotation_matrix[3] * p.y + rotation_matrix[6] * p.z -
-                mControlWidget->trans1->value();
-    point3D.y = rotation_matrix[1] * p.x + rotation_matrix[4] * p.y + rotation_matrix[7] * p.z -
-                mControlWidget->trans2->value();
-    point3D.z = rotation_matrix[2] * p.x + rotation_matrix[5] * p.y + rotation_matrix[8] * p.z -
-                mControlWidget->trans3->value();
-    return point3D;
-}
 bool ExtrCalibration::isOutsideImage(cv::Point2f p2d)
 {
     int bS = mMainWindow->getImage() ? mMainWindow->getImageBorderSize() : 0;
-- 
GitLab


From 7aefd9c697647991808c491c24129c82d46af587 Mon Sep 17 00:00:00 2001
From: Deniz Kilic <d.kilic@fz-juelich.de>
Date: Wed, 23 Mar 2022 15:13:36 +0100
Subject: [PATCH 12/12] Change get3DPoint to const method

---
 include/extrCalibration.h | 2 +-
 src/extrCalibration.cpp   | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/include/extrCalibration.h b/include/extrCalibration.h
index 7710f796c..86183ad10 100644
--- a/include/extrCalibration.h
+++ b/include/extrCalibration.h
@@ -179,7 +179,7 @@ public:
     void                            calibExtrParams();
     bool                            calcReprojectionError();
     virtual cv::Point2f             getImagePoint(cv::Point3f p3d);
-    cv::Point3f                     get3DPoint(const cv::Point2f &p2d, double h);
+    cv::Point3f                     get3DPoint(const cv::Point2f &p2d, double h) const;
     cv::Point3f                     transformRT(cv::Point3f p);
     bool                            isOutsideImage(cv::Point2f p2d);
     inline bool                     isOutsideImage(cv::Point3f p3d) { return isOutsideImage(getImagePoint(p3d)); }
diff --git a/src/extrCalibration.cpp b/src/extrCalibration.cpp
index f91131760..109e930e1 100644
--- a/src/extrCalibration.cpp
+++ b/src/extrCalibration.cpp
@@ -861,7 +861,7 @@ cv::Point2f ExtrCalibration::getImagePoint(cv::Point3f p3d)
  * @param h height i.e. distance to xy-plane in cm
  * @return calculated 3D point in cm
  */
-cv::Point3f ExtrCalibration::get3DPoint(const cv::Point2f &p2d, double h)
+cv::Point3f ExtrCalibration::get3DPoint(const cv::Point2f &p2d, double h) const
 {
     int bS = mMainWindow->getImage() ? mMainWindow->getImageBorderSize() : 0;
 
-- 
GitLab