OpenCV: неправильный результат в функции калибровкиHandEye ⇐ C++

[Anonymous]

Я работаю над приложением для роботов, в котором камера прикреплена к захвату робота. Чтобы вычислить преобразование матрицы между камерой и захватом Hcg, я использую новую функцию калиброватьHandEye, представленную в OpenCV версии 4.1.0

Я сделал 10 снимков шахматную доску с камеры, установленной в захвате, и одновременно записывал положение робота.

Код, над которым я работаю:

Код: Выделить всё

// My_handeye.cpp : This file contains the 'main' function.  Program execution begins and ends there.
//
#include 
#include 
#include 
#include 
#include 
#include "pch.h"

#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 

using namespace cv;
using namespace std;

Mat eulerAnglesToRotationMatrix(Vec3f &theta);
Vec3f rotationMatrixToEulerAngles(Mat &R);
float rad2deg(float radian);
float deg2rad(float degree);

int main()
{

// Camera calibration information

std::vector distortionCoefficients(5);  // camera distortion
distortionCoefficients[0] = 2.4472856611074989e-01;
distortionCoefficients[1] = -8.1042032574246325e-01;
distortionCoefficients[2] = 0;
distortionCoefficients[3] = 0;
distortionCoefficients[4] = 7.8769462320821060e-01;

double f_x = 1.3624172121852105e+03; // Focal length in x axis
double f_y = 1.3624172121852105e+03; // Focal length in y axis (usually the same?)
double c_x = 960; // Camera primary point x
double c_y = 540; // Camera primary point y

cv::Mat cameraMatrix(3, 3, CV_32FC1);
cameraMatrix.at(0, 0) = f_x;
cameraMatrix.at(0, 1) = 0.0;
cameraMatrix.at(0, 2) = c_x;
cameraMatrix.at(1, 0) = 0.0;
cameraMatrix.at(1, 1) = f_y;
cameraMatrix.at(1, 2) = c_y;
cameraMatrix.at(2, 0) = 0.0;
cameraMatrix.at(2, 1) = 0.0;
cameraMatrix.at(2, 2) = 1.0;

Mat rvec(3, 1, CV_32F), tvec(3, 1, CV_32F);
//

std::vector R_gripper2base;
std::vector t_gripper2base;
std::vector R_target2cam;
std::vector t_target2cam;
Mat R_cam2gripper = (Mat_(3, 3));
Mat t_cam2gripper = (Mat_(3, 1));

vector fn;
glob("images/*.bmp", fn, false);

vector images;
size_t num_images = fn.size(); //number of bmp files in images folder
Size patternsize(6, 8); //number of centers
vector
 centers; //this will be filled by the detected centers
float cell_size = 30;
vector obj_points;

R_gripper2base.reserve(num_images);
t_gripper2base.reserve(num_images);
R_target2cam.reserve(num_images);
t_target2cam.reserve(num_images);

for (int i = 0; i < patternsize.height; ++i)
for (int j = 0; j < patternsize.width; ++j)
obj_points.push_back(Point3f(float(j*cell_size),
float(i*cell_size), 0.f));

for (size_t i = 0; i < num_images; i++)
images.push_back(imread(fn[i]));

Mat frame;

for (size_t i = 0; i < num_images; i++)
{
frame = imread(fn[i]); //source image
bool patternfound = findChessboardCorners(frame, patternsize, centers);
if (patternfound)
{
drawChessboardCorners(frame, patternsize, Mat(centers), patternfound);
//imshow("window", frame);
//int key = cv::waitKey(0) & 0xff;
solvePnP(Mat(obj_points), Mat(centers), cameraMatrix, distortionCoefficients, rvec, tvec);

Mat R;
Rodrigues(rvec, R); // R is 3x3
R_target2cam.push_back(R);
t_target2cam.push_back(tvec);
Mat T = Mat::eye(4, 4, R.type()); // T is 4x4
T(Range(0, 3), Range(0, 3)) = R * 1; // copies R into T
T(Range(0, 3), Range(3, 4)) = tvec * 1; // copies tvec into T

cout 

Подробнее здесь: [url]https://stackoverflow.com/questions/57008332/opencv-wrong-result-in-calibratehandeye-function[/url]