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