从手眼标定到三维重建

本文部分图片以及资料参考以下博客，感谢原文作者！
机器人位姿描述与坐标变换
结构光视觉传感器的标定

一、概述

手眼标定是指计算机械手和相机之间的转换关系。具体来说，就是相机坐标系和机械手末端坐标系（后面称为工具坐标系）之间的旋转和平移矩阵。

从相机的安装方式来说，可以分为“眼在手上”和“眼在手外”两种方式，区别在于相机是否安装在机械手末端。“眼在手外”是指相机和机械手分离安装，相机固定安装在机械手本体以外某处，而“眼在手上”是指相机安装在机械手末端，一般跟随机械手一起运动拍照。

下面我们要讨论的“三维重建”是基于“眼在手上”的这种方式，让机械手带动一款3D相机从不同位置对一个静止的目标物体进行拍照，然后将不同位置下拍摄的局部点云拼接在一起，合并成一个完整的目标物体点云，达到重建的目的。由于每个拍照位置就是一个新的相机坐标系，所以相机输出的“局部点云”是分散在不同相机坐标系下的，无法直接拼接。但无论从哪个视角拍照，目标物体在机械手”基坐标系“下的坐标都是唯一确定的，如果我们能有一种办法，将不同相机坐标系下的”局部点云“均转化到”基坐标系“下，再进行拼接（即点云相加），那么不就完成了重建的任务吗？具体原理和实现过程将在下文说明。

注：有的读者可能还不清楚“相机坐标系”、“工具坐标系”和“基坐标系”的定义，以及旋转平移矩阵的含义，后文会进行介绍，这里可能会对上文阅读产生一定障碍。

二、原理

2.1 手眼标定

相机坐标系是三维的，以光心为原点，以光轴为Z轴，分别以平行感光芯片的长宽方向为XY轴。工具坐标系在机械手末端，以法兰盘中心点为原点（法兰盘就是机械手末端有个像圆盘一样的东西，可以在上面安装各种工具，比如焊枪、夹爪），XY轴在法兰盘平面上，Z轴垂直于法兰盘平面。正如概述所说，手眼标定是为了求取相机坐标系和工具坐标系之间的旋转平移矩阵，如下图所示。

该矩阵由一个3 * 3的旋转矩阵和一个3 * 1平移向量组成，上图是它们的齐次坐标形式。

旋转矩阵表示相机坐标系到工具坐标系之间的旋转关系，假设O-X’Y’Z’表示相机坐标系，O-XYZ表示工具坐标系，那么旋转矩阵的第一列表示相机坐标系的X轴与工具坐标系三个轴所成角度的余弦值，第二列和第三列分别代表Y、Z轴角度分量。

平移向量可以理解为两个坐标系原点的距离，也等价于其中一个坐标系的原点在另外一个坐标系里的坐标。计算相机坐标系到工具坐标系的转换矩阵T1可以使用opencv里的算子：estimateAffine3D，得到T1以后，已知某点P在相机坐标系下的坐标，将该点坐标左乘T1便得到该点在工具坐标系下的坐标。又因为从工具坐标系到基坐标系下的转换矩阵T2是可以通过机器人（即机械手）反馈得到，那么我们就可以计算出点P在基坐标系下的坐标P’。

P’ = T2 * T1 * P

下面我将详细说明如何求取T2，即工具坐标系到基坐标系的转化矩阵。

