Camera spatial velocity computation through interaction matrix

[simutisernestas]

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.

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.

#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

• I agree to contribute to the project under Apache 2 License.
• 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
• The PR is proposed to the proper branch
• 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

[savuor]

To my mind:

1. This should go into the tracking module
2. We should use sparse set of point correspondences, not dense and definitely not based on a specific optical flow algorithm.
3. The problem as it is formulated now can be solved by a combination of findEssentialMat + decomposeEssentialMat or recoverPose and Rt-to-twist decomposition available in DualQuat class from 5.x, even without using additional Z coordinates. And it's more preferable since the existing functions are quite robust and fast, thanks to RANSAC.
4. This function would make more sense as a solution to a different problem if it is changed to take pixel velocities as input instead of images or points.
5. The function should be documented
6. The function should be covered by at least one test. You can transform your sample code to a test.

[simutisernestas]

1. This should go into the `tracking` module

moved

2. We should use sparse set of point correspondences, not dense and definitely not based on a specific optical flow algorithm.

removed the the flow calculation from withing the class

3. The problem as it is formulated now can be solved by a combination of `findEssentialMat` + `decomposeEssentialMat` or `recoverPose` and Rt-to-twist decomposition available in `DualQuat` class from 5.x, even without using additional Z coordinates. And it's more preferable since the existing functions are quite robust and fast, thanks to RANSAC.

I suppose this doesn't provide information about the scale of the scene? Having the depth input here, the velocity information is in absolute units right away.

4. This function would make more sense as a solution to a different problem if it is changed to take pixel velocities as input instead of images or points.

I've refactored the code to explicitly take in the pixel velocities.

5. The function should be documented

Done

6. The function should be covered by at least one test. You can transform your sample code to a test.

Done

Also made the interaction matrix computation as public method. Thus one could use that for visual servoing applications, like driving a camera with specific pixel velocities, or up to a pixel location.

[savuor]

I think you're right, we need Z values to get an absolute scene scale. This means that we do need that functionality.
However, there are things to fix before it's merged:

[simutisernestas]

Addressed all of the comments

[simutisernestas]

@savuor Could you rerun the CI, I've removed some dead comments in tests and fixed miss-matched docs parameter. Should pass all the checks now : )

[simutisernestas]

LGTM