用于 EYE IN HAND
理论结束,代码如下:
#include <vector>
#include <opencv2/opencv.hpp>
using namespace std;
vector<cv::Mat> RT_Tcpij; //movement of Tcp coordinates
vector<cv::Mat> RT_Camij; //movement of Camera coordinates
//向量转反对称矩阵
cv::Mat skew(cv::Mat vec) {
cv::Mat skewM(3, 3, CV_64FC1);
double vx = vec.at<double>(0, 0);
double vy = vec.at<double>(1, 0);
double vz = vec.at<double>(2, 0);
skewM.at<double>(0, 0) = 0.0; skewM.at<double>(0, 1) = -vz; sk