3D视觉工坊-手眼标定(附opencv实现代码)

2024年2月12日发(作者：饶兢群)

vector Homo_target2cam; vector Homo_gripper2ba; Mat tempR, tempT, temp; std::vector filenames; // notice here that we are using the Opencv's embedded "String" class cv::String folder = "/home/cai/hans_robot/PIC/hand_calibrate/2021.11.22old3"; // again we are using the Opencv's embedded "String" class cv::glob(folder, filenames); // new function that does the job ;-) string fin_name = folder + "/"; cout << fin_name << endl; ifstream fin(fin_name);

for (int i = 0; i <() / 2; i++) //计算标定板与相机间的齐次矩阵（旋转矩阵与平移向量） {

double data[12] = { 0 }; for (auto &d : data) { fin >> d; //cout << " " << d << endl; } getRT_fromTxt(data, tempR, tempT); temp = R_T2HomogeneousMatrix(tempR, tempR); Homo__back(temp); //tempT = tempT / 1000; /*cout << i << "::" << temp << endl;*/ //cout << i << "::" << tempR << endl; cout << i << "end to ba ::" << tempT.t() << endl; R__back(tempR); T__back(tempT); } cout << "board to camera::" << endl; //for (size_t i = 0; i < ; ++i) for (size_t i = 0; i < () / 2;++i) {

string rgb_name = folder + "/" + to_string(i) + "_"; string depth_name = folder + "/" + to_string(i) + "_"; //std::cout << rgb_name << std::endl; cv::Mat rgb = cv::imread(rgb_name); cv::Mat depth = cv::imread(depth_name, -1); /*cv::imshow("depth", depth); cv::waitKey(0);*/ if (!) std::cerr << "Problem loading image" << std::endl;

cv::Mat pnp_R,pnp_t; inputParameter(rgb, depth, cv::Size(11, 8), 30,pnp_R,pnp_t); //cout << "标定板到相机mat_camera" << mat_camera << endl; Homo__back(mat_camera);

HomogeneousMtr2RT(mat_camera, tempR, tempT); //tempT = tempT / 1000; //cout << i << "标定板到相机 ::" << mat_camera << endl;

//cout << i << "::" << tempT << endl;

R = (Mat_(3, 3) << data[0], data[1], data[2], data[4], data[5], data[6], data[8], data[9], data[10] ); T = (Mat_(3, 1) << data[3], data[7], data[11] );

}/*************************************************** @brief 将旋转矩阵与平移向量合成为齐次矩阵* @note* @param Mat& R 3*3旋转矩阵* @param Mat& T 3*1平移矩阵* @return Mat 4*4齐次矩阵**************************************************/Mat R_T2HomogeneousMatrix(const Mat& R, const Mat& T){ Mat HomoMtr; Mat_ R1 = (Mat_(4, 3) << (0, 0), (0, 1), (0, 2), (1, 0), (1, 1), (1, 2), (2, 0), (2, 1), (2, 2), 0, 0, 0); Mat_ T1 = (Mat_(4, 1) << (0, 0), (1, 0), (2, 0), 1); cv::hconcat(R1, T1, HomoMtr); //矩阵拼接 return HomoMtr;}/*************************************************** @brief 齐次矩阵分解为旋转矩阵与平移矩阵* @note* @param const Mat& HomoMtr 4*4齐次矩阵* @param Mat& R 输出旋转矩阵* @param Mat& T 输出平移矩阵* @return**************************************************/void HomogeneousMtr2RT(Mat& HomoMtr, Mat& R, Mat& T){ //Mat R_HomoMtr = HomoMtr(Rect(0, 0, 3, 3)); //注意Rect取值 //Mat T_HomoMtr = HomoMtr(Rect(3, 0, 1, 3)); //R_(R); //T_(T); /*HomoMtr(Rect(0, 0, 3, 3)).copyTo(R); HomoMtr(Rect(3, 0, 1, 3)).copyTo(T);*/ Rect R_rect(0, 0, 3, 3); Rect T_rect(3, 0, 1, 3); R = HomoMtr(R_rect); T = HomoMtr(T_rect);}/*************************************************** @brief 检查是否是旋转矩阵* @note* @param* @param* @param

cv::solvePnPRansac(board_points,image_corners,intrinsic_matrix,dist_coeffs,rvec,pnp_t); cv::Mat R_; cv::Rodrigues(rvec,pnp_R); std::cout<<" PNP R_ "<