Resolve "Add method for vector rotation for converting to world coordinate system"

include/extrCalibration.h

@@ -181,6 +181,7 @@ public:
     virtual cv::Point2f             getImagePoint(cv::Point3f p3d);
     cv::Point3f                     get3DPoint(const cv::Point2f &p2d, double h) const;
     cv::Point3f                     transformRT(cv::Point3f p);
+    cv::Vec3d                       camToWorldRotation(const cv::Vec3d &vec) const;
     bool                            isOutsideImage(cv::Point2f p2d);
     inline bool                     isOutsideImage(cv::Point3f p3d) { return isOutsideImage(getImagePoint(p3d)); }
     inline std::vector<cv::Point3f> get3DList() { return points3D; }

src/extrCalibration.cpp

@@ -895,6 +895,27 @@ cv::Point2f ExtrCalibration::getImagePoint(cv::Point3f p3d)
     return point2D;
 }
 
+/**
+ * @brief Rotate a given vector from camera coordinate system to world coordinate system
+ *
+ * When the world coordinate system is not aligned to the camera-system,
+ * some direction dependent calculations (like head orientation) have to be rotated to be correctly exported.
+ *
+ * @param camVec the Vector to be rotated matching the camera coordinate system.
+ * @return the rotated vector.
+ */
+cv::Vec3d ExtrCalibration::camToWorldRotation(const cv::Vec3d &camVec) const
+{
+    // Transform the rotation vector into a rotation matrix with opencvs rodrigues method
+    cv::Matx<double, 3, 3> rotMat(3, 3, CV_64F);
+    const auto             rvec = cv::Vec3d(
+        mControlWidget->getCalibExtrRot1(), mControlWidget->getCalibExtrRot2(), mControlWidget->getCalibExtrRot3());
+    Rodrigues(rvec, rotMat);
+
+    auto      rotInv   = rotMat.inv(cv::DECOMP_LU);
+    cv::Vec3d worldVec = rotInv * camVec;
+    return worldVec;
+}
 
 /**
  * @brief Tranforms a 2D point into a 3D point with given height.

tests/unit_test/CMakeLists.txt

@@ -11,4 +11,5 @@ target_sources(petrack_tests PRIVATE
     tst_codeMarkerWidget.cpp
     tst_helper.cpp
     tst_petrack.cpp
+    tst_extrCalibration.cpp
 )

tests/unit_test/tst_extrCalibration.cpp

+/*
+ * PeTrack - Software for tracking pedestrians movement in videos
+ * Copyright (C) 2010-2022 Forschungszentrum Jülich GmbH,
+ * Maik Boltes, Juliane Adrian, Ricardo Martin Brualla, Arne Graf, Paul Häger, Daniel Hillebrand,
+ * Deniz Kilic, Paul Lieberenz, Daniel Salden, Tobias Schrödter, Ann Katrin Seemann
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+#include "control.h"
+#include "extrCalibration.h"
+#include "petrack.h"
+
+#include <catch2/catch.hpp>
+
+
+// use margin for absolute difference, as epsilon would be relative which is useless when comparing to 0
+constexpr float VEC_MARGIN = 0.01;
+
+TEST_CASE("src/extrCalibration/camToWorldRotation", "[extrCalibration]")
+{
+    Petrack  petrack{};
+    auto     calib   = petrack.getExtrCalibration();
+    Control *control = petrack.getControlWidget();
+
+    control->setCalibExtrRot1(0);
+    control->setCalibExtrRot2(0);
+    control->setCalibExtrRot3(0);
+
+    SECTION("Identity Coordinate System")
+    {
+        REQUIRE(
+            cv::norm(calib->camToWorldRotation(cv::Vec3d(1, 0, 0)) - cv::Vec3d(1, 0, 0)) ==
+            Approx(0).margin(VEC_MARGIN));
+
+        REQUIRE(
+            cv::norm(calib->camToWorldRotation(cv::Vec3d(1, 2, 3)) - cv::Vec3d(1, 2, 3)) ==
+            Approx(0).margin(VEC_MARGIN));
+    }
+
+    SECTION("Rotated around z-axis")
+    {
+        // rotate 90 degrees
+        control->setCalibExtrRot3(PI / 2);
+        REQUIRE(
+            cv::norm(calib->camToWorldRotation(cv::Vec3d(1, 0, 0)) - cv::Vec3d(0, -1, 0)) ==
+            Approx(0).margin(VEC_MARGIN));
+
+        REQUIRE(
+            cv::norm(calib->camToWorldRotation(cv::Vec3d(0, 1, 0)) - cv::Vec3d(1, 0, 0)) ==
+            Approx(0).margin(VEC_MARGIN));
+
+        REQUIRE(
+            cv::norm(calib->camToWorldRotation(cv::Vec3d(1, 2, 3)) - cv::Vec3d(2, -1, 3)) ==
+            Approx(0).margin(VEC_MARGIN));
+
+        REQUIRE(
+            cv::norm(calib->camToWorldRotation(cv::Vec3d(-5, 5, -2)) - cv::Vec3d(5, 5, -2)) ==
+            Approx(0).margin(VEC_MARGIN));
+
+        // negative rotation
+        control->setCalibExtrRot3(-PI);
+        REQUIRE(
+            cv::norm(calib->camToWorldRotation(cv::Vec3d(1, 0, 0)) - cv::Vec3d(-1, 0, 0)) ==
+            Approx(0).margin(VEC_MARGIN));
+
+        REQUIRE(
+            cv::norm(calib->camToWorldRotation(cv::Vec3d(1, 2, -3)) - cv::Vec3d(-1, -2, -3)) ==
+            Approx(0).margin(VEC_MARGIN));
+    }
+
+    SECTION("Wild rotation")
+    {
+        // vector (1, 1, 1) with length pi/2
+        control->setCalibExtrRot1(0.9067);
+        control->setCalibExtrRot2(0.9067);
+        control->setCalibExtrRot3(0.9067);
+
+        REQUIRE(
+            cv::norm(calib->camToWorldRotation(cv::Vec3d(1, 1, 1)) - cv::Vec3d(1, 1, 1)) ==
+            Approx(0).margin(VEC_MARGIN));
+
+        REQUIRE(
+            cv::norm(calib->camToWorldRotation(cv::Vec3d(1, 0, 0)) - cv::Vec3d(0.33, -0.24, 0.91)) ==
+            Approx(0).margin(VEC_MARGIN));
+
+        REQUIRE(
+            cv::norm(calib->camToWorldRotation(cv::Vec3d(1, 2, 3)) - cv::Vec3d(1.42, 3.15, 1.42)) ==
+            Approx(0).margin(VEC_MARGIN));
+
+        SECTION("Translation should not matter")
+        {
+            control->setCalibExtrTrans1(10);
+            control->setCalibExtrTrans2(-20);
+            control->setCalibExtrTrans3(-500);
+            REQUIRE(
+                cv::norm(calib->camToWorldRotation(cv::Vec3d(1, 0, 0)) - cv::Vec3d(0.33, -0.24, 0.91)) ==
+                Approx(0).margin(VEC_MARGIN));
+
+            REQUIRE(
+                cv::norm(calib->camToWorldRotation(cv::Vec3d(1, 2, 3)) - cv::Vec3d(1.42, 3.15, 1.42)) ==
+                Approx(0).margin(VEC_MARGIN));
+        }
+    }
+
+    SECTION("Another Wild Rotation")
+    {
+        control->setCalibExtrRot1(0.5);
+        control->setCalibExtrRot2(-2);
+        control->setCalibExtrRot3(1.1);
+
+        REQUIRE(
+            cv::norm(calib->camToWorldRotation(cv::Vec3d(1, 1, 1)) - cv::Vec3d(0.2, -0.63, -1.6)) ==
+            Approx(0).margin(VEC_MARGIN));
+
+        REQUIRE(
+            cv::norm(calib->camToWorldRotation(cv::Vec3d(1, -2, 3)) - cv::Vec3d(1.69, -3.33, 0.27)) ==
+            Approx(0).margin(VEC_MARGIN));
+    }
+}