Dobot magician + realsense D435i 手眼标定（外参）C++

之前写过一篇用python实现的，最近在ubuntu18.04弄了一个c++的，记录一下

一、pipeline:

1.下载aruco_ros、realsense-ros，打印aruco二维码，最好打印这个ros包里面的二维码，写好启动realsense和aruco的文件，直接通过获取ros消息获得二维码在相机坐标系里的位置，默认topic是“/aruco_tracker/position”；

2.dobot跑8个目标点，获得对应8个二维码坐标位置；

3.利用T*aruco_position=dobot_position, T=dobot_positon*inv(aruco_position)求出变换矩阵T。

二、过程中的坑

1.注意dobot运动到目标点后才读取aruco位置数据，需要对dobot运行的cmd消息队列中的index和完成的cmd index进行比较延时。一开始想着用dobot_ros ，调用服务的方式进行，不过太麻烦，改正直接用dobot的API，注意用得出的变化矩阵，通过相机得到的数据变换到dobot base基坐标下不是末端TCP点的位置，有个偏差，因为如图所示，二维码的中心点位置与吸盘有一定偏差，所以在后面用的时候主要把这个修正这个偏差。

2.ros中的消息可以按需求时再去订阅，不用循环订阅，代码如下

	aruco_position_msg = ros::topic::waitForMessage<geometry_msgs::Vector3Stamped >("/aruco_tracker/position",ros::Duration(5));
	aruco_position(0,i)=aruco_position_msg->vector.x;
    aruco_position(1,i)=aruco_position_msg->vector.y;
	aruco_position(2,i)=aruco_position_msg->vector.z;
	aruco_position(3,i)=1;

3.运行前，先获得串口权限

终端运行 sudo chmod a+rw /dev/ttyUSB0 , 后面的串口要根据自己的端口写。

三、完整代码

#include<iostream> 
#include<fstream>
#include<sstream>
#include<cmath> 
#include<cstdlib>
#include<vector>
#include <unistd.h>
#include<eigen3/Eigen/Core>
#include<eigen3/Eigen/SVD>
using namespace std;

#include "ros/ros.h"
//#include "std_msgs/String.h"
//#include "std_msgs/Float32MultiArray.h"
#include "geometry_msgs/Vector3Stamped.h"

#include "DobotDll.h"

//用eigen计算伪逆矩阵函数
template<typename _Matrix_Type_> 
_Matrix_Type_ pseudoInverse(const _Matrix_Type_ &a, double epsilon = 
    std::numeric_limits<double>::epsilon()) 
{  
    Eigen::JacobiSVD< _Matrix_Type_ > svd(a ,Eigen::ComputeThinU | Eigen::ComputeThinV);  
    double tolerance = epsilon * std::max(a.cols(), a.rows()) *svd.singularValues().array().abs()(0);  
    return svd.matrixV() *  (svd.singularValues().array().abs() > tolerance).select(svd.singularValues().array().inverse(), 0).matrix().asDiagonal() * svd.matrixU().adjoint(); 
}  



