From a2ba3e9ea10b0d87aeb14d5cb3897179bc65d90d Mon Sep 17 00:00:00 2001 From: Maksim Shabunin Date: Tue, 16 Apr 2024 20:47:46 +0300 Subject: [PATCH 1/2] xfeatures2d: reduce test avg memory consumption --- .../test_rotation_and_scale_invariance.cpp | 126 +++++++++--------- 1 file changed, 63 insertions(+), 63 deletions(-) diff --git a/modules/xfeatures2d/test/test_rotation_and_scale_invariance.cpp b/modules/xfeatures2d/test/test_rotation_and_scale_invariance.cpp index a3dd1a98845..ce7281d6c06 100644 --- a/modules/xfeatures2d/test/test_rotation_and_scale_invariance.cpp +++ b/modules/xfeatures2d/test/test_rotation_and_scale_invariance.cpp @@ -17,56 +17,56 @@ static const char* const IMAGE_BIKES = "detectors_descriptors_evaluation/images_ #ifdef OPENCV_ENABLE_NONFREE INSTANTIATE_TEST_CASE_P(SURF, DetectorRotationInvariance, Values( - make_tuple(IMAGE_TSUKUBA, SURF::create(), 0.40f, 0.76f) + make_tuple(IMAGE_TSUKUBA, []() { return SURF::create(); }, 0.40f, 0.76f) )); INSTANTIATE_TEST_CASE_P(SURF, DescriptorRotationInvariance, Values( - make_tuple(IMAGE_TSUKUBA, SURF::create(), SURF::create(), 0.83f) + make_tuple(IMAGE_TSUKUBA, []() { return SURF::create(); }, []() { return SURF::create(); }, 0.83f) )); #endif // NONFREE INSTANTIATE_TEST_CASE_P(LATCH, DescriptorRotationInvariance, Values( - make_tuple(IMAGE_TSUKUBA, SIFT::create(), LATCH::create(), 0.98f) + make_tuple(IMAGE_TSUKUBA, []() { return SIFT::create(); }, []() { return LATCH::create(); }, 0.98f) )); INSTANTIATE_TEST_CASE_P(BEBLID, DescriptorRotationInvariance, Values( - make_tuple(IMAGE_TSUKUBA, SIFT::create(), BEBLID::create(6.75), 0.98f) + make_tuple(IMAGE_TSUKUBA, []() { return SIFT::create(); }, []() { return BEBLID::create(6.75); }, 0.98f) )); INSTANTIATE_TEST_CASE_P(TEBLID, DescriptorRotationInvariance, Values( - make_tuple(IMAGE_TSUKUBA, SIFT::create(), TEBLID::create(6.75), 0.98f) + make_tuple(IMAGE_TSUKUBA, []() { return SIFT::create(); }, []() { return TEBLID::create(6.75); }, 0.98f) )); INSTANTIATE_TEST_CASE_P(DAISY, DescriptorRotationInvariance, Values( make_tuple(IMAGE_TSUKUBA, - BRISK::create(), - DAISY::create(15, 3, 8, 8, DAISY::NRM_NONE, noArray(), true, true), + []() { return BRISK::create(); }, + []() { return DAISY::create(15, 3, 8, 8, DAISY::NRM_NONE, noArray(), true, true); }, 0.79f) )); #ifdef OPENCV_XFEATURES2D_HAS_VGG_DATA INSTANTIATE_TEST_CASE_P(VGG120, DescriptorRotationInvariance, Values( make_tuple(IMAGE_TSUKUBA, - KAZE::create(), - VGG::create(VGG::VGG_120, 1.4f, true, true, 48.0f, false), + []() { return KAZE::create(); }, + []() { return VGG::create(VGG::VGG_120, 1.4f, true, true, 48.0f, false); }, 0.97f) )); INSTANTIATE_TEST_CASE_P(VGG80, DescriptorRotationInvariance, Values( make_tuple(IMAGE_TSUKUBA, - KAZE::create(), - VGG::create(VGG::VGG_80, 1.4f, true, true, 48.0f, false), + []() { return KAZE::create(); }, + []() { return VGG::create(VGG::VGG_80, 1.4f, true, true, 48.0f, false); }, 0.97f) )); INSTANTIATE_TEST_CASE_P(VGG64, DescriptorRotationInvariance, Values( make_tuple(IMAGE_TSUKUBA, - KAZE::create(), - VGG::create(VGG::VGG_64, 1.4f, true, true, 48.0f, false), + []() { return KAZE::create(); }, + []() { return VGG::create(VGG::VGG_64, 1.4f, true, true, 48.0f, false); }, 0.97f) )); INSTANTIATE_TEST_CASE_P(VGG48, DescriptorRotationInvariance, Values( make_tuple(IMAGE_TSUKUBA, - KAZE::create(), - VGG::create(VGG::VGG_48, 1.4f, true, true, 48.0f, false), + []() { return KAZE::create(); }, + []() { return VGG::create(VGG::VGG_48, 1.4f, true, true, 48.0f, false); }, 0.97f) )); #endif // OPENCV_XFEATURES2D_HAS_VGG_DATA @@ -75,79 +75,79 @@ INSTANTIATE_TEST_CASE_P(VGG48, DescriptorRotationInvariance, Values( INSTANTIATE_TEST_CASE_P(BRIEF_64, DescriptorRotationInvariance, Values( make_tuple(IMAGE_TSUKUBA, - SURF::create(), - BriefDescriptorExtractor::create(64,true), + []() { return SURF::create(); }, + []() { return BriefDescriptorExtractor::create(64,true); }, 0.98f) )); INSTANTIATE_TEST_CASE_P(BRIEF_32, DescriptorRotationInvariance, Values( make_tuple(IMAGE_TSUKUBA, - SURF::create(), - BriefDescriptorExtractor::create(32,true), + []() { return SURF::create(); }, + []() { return BriefDescriptorExtractor::create(32,true); }, 0.97f) )); INSTANTIATE_TEST_CASE_P(BRIEF_16, DescriptorRotationInvariance, Values( make_tuple(IMAGE_TSUKUBA, - SURF::create(), - BriefDescriptorExtractor::create(16, true), + []() { return SURF::create(); }, + []() { return BriefDescriptorExtractor::create(16, true); }, 0.98f) )); INSTANTIATE_TEST_CASE_P(FREAK, DescriptorRotationInvariance, Values( make_tuple(IMAGE_TSUKUBA, - SURF::create(), - FREAK::create(), + []() { return SURF::create(); }, + []() { return FREAK::create(); }, 0.90f) )); #ifdef OPENCV_XFEATURES2D_HAS_BOOST_DATA INSTANTIATE_TEST_CASE_P(BoostDesc_BGM, DescriptorRotationInvariance, Values( make_tuple(IMAGE_TSUKUBA, - SURF::create(), - BoostDesc::create(BoostDesc::BGM, true, 6.25f), + []() { return SURF::create(); }, + []() { return BoostDesc::create(BoostDesc::BGM, true, 6.25f); }, 0.999f) )); INSTANTIATE_TEST_CASE_P(BoostDesc_BGM_HARD, DescriptorRotationInvariance, Values( make_tuple(IMAGE_TSUKUBA, - SURF::create(), - BoostDesc::create(BoostDesc::BGM_HARD, true, 6.25f), + []() { return SURF::create(); }, + []() { return BoostDesc::create(BoostDesc::BGM_HARD, true, 6.25f); }, 0.98f) )); INSTANTIATE_TEST_CASE_P(BoostDesc_BGM_BILINEAR, DescriptorRotationInvariance, Values( make_tuple(IMAGE_TSUKUBA, - SURF::create(), - BoostDesc::create(BoostDesc::BGM_BILINEAR, true, 6.25f), + []() { return SURF::create(); }, + []() { return BoostDesc::create(BoostDesc::BGM_BILINEAR, true, 6.25f); }, 0.98f) )); INSTANTIATE_TEST_CASE_P(BoostDesc_BGM_LBGM, DescriptorRotationInvariance, Values( make_tuple(IMAGE_TSUKUBA, - SURF::create(), - BoostDesc::create(BoostDesc::LBGM, true, 6.25f), + []() { return SURF::create(); }, + []() { return BoostDesc::create(BoostDesc::LBGM, true, 6.25f); }, 0.999f) )); INSTANTIATE_TEST_CASE_P(BoostDesc_BINBOOST_64, DescriptorRotationInvariance, Values( make_tuple(IMAGE_TSUKUBA, - SURF::create(), - BoostDesc::create(BoostDesc::BINBOOST_64, true, 6.25f), + []() { return SURF::create(); }, + []() { return BoostDesc::create(BoostDesc::BINBOOST_64, true, 6.25f); }, 0.98f) )); INSTANTIATE_TEST_CASE_P(BoostDesc_BINBOOST_128, DescriptorRotationInvariance, Values( make_tuple(IMAGE_TSUKUBA, - SURF::create(), - BoostDesc::create(BoostDesc::BINBOOST_128, true, 6.25f), + []() { return SURF::create(); }, + []() { return BoostDesc::create(BoostDesc::BINBOOST_128, true, 6.25f); }, 0.98f) )); INSTANTIATE_TEST_CASE_P(BoostDesc_BINBOOST_256, DescriptorRotationInvariance, Values( make_tuple(IMAGE_TSUKUBA, - SURF::create(), - BoostDesc::create(BoostDesc::BINBOOST_256, true, 6.25f), + []() { return SURF::create(); }, + []() { return BoostDesc::create(BoostDesc::BINBOOST_256, true, 6.25f); }, 0.999f) )); #endif // OPENCV_XFEATURES2D_HAS_BOOST_DATA @@ -159,11 +159,11 @@ INSTANTIATE_TEST_CASE_P(BoostDesc_BINBOOST_256, DescriptorRotationInvariance, Va #ifdef OPENCV_ENABLE_NONFREE INSTANTIATE_TEST_CASE_P(SURF, DetectorScaleInvariance, Values( - make_tuple(IMAGE_BIKES, SURF::create(), 0.64f, 0.84f) + make_tuple(IMAGE_BIKES, []() { return SURF::create(); }, 0.64f, 0.84f) )); INSTANTIATE_TEST_CASE_P(SURF, DescriptorScaleInvariance, Values( - make_tuple(IMAGE_BIKES, SURF::create(), SURF::create(), 0.7f) + make_tuple(IMAGE_BIKES, []() { return SURF::create(); }, []() { return SURF::create(); }, 0.7f) )); #endif // NONFREE @@ -171,8 +171,8 @@ INSTANTIATE_TEST_CASE_P(SURF, DescriptorScaleInvariance, Values( #if 0 // DAISY is not scale invariant INSTANTIATE_TEST_CASE_P(DISABLED_DAISY, DescriptorScaleInvariance, Values( make_tuple(IMAGE_BIKES, - BRISK::create(), - DAISY::create(15, 3, 8, 8, DAISY::NRM_NONE, noArray(), true, true), + []() { return BRISK::create(); }, + []() { return DAISY::create(15, 3, 8, 8, DAISY::NRM_NONE, noArray(), true, true); }, 0.1f) )); #endif @@ -180,26 +180,26 @@ INSTANTIATE_TEST_CASE_P(DISABLED_DAISY, DescriptorScaleInvariance, Values( #ifdef OPENCV_XFEATURES2D_HAS_VGG_DATA INSTANTIATE_TEST_CASE_P(VGG120, DescriptorScaleInvariance, Values( make_tuple(IMAGE_BIKES, - KAZE::create(), - VGG::create(VGG::VGG_120, 1.4f, true, true, 48.0f, false), + []() { return KAZE::create(); }, + []() { return VGG::create(VGG::VGG_120, 1.4f, true, true, 48.0f, false); }, 0.98f) )); INSTANTIATE_TEST_CASE_P(VGG80, DescriptorScaleInvariance, Values( make_tuple(IMAGE_BIKES, - KAZE::create(), - VGG::create(VGG::VGG_80, 1.4f, true, true, 48.0f, false), + []() { return KAZE::create(); }, + []() { return VGG::create(VGG::VGG_80, 1.4f, true, true, 48.0f, false); }, 0.98f) )); INSTANTIATE_TEST_CASE_P(VGG64, DescriptorScaleInvariance, Values( make_tuple(IMAGE_BIKES, - KAZE::create(), - VGG::create(VGG::VGG_64, 1.4f, true, true, 48.0f, false), + []() { return KAZE::create(); }, + []() { return VGG::create(VGG::VGG_64, 1.4f, true, true, 48.0f, false); }, 0.97f) )); INSTANTIATE_TEST_CASE_P(VGG48, DescriptorScaleInvariance, Values( make_tuple(IMAGE_BIKES, - KAZE::create(), - VGG::create(VGG::VGG_48, 1.4f, true, true, 48.0f, false), + []() { return KAZE::create(); }, + []() { return VGG::create(VGG::VGG_48, 1.4f, true, true, 48.0f, false); }, 0.93f) )); #endif // OPENCV_XFEATURES2D_HAS_VGG_DATA @@ -208,44 +208,44 @@ INSTANTIATE_TEST_CASE_P(VGG48, DescriptorScaleInvariance, Values( #ifdef OPENCV_XFEATURES2D_HAS_BOOST_DATA INSTANTIATE_TEST_CASE_P(BoostDesc_BGM, DescriptorScaleInvariance, Values( make_tuple(IMAGE_BIKES, - SURF::create(), - BoostDesc::create(BoostDesc::BGM, true, 6.25f), + []() { return SURF::create(); }, + []() { return BoostDesc::create(BoostDesc::BGM, true, 6.25f); }, 0.98f) )); INSTANTIATE_TEST_CASE_P(BoostDesc_BGM_HARD, DescriptorScaleInvariance, Values( make_tuple(IMAGE_BIKES, - SURF::create(), - BoostDesc::create(BoostDesc::BGM_HARD, true, 6.25f), + []() { return SURF::create(); }, + []() { return BoostDesc::create(BoostDesc::BGM_HARD, true, 6.25f); }, 0.75f) )); INSTANTIATE_TEST_CASE_P(BoostDesc_BGM_BILINEAR, DescriptorScaleInvariance, Values( make_tuple(IMAGE_BIKES, - SURF::create(), - BoostDesc::create(BoostDesc::BGM_BILINEAR, true, 6.25f), + []() { return SURF::create(); }, + []() { return BoostDesc::create(BoostDesc::BGM_BILINEAR, true, 6.25f); }, 0.95f) )); INSTANTIATE_TEST_CASE_P(BoostDesc_LBGM, DescriptorScaleInvariance, Values( make_tuple(IMAGE_BIKES, - SURF::create(), - BoostDesc::create(BoostDesc::LBGM, true, 6.25f), + []() { return SURF::create(); }, + []() { return BoostDesc::create(BoostDesc::LBGM, true, 6.25f); }, 0.95f) )); INSTANTIATE_TEST_CASE_P(BoostDesc_BINBOOST_64, DescriptorScaleInvariance, Values( make_tuple(IMAGE_BIKES, - SURF::create(), - BoostDesc::create(BoostDesc::BINBOOST_64, true, 6.25f), + []() { return SURF::create(); }, + []() { return BoostDesc::create(BoostDesc::BINBOOST_64, true, 6.25f); }, 0.75f) )); INSTANTIATE_TEST_CASE_P(BoostDesc_BINBOOST_128, DescriptorScaleInvariance, Values( make_tuple(IMAGE_BIKES, - SURF::create(), - BoostDesc::create(BoostDesc::BINBOOST_128, true, 6.25f), + []() { return SURF::create(); }, + []() { return BoostDesc::create(BoostDesc::BINBOOST_128, true, 6.25f); }, 0.95f) )); INSTANTIATE_TEST_CASE_P(BoostDesc_BINBOOST_256, DescriptorScaleInvariance, Values( make_tuple(IMAGE_BIKES, - SURF::create(), - BoostDesc::create(BoostDesc::BINBOOST_256, true, 6.25f), + []() { return SURF::create(); }, + []() { return BoostDesc::create(BoostDesc::BINBOOST_256, true, 6.25f); }, 0.98f) )); #endif // OPENCV_XFEATURES2D_HAS_BOOST_DATA From ac994ed2b5b6dd37d60ae5cd4267b61ceefa052d Mon Sep 17 00:00:00 2001 From: ernie <35775651+simutisernestas@users.noreply.github.com> Date: Wed, 17 Apr 2024 16:15:44 +0200 Subject: [PATCH 2/2] Merge pull request #3641 from simutisernestas:4.x MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Camera spatial velocity computation through interaction matrix #3641 ### Feature The code computed camera spatial velocity given two images, pixels depth, camera matrix and the timestamp between the images. This is enabled by, so called, interaction matrix (usually utilized in visual servoing applications) relating pixel plane velocities to the camera spatial velocity in 3D i.e., twist - velocity and angular rate of the camera. The inverse problem can be solved by sampling pixel & their velocities to solve least-squares for twist. The relationship can be seen below in the picture. ![image](https://github.com/opencv/opencv_contrib/assets/35775651/b83179ba-9d5a-4324-863a-4ad6e158564a) The code does not include a proper example and is not tested but if there is interest I could contribute more appealing example and use case for camera velocity computation. However, I attach below a dummy example with random data simply to make sure that it's running as is. I have used this before in aiding UAV localization and thought someone else might benefit from it being integrated into `opencv`. ```c++ #include "opencv2/rgbd/twist.hpp" #include "opencv2/core.hpp" #include "opencv2/imgproc.hpp" int main() { using namespace cv::rgbd; Twist t; // create two random image cv::Mat im0(480, 640, CV_8UC1); cv::Mat im1(480, 640, CV_8UC1); cv::Mat depths0(480, 640, CV_32F); cv::Mat K = cv::Mat::eye(3, 3, CV_32F); cv::theRNG().state = time(0); cv::randn(im0, 0, 255); cv::randn(im1, 0, 255); cv::randn(depths0, 0, 100); cv::Vec6d twist = t.compute(im0, im1, depths0, K, 0.1); return 0; } ``` References 1. Chaumette, François, and Seth Hutchinson. "Visual servo control. I. Basic approaches." IEEE Robotics & Automation Magazine 13.4 (2006): 82-90. 2. https://robotacademy.net.au/lesson/image-based-visual-servoing/ ### Pull Request Readiness Checklist See details at https://github.com/opencv/opencv/wiki/How_to_contribute#making-a-good-pull-request - [x] I agree to contribute to the project under Apache 2 License. - [x] To the best of my knowledge, the proposed patch is not based on a code under GPL or another license that is incompatible with OpenCV - [x] The PR is proposed to the proper branch - [x] There is a reference to the original bug report and related work - [ ] There is accuracy test, performance test and test data in opencv_extra repository, if applicable Patch to opencv_extra has the same branch name. - [ ] The feature is well documented and sample code can be built with the project CMake --- .../include/opencv2/tracking/twist.hpp | 53 +++++++++ modules/tracking/src/twist.cpp | 76 ++++++++++++ modules/tracking/test/test_twist.cpp | 111 ++++++++++++++++++ 3 files changed, 240 insertions(+) create mode 100644 modules/tracking/include/opencv2/tracking/twist.hpp create mode 100644 modules/tracking/src/twist.cpp create mode 100644 modules/tracking/test/test_twist.cpp diff --git a/modules/tracking/include/opencv2/tracking/twist.hpp b/modules/tracking/include/opencv2/tracking/twist.hpp new file mode 100644 index 00000000000..8d998beda33 --- /dev/null +++ b/modules/tracking/include/opencv2/tracking/twist.hpp @@ -0,0 +1,53 @@ +#ifndef OPENCV_TWIST_HPP +#define OPENCV_TWIST_HPP + +#include "opencv2/core.hpp" + +namespace cv +{ +namespace detail +{ +inline namespace tracking +{ +//! @addtogroup tracking_detail +//! @{ + +/** + * @brief Compute the camera twist from a set of 2D pixel locations, their + * velocities, depth values and intrinsic parameters of the camera. The pixel + * velocities are usually obtained from optical flow algorithms, both dense and + * sparse flow can be used to compute the flow between images and duv computed by + * dividing the flow by the time interval between the images. + * + * @param uv 2xN matrix of 2D pixel locations + * @param duv 2Nx1 matrix of 2D pixel velocities + * @param depths 1xN matrix of depth values + * @param K 3x3 camera intrinsic matrix + * + * @return cv::Vec6d 6x1 camera twist + */ +CV_EXPORTS cv::Vec6d computeTwist(const cv::Mat& uv, const cv::Mat& duv, const cv::Mat& depths, + const cv::Mat& K); + +/** + * @brief Compute the interaction matrix for a set of 2D pixels. This is usually + * used in visual servoing applications to command a robot to move at desired pixel + * locations/velocities. By inverting this matrix one can estimate camera spatial + * velocity i.e., the twist. + * + * @param uv 2xN matrix of 2D pixel locations + * @param depths 1xN matrix of depth values + * @param K 3x3 camera intrinsic matrix + * @param J 2Nx6 interaction matrix + * + */ +CV_EXPORTS void getInteractionMatrix(const cv::Mat& uv, const cv::Mat& depths, const cv::Mat& K, + cv::Mat& J); + +//! @} + +} // namespace tracking +} // namespace detail +} // namespace cv + +#endif diff --git a/modules/tracking/src/twist.cpp b/modules/tracking/src/twist.cpp new file mode 100644 index 00000000000..1ff84c42582 --- /dev/null +++ b/modules/tracking/src/twist.cpp @@ -0,0 +1,76 @@ + +#include "precomp.hpp" +#include "opencv2/tracking/twist.hpp" + +namespace cv +{ +namespace detail +{ +inline namespace tracking +{ + +void getInteractionMatrix(const cv::Mat& uv, const cv::Mat& depths, const cv::Mat& K, cv::Mat& J) +{ + CV_Assert(uv.cols == depths.cols); + CV_Assert(depths.type() == CV_32F); + CV_Assert(K.cols == 3 && K.rows == 3); + + J.create(depths.cols * 2, 6, CV_32F); + J.setTo(0); + + cv::Mat Kinv; + cv::invert(K, Kinv); + + cv::Mat xy(3, 1, CV_32F); + cv::Mat Jp(2, 6, CV_32F); + for (int i = 0; i < uv.cols; i++) + { + const float z = depths.at(i); + // skip points with zero depth + if (cv::abs(z) < 0.001f) + continue; + + const cv::Point3f p(uv.at(0, i), uv.at(1, i), 1.0); + + // convert to normalized image-plane coordinates + xy = Kinv * cv::Mat(p); + float x = xy.at(0); + float y = xy.at(1); + + // 2x6 Jacobian for this point + Jp.at(0, 0) = -1 / z; + Jp.at(0, 1) = 0.0; + Jp.at(0, 2) = x / z; + Jp.at(0, 3) = x * y; + Jp.at(0, 4) = -(1 + x * x); + Jp.at(0, 5) = y; + Jp.at(1, 0) = 0.0; + Jp.at(1, 1) = -1 / z; + Jp.at(1, 2) = y / z; + Jp.at(1, 3) = 1 + y * y; + Jp.at(1, 4) = -x * y; + Jp.at(1, 5) = -x; + + Jp = K(cv::Rect(0, 0, 2, 2)) * Jp; + + // push into Jacobian + Jp.copyTo(J(cv::Rect(0, 2 * i, 6, 2))); + } +} + +cv::Vec6d computeTwist(const cv::Mat& uv, const cv::Mat& duv, const cv::Mat& depths, + const cv::Mat& K) +{ + CV_Assert(uv.cols * 2 == duv.rows); + + cv::Mat J; + getInteractionMatrix(uv, depths, K, J); + cv::Mat Jinv; + cv::invert(J, Jinv, cv::DECOMP_SVD); + cv::Mat twist = Jinv * duv; + return twist; +} + +} // namespace tracking +} // namespace detail +} // namespace cv diff --git a/modules/tracking/test/test_twist.cpp b/modules/tracking/test/test_twist.cpp new file mode 100644 index 00000000000..3911f28aea8 --- /dev/null +++ b/modules/tracking/test/test_twist.cpp @@ -0,0 +1,111 @@ +#include "test_precomp.hpp" + +#include "opencv2/core.hpp" +#include "opencv2/tracking/twist.hpp" + +namespace opencv_test +{ +namespace +{ + +using namespace cv::detail::tracking; + +float const eps = 1e-4f; + +class TwistTest : public ::testing::Test +{ +protected: + cv::Mat J, K; + + void SetUp() override + { + cv::Matx33f K = {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0}; + this->K = cv::Mat(K); + } +}; + +TEST_F(TwistTest, TestInteractionMatrix) +{ + // import machinevisiontoolbox as mv + // cam = mv.CentralCamera() + // print(cam.K) + // print(cam.visjac_p([1, 1], 2.0)) + // [[1. 0. 0.] + // [0. 1. 0.] + // [0. 0. 1.]] + // [[-0.5 0. 0.5 1. -2. 1. ] + // [ 0. -0.5 0.5 2. -1. -1. ]] + + cv::Mat uv = cv::Mat(2, 1, CV_32F, {1.0f, 1.0f}); + cv::Mat depth = cv::Mat(1, 1, CV_32F, {2.0f}); + + getInteractionMatrix(uv, depth, K, J); + ASSERT_EQ(J.cols, 6); + ASSERT_EQ(J.rows, 2); + float expected[2][6] = {{-0.5f, 0.0f, 0.5f, 1.0f, -2.0f, 1.0f}, + {0.0f, -0.5f, 0.5f, 2.0f, -1.0f, -1.0f}}; + for (int i = 0; i < 2; i++) + for (int j = 0; j < 6; j++) + ASSERT_NEAR(J.at(i, j), expected[i][j], eps); +} + +TEST_F(TwistTest, TestComputeWithZeroPixelVelocities) +{ + cv::Mat uv = cv::Mat(2, 2, CV_32F, {1.0f, 0.0f, 3.0f, 0.0f}); + cv::Mat depths = cv::Mat(1, 2, CV_32F, {1.1f, 1.0f}); + cv::Mat duv = cv::Mat(4, 1, CV_32F, {0.0f, 0.0f, 0.0f, 0.0f}); + + cv::Vec6d result = computeTwist(uv, duv, depths, K); + for (int i = 0; i < 6; i++) + ASSERT_NEAR(result[i], 0.0, eps); +} + +TEST_F(TwistTest, TestComputeWithNonZeroPixelVelocities) +{ + // import machinevisiontoolbox as mv + // cam = mv.CentralCamera() + // pixels = np.array([[1, 2, 3], + // [1, 2, 3]], dtype=float) + // depths = np.array([1.0, 2.0, 3.0]) + // Jac = cam.visjac_p(pixels, depths) + // duv = np.array([1, 2, 1, 3, 1, 4]) + // twist = np.linalg.lstsq(Jac, duv, rcond=None)[0] + // print(twist) + // print(Jac) + // [ 0.5 0.5 1.875 0.041667 -0.041667 -0.5 ] + // [[ -1. 0. 1. 1. -2. 1. ] + // [ 0. -1. 1. 2. -1. -1. ] + // [ -0.5 0. 1. 4. -5. 2. ] + // [ 0. -0.5 1. 5. -4. -2. ] + // [ -0.333333 0. 1. 9. -10. 3. ] + // [ 0. -0.333333 1. 10. -9. -3. ]] + + float uv_data[] = {1.0f, 2.0f, 3.0f, 1.0f, 2.0f, 3.0f}; + cv::Mat uv = cv::Mat(2, 3, CV_32F, uv_data); + float depth_data[] = {1.0f, 2.0f, 3.0f}; + cv::Mat depth = cv::Mat(1, 3, CV_32F, depth_data); + float duv_data[] = {1.0f, 2.0f, 1.0f, 3.0f, 1.0f, 4.0f}; + cv::Mat duv = cv::Mat(6, 1, CV_32F, duv_data); + + getInteractionMatrix(uv, depth, K, J); + ASSERT_EQ(J.cols, 6); + ASSERT_EQ(J.rows, 6); + float expected_jac[6][6] = {{-1.0f, 0.0f, 1.0f, 1.0f, -2.0f, 1.0f}, + {0.0f, -1.0f, 1.0f, 2.0f, -1.0f, -1.0f}, + {-0.5f, 0.0f, 1.0f, 4.0f, -5.0f, 2.0f}, + {0.0f, -0.5f, 1.0f, 5.0f, -4.0f, -2.0f}, + {-0.333333f, 0.0f, 1.0f, 9.0f, -10.0f, 3.0f}, + {0.0f, -0.333333f, 1.0f, 10.0f, -9.0f, -3.0f}}; + + for (int i = 0; i < 6; i++) + for (int j = 0; j < 6; j++) + ASSERT_NEAR(J.at(i, j), expected_jac[i][j], eps); + + cv::Vec6d result = computeTwist(uv, duv, depth, K); + float expected_twist[6] = {0.5f, 0.5f, 1.875f, 0.041667f, -0.041667f, -0.5f}; + for (int i = 0; i < 6; i++) + ASSERT_NEAR(result[i], expected_twist[i], eps); +} + +} // namespace +} // namespace opencv_test