2.2 三维重建
基坐标系在机器人的基座上，在机器人运动过程中基座保持静止，目标物体相对基座系静止不动。在机器人运动过程中，会实时输出末端的位姿，即法兰盘中心点在基坐标系下的位姿（X Y Z R P Y），XYZ分别表示中心点在基坐标系下的三轴坐标，RPY是欧拉角的三个分量（Roll、Pitch、Yaw)。可以由这6个数值计算出工具坐标系到基坐标系的转换矩阵T2。具体计算方法如下。(注：代码只供参考）

 Eigen::Isometry3d T;
 T = Eigen::Isometry3d::Identity();
 Eigen::Matrix3d rotation_matrix;
 Eigen::Vector3d eulerAngle(pose[3],pose[4],pose[5]);
 Eigen::AngleAxisd rollAngle1(eulerAngle(0) * -1 / 180.0 * PI,Eigen::Vector3d::UnitZ());
 Eigen::AngleAxisd pitchAngle1(eulerAngle(1) / 180.0 * PI,Eigen::Vector3d::UnitY());
 Eigen::AngleAxisd yawAngle1(eulerAngle(2) / 180.0 * PI,Eigen::Vector3d::UnitX());
 rotation_matrix = yawAngle1 * pitchAngle1 * rollAngle1;
 T.rotate(rotation_matrix);
 T.pretranslate( Eigen::Vector3d( pose[0], pose[1], pose[2] ) 

三、关键代码

注：下面只展示了关键代码，具体实现还是要自己写的，尤其是手眼标定，相机和机器人之间的转换关系需要根据自己的实际情况来进行标定得出，代码只供参考。

3.1 手眼标定（计算T1)

 
cv::Mat Calibrate::GetAffineMartix() {
  cv::Mat inliers;
  // 点云单位换算，将m换算成mm
  for (int i = 0; i < src_points_.size(); i++) {
    src_points_[i].x *= 1000;
    src_points_[i].y *= 1000;
    src_points_[i].z *= 1000;
  }
  cv::estimateAffine3D(src_points_,dst_points_,transfrom_martix,inliers);
  return transfrom_martix;
}

3.2 三维重建（计算T2）

 
void Calibrate::AffineCloud(pcl::PointCloud::Ptr src_cloud,float pose[6],pcl::PointCloud::Ptr dst_cloud){
  cv::Mat r_mat,t_mat,src_mat,dst_mat;
  r_mat = (cv::Mat_(3,3) << 
  transfrom_martix.at(0,0), transfrom_martix.at(0,1), transfrom_martix.at(0,2),
  transfrom_martix.at(1,0), transfrom_martix.at(1,1), transfrom_martix.at(1,2),
  transfrom_martix.at(2,0), transfrom_martix.at(2,1), transfrom_martix.at(2,2));
  t_mat = (cv::Mat_(3,1) << transfrom_martix.at(0,3), transfrom_martix.at(1,3), transfrom_martix.at(2,3));

  Eigen::Isometry3d T;
  T = Eigen::Isometry3d::Identity();
  Eigen::Matrix3d rotation_matrix;
  Eigen::Vector3d eulerAngle(pose[3],pose[4],pose[5]);
  Eigen::AngleAxisd rollAngle1(eulerAngle(0) * -1 / 180.0 * PI,Eigen::Vector3d::UnitZ());
  Eigen::AngleAxisd pitchAngle1(eulerAngle(1) / 180.0 * PI,Eigen::Vector3d::UnitY());
  Eigen::AngleAxisd yawAngle1(eulerAngle(2) / 180.0 * PI,Eigen::Vector3d::UnitX());
  rotation_matrix = yawAngle1 * pitchAngle1 * rollAngle1;
  T.rotate(rotation_matrix);
  T.pretranslate( Eigen::Vector3d( pose[0], pose[1], pose[2] ) );

  for (int i = 0; i < src_cloud->points.size(); i++) {
    pcl::PointXYZRGB dst_point;
    src_mat = (cv::Mat_(3,1) << src_cloud->points[i].x * 1000, src_cloud->points[i].y * 1000, src_cloud->points[i].z * 1000);
    dst_mat = r_mat * src_mat + t_mat;
    Eigen::Vector3d point;
    point[0] = dst_mat.at(0,0);
    point[1] = dst_mat.at(1,0);
    point[2] = dst_mat.at(2,0);
    Eigen::Vector3d point_robot =  T * point;
    dst_point.x = point_robot[0];
    dst_point.y = point_robot[1];
    dst_point.z = point_robot[2];
    dst_point.r = src_cloud->points[i].r;
    dst_point.g = src_cloud->points[i].g;
    dst_point.b = src_cloud->points[i].b;
    dst_cloud->points.push_back(dst_point);
  }
}