Skip to content

Commit

Permalink
refactor class to functions & cv::Mat initialiation
Browse files Browse the repository at this point in the history
  • Loading branch information
@simutisernestas
simutisernestas committed Mar 13, 2024
1 parent 3031e4b commit 3310504
Show file tree
Hide file tree
Showing 3 changed files with 85 additions and 63 deletions.
67 changes: 31 additions & 36 deletions modules/tracking/include/opencv2/tracking/twist.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,42 +12,37 @@ inline namespace tracking
//! @addtogroup tracking_detail
//! @{

class CV_EXPORTS Twist
{
public:
Twist() = default;

/**
* @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::Vec6d compute(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 depth 1xN matrix of depth values
* @param K 3x3 camera intrinsic matrix
* @param J 2Nx6 interaction matrix
*
*/
void interactionMatrix(const cv::Mat& uv, const cv::Mat& depth, const cv::Mat& K, cv::Mat& J);
};
/**
* @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 depth 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);

//! @}

Expand Down
30 changes: 20 additions & 10 deletions modules/tracking/src/twist.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,13 +9,13 @@ namespace detail
inline namespace tracking
{

void Twist::interactionMatrix(const cv::Mat& uv, const cv::Mat& depth, const cv::Mat& K, cv::Mat& J)
void getInteractionMatrix(const cv::Mat& uv, const cv::Mat& depths, const cv::Mat& K, cv::Mat& J)
{
CV_Assert(uv.cols == depth.cols);
CV_Assert(depth.type() == CV_32F);
CV_Assert(uv.cols == depths.cols);
CV_Assert(depths.type() == CV_32F);
CV_Assert(K.cols == 3 && K.rows == 3);

J.create(depth.cols * 2, 6, CV_32F);
J.create(depths.cols * 2, 6, CV_32F);
J.setTo(0);

cv::Mat Kinv;
Expand All @@ -25,7 +25,7 @@ void Twist::interactionMatrix(const cv::Mat& uv, const cv::Mat& depth, const cv:
cv::Mat Jp(2, 6, CV_32F);
for (int i = 0; i < uv.cols; i++)
{
const float z = depth.at<float>(i);
const float z = depths.at<float>(i);
// skip points with zero depth
if (cv::abs(z) < 0.001f)
continue;
Expand All @@ -38,8 +38,18 @@ void Twist::interactionMatrix(const cv::Mat& uv, const cv::Mat& depth, const cv:
float y = xy.at<float>(1);

// 2x6 Jacobian for this point
Jp = (cv::Mat_<float>(2, 6) << -1 / z, 0.0, x / z, x * y, -(1 + x * x), y, 0.0, -1 / z,
y / z, 1 + y * y, -x * y, -x);
Jp.at<float>(0, 0) = -1 / z;
Jp.at<float>(0, 1) = 0.0;
Jp.at<float>(0, 2) = x / z;
Jp.at<float>(0, 3) = x * y;
Jp.at<float>(0, 4) = -(1 + x * x);
Jp.at<float>(0, 5) = y;
Jp.at<float>(1, 0) = 0.0;
Jp.at<float>(1, 1) = -1 / z;
Jp.at<float>(1, 2) = y / z;
Jp.at<float>(1, 3) = 1 + y * y;
Jp.at<float>(1, 4) = -x * y;
Jp.at<float>(1, 5) = -x;

Jp = K(cv::Rect(0, 0, 2, 2)) * Jp;

Expand All @@ -48,13 +58,13 @@ void Twist::interactionMatrix(const cv::Mat& uv, const cv::Mat& depth, const cv:
}
}

cv::Vec6d Twist::compute(const cv::Mat& uv, const cv::Mat& duv, const cv::Mat depths,
const cv::Mat& K)
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;
interactionMatrix(uv, depths, K, J);
getInteractionMatrix(uv, depths, K, J);
cv::Mat Jinv;
cv::invert(J, Jinv, cv::DECOMP_SVD);
cv::Mat twist = Jinv * duv;
Expand Down
51 changes: 34 additions & 17 deletions modules/tracking/test/test_twist.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,22 +3,24 @@
#include "opencv2/core.hpp"
#include "opencv2/tracking/twist.hpp"

#define eps 1e-4

namespace opencv_test
{
namespace
{

using namespace cv::detail::tracking;

float const eps = 1e-4f;

class TwistTest : public ::testing::Test
{
protected:
cv::detail::tracking::Twist twist;
cv::Mat K, J;
cv::Mat J, K;

void SetUp() override
{
K = (cv::Mat_<float>(3, 3) << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0);
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);
}
};

Expand All @@ -34,10 +36,13 @@ TEST_F(TwistTest, TestInteractionMatrix)
// [[-0.5 0. 0.5 1. -2. 1. ]
// [ 0. -0.5 0.5 2. -1. -1. ]]

cv::Mat uv = (cv::Mat_<float>(2, 1) << 1.0, 1.0);
cv::Mat depth = (cv::Mat_<float>(1, 1) << 2.0);
// float data[] = {1.5f, 2.0f, 3.0f, 4.2f};
// cv::Mat mat = cv::Mat(2, 2, CV_32F, data);

twist.interactionMatrix(uv, depth, K, J);
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},
Expand All @@ -49,11 +54,11 @@ TEST_F(TwistTest, TestInteractionMatrix)

TEST_F(TwistTest, TestComputeWithZeroPixelVelocities)
{
cv::Mat uv = (cv::Mat_<float>(2, 2) << 0.0, 0.0, 0.0, 0.0);
cv::Mat depth = (cv::Mat_<float>(1, 2) << 1.0, 1.0);
cv::Mat duv = (cv::Mat_<float>(4, 1) << 0.0, 0.0, 0.0, 0.0);
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 = twist.compute(uv, duv, depth, K);
cv::Vec6d result = computeTwist(uv, duv, depths, K);
for (int i = 0; i < 6; i++)
ASSERT_NEAR(result[i], 0.0, eps);
}
Expand All @@ -78,11 +83,23 @@ TEST_F(TwistTest, TestComputeWithNonZeroPixelVelocities)
// [ -0.333333 0. 1. 9. -10. 3. ]
// [ 0. -0.333333 1. 10. -9. -3. ]]

cv::Mat uv = (cv::Mat_<float>(2, 3) << 1.0, 2.0, 3.0, 1.0, 2.0, 3.0);
cv::Mat depth = (cv::Mat_<float>(1, 3) << 1.0, 2.0, 3.0);
cv::Mat duv = (cv::Mat_<float>(6, 1) << 1.0, 2.0, 1.0, 3.0, 1.0, 4.0);
// cv::Mat uv = (cv::Mat_<float>(2, 3) << 1.0, 2.0, 3.0, 1.0, 2.0, 3.0);
// std::vector<float> depth_data = {1.0f, 2.0f, 3.0f};
// cv::Mat depth = cv::Mat(depth_data).reshape(0, 3);
// cv::Mat duv = (cv::Mat_<float>(6, 1) << 1.0, 2.0, 1.0, 3.0, 1.0, 4.0);

// cv::Mat uv = (cv::Mat_<float>(2, 3) << 1.0, 2.0, 3.0, 1.0, 2.0, 3.0);
// cv::Mat depth = (cv::Mat_<float>(1, 3) << 1.0, 2.0, 3.0);
// cv::Mat duv = (cv::Mat_<float>(6, 1) << 1.0, 2.0, 1.0, 3.0, 1.0, 4.0);

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);

twist.interactionMatrix(uv, depth, K, J);
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},
Expand All @@ -96,7 +113,7 @@ TEST_F(TwistTest, TestComputeWithNonZeroPixelVelocities)
for (int j = 0; j < 6; j++)
ASSERT_NEAR(J.at<float>(i, j), expected_jac[i][j], eps);

cv::Vec6d result = twist.compute(uv, duv, depth, K);
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);
Expand Down

0 comments on commit 3310504

Please sign in to comment.