You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
111 lines
3.7 KiB
111 lines
3.7 KiB
#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}); |
|
|
|
computeInteractionMatrix(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<float>(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); |
|
|
|
computeInteractionMatrix(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<float>(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
|
|
|