（一）面扫描仪与机械臂的手眼标定（眼在手上）

最新推荐文章于 2025-09-02 16:17:31 发布

原创

最新推荐文章于 2025-09-02 16:17:31 发布 · 2.6k 阅读

6 ·

CC 4.0 BY-SA版权

文章标签：

#opencv #计算机视觉 #c++

本文档介绍了一个使用opencv和Eigen库进行手眼标定的C++程序。首先配置opencv和Eigen库，然后获取点在标定板和相机坐标系中的坐标。通过读取不同位姿下的坐标数据，计算标定板到相机的旋转和平移矩阵，接着求解机械臂末端到基底的转换。最后，利用opencv的calibrateHandEye函数求解相机到机械臂末端的转换矩阵，并进行了结果测试。

1、配置opencv库与Eigen库
需用到opencv中求解手眼标定的算法
需用到Eigen中SVD分解算法
2、获取点
点相对于标定板坐标系的坐标
30 30 0
30 60 0
30 150 0
60 30 0
60 150 0
150 30 0
150 60 0
150 150 0
存为board1.txt文件

点相对于相机坐标系的坐标
63.5676 355.2545 81.8691
32.552 356.6499 79.4237
-56.0291 359.4486 74.1348
66.4339 356.5228 50.7242
-53.22 360.1268 42.9512
71.6217 356.7818 -37.7328
40.2484 357.923 -40.4321
-48.236 360.6457 -45.6185
存为camera1.txt文件

改变机械臂位姿，再获取点相对于相机坐标系的坐标
存为camera2.txt
多次循环，获取多组数据

3、求解程序

#include <vector>
#include <opencv2/opencv.hpp>
#include <iostream>
#include <math.h>
#include <map>
#include <fstream>
#include <sstream>
#include <Eigen/SVD>
#include <Eigen/Core>
#include<opencv2/core/eigen.hpp>


using namespace std;
using namespace cv;
using namespace Eigen;



//机械臂末端位姿,x,y,z,rx,ry,rz
Mat_<double> ToolPose = (cv::Mat_<double>(7, 6) <<
	350, 80, 480, 0, 178, -86,
	355, 55, 500, 0, 184, -94,
	365, 65, 490, 0, 179, -82,
	385, 90, 465, 0, 183, -96,
	385, 70, 485, 0, 181, -94,
	370, 80, 470, 0, 178, -80,
	365, 85, 480, 0, 182, -82);
	



class coordinate {
   
   
public:
	double x;
	double y;
	double z;
};

//求解标定板到相机的旋转矩阵和平移矩阵
void Solve_board2camera(int num, Mat& T_board2camera, Mat& R_board2camera);
//求解机械臂末端到基底的旋转矩阵和平移矩阵
void Solve_gripper2base(const Mat& m, Mat& T_gripper2base, Mat& R_gripper2base);
//求解欧拉角转旋转矩阵
Mat eulerAngleToRotateMatrix(const Mat& eulerAngle);
//测试
void testResult(Mat& R_camera2gripper, Mat& T_camera2gripper, Mat& tempH_T);

vector<Mat> v_H_board2camera, v_H_camera2gripper, v_H_gripper2base;
vector<Mat> v_Pc; //存储相机中点坐标


int main()
{
   
   
	cv::Mat T_gripper2base;
	cv::Mat R_gripper2base;
	cv::Mat H_gripper2base = Mat::eye(4, 4, CV_64FC1);
	cv::Mat T_board2camera;
	cv::Mat R_board2camera;
	cv::Mat H_board2camera = Mat::eye(4, 4, CV_64FC1);
	cv::Mat T_camera2gripper;
	cv::Mat R_camera2gripper;
	cv::Mat H_camera2gripper = Mat::eye(4, 4, CV_64FC1);

	cv::Mat tempR, tempT, tempH;

	vector<Mat> v_T_gripper2base;
	vector<Mat> v_R_gripper2base;
	vector<Mat> v_T_board2camera;
	vector<Mat> v_R_board2camera;


	//计算board2camera
	//计算板到相机的转换关系
	int num = 0;
	for (int i = 0; i < ToolPose.rows; i++)
	{
   
   
		num = i + 1;//用于打开文件

		//求解标定板到相机的旋转矩阵和平移矩阵
		Solve_board2camera(num, T_board2camera, R_board2camera);
		tempT = T_board2camera.clone();
		tempR = R_board2camera.clone();

		//存储标定板到相机的旋转矩阵和平移矩阵
		v_T_board2camera.push_back(tempT);
		v_R_board2camera.push_back(tempR);

		//存储转换矩阵
		tempR({
   
    0,0,3,3 }).copyTo(H_board2camera({
   
    0,0,3,3 }));
		tempT({
   
    0,0,