From 6494e3abb54ee9701df6ff27cfb0a7fa1409dded Mon Sep 17 00:00:00 2001
From: "Arens, Tobias" <t.arens@fz-juelich.de>
Date: Mon, 12 Sep 2022 09:51:04 +0200
Subject: [PATCH] Resolve "Add method for vector rotation for converting to
world coordinate system"
---
include/extrCalibration.h | 1 +
src/extrCalibration.cpp | 21 ++++
tests/unit_test/CMakeLists.txt | 1 +
tests/unit_test/tst_extrCalibration.cpp | 130 ++++++++++++++++++++++++
4 files changed, 153 insertions(+)
create mode 100644 tests/unit_test/tst_extrCalibration.cpp
diff --git a/include/extrCalibration.h b/include/extrCalibration.h
index 86183ad10..c2c29cba3 100644
--- a/include/extrCalibration.h
+++ b/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; }
diff --git a/src/extrCalibration.cpp b/src/extrCalibration.cpp
index f256bcad4..5e96b3dc1 100644
--- a/src/extrCalibration.cpp
+++ b/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.
diff --git a/tests/unit_test/CMakeLists.txt b/tests/unit_test/CMakeLists.txt
index 30e66a31d..e814ff93b 100644
--- a/tests/unit_test/CMakeLists.txt
+++ b/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
)
diff --git a/tests/unit_test/tst_extrCalibration.cpp b/tests/unit_test/tst_extrCalibration.cpp
new file mode 100644
index 000000000..16f6c298c
--- /dev/null
+++ b/tests/unit_test/tst_extrCalibration.cpp
@@ -0,0 +1,130 @@
+/*
+ * 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));
+ }
+}
--
GitLab