相機標定(三)——手眼標定

相機標定(一)——內參標定與程序實現

相機標定(二)——圖像座標與世界座標轉換

相機標定(三)——手眼標定

一、簡述

手眼標定目的在於實現物體在世界座標系和機器人座標系中的變換。

在標定時，一般在工作平面設置一個世界座標系，該座標系與機器人座標系不重合，在完成相機的內外參標定後，可計算獲得物體在世界座標系中的位置。若需要機器人與視覺聯動，需要獲得物體在在機器人座標系中的座標。

二、實現步驟

1. 通過張正友法標定相機的內參矩陣和畸變參數；(相機標定(一)——內參標定與程序實現)
2. 標定相機外參矩陣，用於圖像座標與世界座標的轉換；(相機標定(二)——圖像座標與世界座標轉換)
3. 設置N個特徵點($N>3$)，計算其世界座標，移動機械臂工作末端到特徵點，記錄末端座標，獲得N組數據；
4. 計算兩組數據的$R$和$t$，其中特徵點世界座標爲A組數據，末端座標爲B組數據；

三、 原理

3.1 推導

參考：計算兩個對應點集之間的旋轉矩陣R和轉移矩陣T

假設有兩個點集$A$和$B$，且這兩個點集合的元素數目相同且一一對應。爲了尋找這兩個點集之間的旋轉矩陣$R$和平移矩陣$t$。可以將這個問題建模成如下的公式：
$B=R*A+t$
求解步驟

• 計算點集合的中心點
• 將點集合移動到原點，計算最優旋轉矩陣$R$
• 計算轉移矩陣$t$

求解

1. 旋轉矩陣$R$

• 計算中心點

$P_A = \left[ \begin{matrix} x_A\\ y_A \\ z_A\end{matrix} \right], P_B = \left[ \begin{matrix} x_B\\ y_B \\ z_B\end{matrix} \right]\\ \mu_A = \frac{1}{N} \sum_{i=1}^{N} P_{A}^i, \mu_B = \frac{1}{N} \sum_{i=1}^{N} P_{B}^i$

注意：$P_{A}^i$，$P_{B}^i$，$\mu_A$和$\mu_B$爲向量

• 點集重新中心化

$A'_i = \{ P_A^i-\mu_A\} \\ B'_i = \{ P_B^i-\mu_B \}$

• 計算點集之間的協方差矩陣$H$

$H = \sum_{i=1}^{N}A_{i}^{'} {B_{i}^{'}}^T = \sum_{i=1}^{N} (P_A^i-\mu_A)(P_B^i-\mu_B)^T$

• 通過奇異值分解計算最優旋轉矩陣

$\left[ U,S,V\right] = SVD(H) \\ R = VU^T$

2. 平移矩陣$t$

$t = -R\times \mu_A + \mu_B$

3.2 補充知識

1. 協方差

協方差（Covariance）是一種用來度量兩個隨機變量關係的統計量，定義爲：
$cov(A,B) = \frac{1}{N-1}\sum_{i=1}^{N}(A_i-\mu_A)*(B_i-\mu_B)$
其中$\mu_A$，$\mu_B$分別爲$A$，$B$的均值

2. 奇異值分解(SVD,Singular Value Decomposition)

奇異值分解是一個能適用於任意的矩陣的一種分解的方法，公式爲：
$A = U\Sigma V^T$
幾何含義：對於任意一個矩陣，找到一組兩兩正交單位向量序列，使得矩陣作用在此向量序列上後得到新的向量序列保持兩兩正交。

四、程序實現

• 求解變換矩陣

bool RtbySVDSrv( vector<Eigen::Vector3d> worldPoints, vector<Eigen::Vector3d> robotPoints,Eigen::Vector3d &t,Eigen::Matrix3d &R, Eigen::Quaterniond &q) {

  // check data
  if (worldPoints.size() != robotPoints.size() || worldPoints.size() < 3)
	  return false;

  // save data
  int size = worldPoints.size();

  // count centre points
  Eigen::Vector3d worldCentre, robotCentre;
  for (int i = 0; i < size; i++) {
    worldCentre += worldPoints[i];
    robotCentre += robotPoints[i];
  }
  worldCentre /= size;
  robotCentre /= size;

  // count the vector
  vector<Eigen::Vector3d> worldVectors(size), robotVectors(size);
  for (int i = 0; i < size; i++) {
    worldVectors[i] = worldPoints[i] - worldCentre;
    robotVectors[i] = robotPoints[i] - robotCentre;
  }

  // count H
  Eigen::Matrix3d H;
  for (int i = 0; i < size; i++) {
    H += worldVectors[i] * robotVectors[i].transpose();
  }

  // svd count R and Q
  Eigen::JacobiSVD<Eigen::MatrixXd> svd(H, Eigen::ComputeThinU |
                                               Eigen::ComputeThinV);
  Eigen::Matrix3d V = svd.matrixV(), U = svd.matrixU();
  R = V * U.transpose();

  if (R.determinant() < 0)
    R *= -1;
  q = Eigen::Quaterniond(R);
  q.normalize();

  // count t
  t = robotCentre - R * worldCentre;
    
  return true;
}

• 座標變換

void worldToRobot(Eigen::Vector3d& worldPoint, Eigen::Vector3d& robotPoint,  Eigen::Matrix3d R, Eigen::Vector3d t)
{
	robotPoint = R* worldPoint + t;
}

參考

計算兩個對應點集之間的旋轉矩陣R和轉移矩陣T

Finding optimal rotation and translation between corresponding 3D points

奇異值的物理意義是什麼？