int main(int argc, char **argv)
{
   //connecte dobot
    if (argc < 2) {
        cout<<"Application portName"<<endl;
        return -1;
    }
    // Connect Dobot before start the service
    int result = ConnectDobot(argv[1], 115200, 0, 0);
    switch (result) {
    case DobotConnect_NoError:
        break;
    case DobotConnect_NotFound:
        cout<<"Dobot not found!"<<endl;
        return -2;
        break;
    case DobotConnect_Occupied:
        cout<<"Invalid port name or Dobot is occupied by other application!"<<endl;
        return -3;
        break;
    default:
        break;
    }

    //读取8个目标数据,xyzr数据
    std::cout << "-----------test----------- " <<std:: endl;
	std::ifstream in("/home/yu/test_ws/src/dobot/data/trainData.txt");//自己的路径，可以用相对路径
	std::string line;
   double  dobot_target_data[8][4];

	if (in.fail())
	{
		std::cout << "no file!" << std::endl;
		std::getchar();
		std::exit(0);
	}
	std::cout << "there is  file!" << std::endl;
	/*vector< vector<float> >trainData(8,vector<float>(4));*/
	int count = 0;
	int i = 0, j = 0;
	while (getline(in, line)) {
		std::stringstream ss(line);
		float x;
		while (ss >> x) {
			count++;
			if (count < 5)
			{
				 dobot_target_data[i][j] = x;
				std::cout<< dobot_target_data[i][j] <<"\t";
				j++;
			}
			
		}
		std::cout << std::endl;
		i++;
		j = 0;
		count = 0;
		
	}
	in.close();

      //初始化节点
	ros::init(argc, argv, "aruco_position_subscriber");
	ros::NodeHandle n;
	// ros::ServiceClient client;
	ros::Rate loop_rate(2);
    ROS_INFO("arocu_position_subscriber running.../n");

    boost::shared_ptr<geometry_msgs::Vector3Stamped const>  aruco_position_msg;
	Eigen::MatrixXd  aruco_position(4,8);
	Eigen::MatrixXd dobot_target_position(4,8);

     SetCmdTimeout(3000);
     SetQueuedCmdClear();
     SetQueuedCmdStartExec();

    uint64_t executedCmdIndex=0 ;
    uint64_t  queuedCmdIndex;
    PTPCmd  ptpcmd[8 ];
    WAITCmd cmd;
    cmd.timeout = 1500;
    for(i=0;i<8;i++)
    {
        ptpcmd[i].ptpMode = 1;
        ptpcmd[i].x = dobot_target_data[i][0] ;
        ptpcmd[i].y = dobot_target_data[i][1] ;
        ptpcmd[i].z = dobot_target_data[i][2] ;
        ptpcmd[i].r = dobot_target_data[i][3] ;

        SetPTPCmd(&ptpcmd[i], true, &queuedCmdIndex);
        while(executedCmdIndex != queuedCmdIndex)
        {
          
            GetQueuedCmdCurrentIndex(&executedCmdIndex);
            sleep(2);
        }
        SetWAITCmd(&cmd, true, &queuedCmdIndex);
        cout<<"if delay print?"<<i<<endl;

        //接收aruco位置数据，最后得到4*8的矩阵
		aruco_position_msg = ros::topic::waitForMessage<geometry_msgs::Vector3Stamped >("/aruco_tracker/position",ros::Duration(5));
		aruco_position(0,i)=aruco_position_msg->vector.x;
		aruco_position(1,i)=aruco_position_msg->vector.y;
		aruco_position(2,i)=aruco_position_msg->vector.z;
		aruco_position(3,i)=1;
		cout<<"aruco_position is "<<aruco_position(0,i)<<"\t"<<aruco_position(1,i)<<"\t"<<aruco_position(2,i)<<endl;
		// 把dobot运动的目标点数据改成4*8的matrix,
		dobot_target_position(0,i)=dobot_target_data[i][0]+30;//aruco position is differ 30MM from TCP
		dobot_target_position(1,i)=dobot_target_data[i][1];
		dobot_target_position(2,i)=dobot_target_data[i][2];
		dobot_target_position(3,i)=1;

    }

    SetQueuedCmdStopExec();
       // Disconnect Dobot
    DisconnectDobot();

   
    //计算AX=BX 求解变换矩阵
	Eigen::MatrixXd image_to_arm(4,4);
	Eigen::MatrixXd arm_to_image(4,4);
   
    image_to_arm = dobot_target_position* pseudoInverse(aruco_position);
	arm_to_image = pseudoInverse( image_to_arm);
    cout<<"image to arm matrix is "<<image_to_arm<<endl;
    cout<<"arm to image matrix is "<<arm_to_image<<endl;

    	// //验证变换矩阵准确度
	cout<<"test the result!!!!"<<endl<<"image_to_arm"<<endl;
	cout<<image_to_arm*aruco_position<<endl;
	cout<<"Expected:"<<endl<<dobot_target_position.transpose()<<endl;

    //  transition matrix of  image to base,保存变换矩阵到txe文档，方便后面用
    ofstream output_stream;
    string txt_path = "/home/yu/"; 
    string txt_name = "";
    
    txt_name = txt_path +  "handeye_param.txt";
    output_stream.open(txt_name);
    for(i=0;i<4;i++)//其中originFeat是一个vector<float>，
    {
        for(int j = 0;j<4;j++)
        {

            output_stream << image_to_arm (i,j);
            if(j<3)
                output_stream << "\t";
            else
                output_stream<<"\n";
        }
        
    }
    output_stream.close();



}