采集数据格式

T_t_cal: transform from robot tool to calibration board (fixed)
T_b_t: transform from robot base to tool
T_c_cal: transform from camera to calibration board
T_b_c: transform from robot base to camera (fixed). You need to calibrate this transform
Data is in the form of [x, y, z, ox, oy, oz, ow], position + quaternion

这里我们实现的是hand to eye 标定，给定机器人基座到末端，相机到标定板的两组数据（数据集大小为30），求解从基座到相机坐标系的转化。

数据格式做适当转换便于Eigen处理

这里分为两步，首先读取csv格式的数据，将30组数据存到307矩阵里面，然后将每一行转化为一个44齐次矩阵，最终单个数据集（eg.相机到标定板变换）以 std::vector<Eigen::MatrixXd>存储。
这里直接贴上代码

1.读取csv数据

int CsvRead(MatrixXd& outputMatrix, const string& fileName, const streamsize dPrec) {
    
  ifstream inputData;
  inputData.open(fileName);
  cout.precision(dPrec); //设置输出精度
  if (!inputData)
    return -1;
  string fileline, filecell;
  unsigned int  noOfRows = 0, noOfCols = 0;
  //确定输入矩阵行列
    while (getline(inputData, fileline)) {
        noOfCols = 0;
        stringstream linestream(fileline);
        while (getline(linestream, filecell, ',')) {
            try {
                stod(filecell);
            }
            catch (...) {
                return -1;
            }
            noOfCols++;
        }
        noOfRows++;
    }
    inputData.close();
    outputMatrix.resize(noOfRows, noOfCols);
    inputData.open(fileName);
    noOfRows = 0;
    while (getline(inputData, fileline)) {
        noOfCols = 0;
        stringstream linestream(fileline);
        while (getline(linestream, filecell, ',')) {
            outputMatrix(noOfRows, noOfCols++) = stod(filecell);
        }
        noOfRows++;
    }
    return 0;
}

上面读取csv的代码块，参考了网上的，比较好的方面是做了一些异常判断，比如关于文件的读取以及字符转化是否成功。但是不好的方面是做了两次大循环，第一次循环仅仅为了判断输出矩阵大小，第二次才是将数据写入矩阵。这样在时间效率上我觉得不行。

自己最早写的一个读取csv的代码块，出现了一些暂时还不能理解的错误，思路是将csv所有数据存在一个vector里面，然后通过MatrixXd::Map转化，我觉得这个思路比上述读取csv更加简单明了，但是当我输出读取的数据，发现并没有按照我预想的格式存储。也将代码贴在这，望各位大佬指正。

Eigen::MatrixXd csv2matrix(std::string path) { //path csv文件路径
    std::ifstream fin;
    fin.open(path);
    std::string line;
    vector<double>values;
    int rows = 0;
    while (getline(fin,line))
    {
        stringstream ss(line);
        std::string cell;
        while (getline(ss, cell, ',')) {    
            double value = stod(cell);
            values.push_back(value);
        }
        rows++;
    }
    MatrixXd re = MatrixXd::Map(&values[0], rows, values.size() / rows);
    cout << re.size() << endl;
    return re;
}

2.将四元数转化为旋转矩阵

这个没有太多要讲的，直接上代码

void Vector2Homogeneous(MatrixXd source, poses&target)
{
    for (int i = 0; i < source.rows(); i++) {
        Eigen::Vector3d translation_base = source.block<1, 3>(i, 0);
        Eigen::VectorXd rotation_base = source.block<1, 4>(i, 3);
        Eigen::Quaterniond q_rotaion_base(rotation_base[3], rotation_base[0], rotation_base[1], rotation_base[2]);
        Eigen::Matrix3d Rotation_base = q_rotaion_base.matrix();
        Eigen::Matrix4d T = Eigen::Matrix4d::Identity();
        T.block<3,3>(0,0)=Rotation_base;
        T.block<3,1>(0,3)=translation_base;
        target.push_back(T);
    }
}

需要注意几点：

• pose 为我自己定义的数据类型 typedef vector<MatrixXd>poses;
• 通过传引用形式实现输出target
• 我们采集到的四元数格式为x,y,z,w，Eigen里面四元数构造格式为w x y z

复现Tsai论文，求解AX=XB

