Camera spatial velocity computation through interaction matrix opencv#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