对于论文的理解，我有个过程，并不是一步到位的。在A,B矩阵，以及(+)*()=(-)里面旋转轴相加的含义都有过误解。

A,B对应的是两次运动中，末端到末端，相机到相机之间的变换矩阵。这个在hand to eye，hand in eye求解时候有细微差别。

if (t == EyeToHand)
            {
                MatrixXd A = vH_robot[j] * vH_robot[i].inverse();//vH_robot:base to  tool
                MatrixXd B = vH_mark[j] * vH_mark[i].inverse();//vH_mark:camera to cal   

                vA.push_back(A);
                vB.push_back(B);
            }
  else if (t == EyeInHand)
            {

                MatrixXd A = vH_robot[j].inverse() * vH_robot[i];
                MatrixXd B = vH_mark[j] * vH_mark[i].inverse();

                vA.push_back(A);
                vB.push_back(B);

旋转轴相加并不是对应旋转矩阵的乘法，上述的等式在论文里面有详细推导，这个是基于矢量的封闭运算。具体还是要详细阅读论文。

这里贴出代码块，全部代码参见我的github

MatrixXd Solve_Axxb(const poses AA,const poses BB) {
    Matrix4d Hx = Matrix4d::Identity(); //待求解标定矩阵
    int num = AA.size();//观察数据组个数
    cout << "数据对个数:" << num << endl;
    //MatrixXd output=MatrixXd::Identity();
    MatrixXd A = MatrixXd::Zero(num * 3, 3); // solve Ax=B for rotation
    VectorXd B = MatrixXd::Zero(num * 3, 1);
    //solve rotation
    for (size_t i = 0; i < AA.size(); i++) {
        //将旋转矩阵转化为旋转轴形式
        AngleAxisd rgij, rcij;
        rgij.fromRotationMatrix(AA[i].block<3,3>(0,0));
        rcij.fromRotationMatrix(BB[i].block<3,3>(0,0));
        double theta_gij = rgij.angle();
        double theta_cij = rcij.angle();
        Vector3d rngij = rgij.axis();
        Vector3d rncij = rcij.axis();
        AngleAxisd Pgij(2*sin(theta_gij / 2),rngij);
        AngleAxisd Pcij (2 * sin(theta_cij / 2),rncij);
        
        Vector3d pgij_vector = Pgij.axis()*Pgij.angle();
        Vector3d pcij_vector = Pcij.axis()*Pcij.angle();
        Vector3d vec_sum_axis = pgij_vector + pcij_vector;
        Matrix3d S = skew(vec_sum_axis);
        Vector3d vec_sub_axis = pcij_vector - pgij_vector;
        A.block<3, 3>(3 * i, 0) = S;
        //cout << A.block<3, 3>(3 * i, 0) << endl;
        B.block<3, 1>(3 * i, 0) = vec_sub_axis;
    }
    //求解旋转
    VectorXd Pce1 = SvdInverse(A)*B;
    VectorXd Pce = 2 * Pce1 / sqrt(1 + Pce1.norm()*Pce1.norm());
    Matrix3d I=Matrix3d::Identity();
    double np2 = Pce.norm()*Pce.norm();
    MatrixXd Rx = (1 - np2 * 0.5)*I + 0.5*(Pce*Pce.transpose() + sqrt(4 - np2)*skew(Pce));
    cout<<"旋转矩阵是否正交:" <<Rx * Rx.transpose()<<endl;
    //求解平移
    A.setZero();
    B.setZero();
    for (size_t i = 0; i < AA.size(); i++) {
        Vector3d T_A, T_B;
        T_A = AA[i].block<3, 1>(0, 3);
        T_B = BB[i].block<3, 1>(0, 3);
        MatrixXd R_A = AA[i].block<3, 3>(0, 0);
        A.block<3, 3>(3 * i, 0) = R_A - I;
        B.block<3, 1>(3 * i, 0) = Rx*T_B-T_A;
    }
    VectorXd Tx = SvdInverse(A)*B;
    Hx.block<3, 3>(0, 0) = Rx;
    Hx.block<3, 1>(0, 3) = Tx;
    return Hx;

总结

网上大多教程都语焉不详，这种时候还是需要阅读原论文

参考

1.A New Technique for Fully Autonomous and
Efficient 3D Robotics Hand/Eye Calibration
2.码云