雷達座標變換及其相關運算

座標系變換

座標系變換包括：平移，旋轉。

旋轉矩陣

例如，繞X軸旋轉有：

\[ Y_L` = Y_Lcosa - Z_Lsina \\ Z_L' = Y_Lsina + Z_Lcosa \\ \]

轉換為矩陣形式

\[ \begin{bmatrix} X_L` \\ Y_L` \\ Z_L` \end{bmatrix} = \begin{bmatrix} 1 & 0 & 0 \\ 0 & cosa & -sina \\ 0 & sina & cosa \end{bmatrix} \begin{bmatrix} X_L \\ Y_L \\ Z_L \end{bmatrix} \]

同理，繞Y軸、X軸旋轉有：

\[\begin{bmatrix} X_L` \\ Y_L` \\ Z_L` \end{bmatrix} = \begin{bmatrix} cosb & 0 & sinb \\ 0 & 1 & 0 \\ -sinb & 0 & cosb \end{bmatrix} \begin{bmatrix} X_L \\ Y_L \\ Z_L \end{bmatrix} \]

\[\begin{bmatrix} X_L` \\ Y_L` \\ Z_L` \end{bmatrix} = \begin{bmatrix} cosy & -siny & 0 \\ siny & cosy & 0 \\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} X_L \\ Y_L \\ Z_L \end{bmatrix} \]

將上面三個矩陣相乘得到旋轉矩陣：

\[ R= \begin{bmatrix} cosbcosa & -sinacosb & sinb \\ sinasinbcosy + cosasiny & cosacosy-sinasinbsiny & -sinacosb \\ sinasiny+cosacosbcosy & cosasinbsiny + sinacosy & cosacosb \end{bmatrix} \]

平移矩陣

\[T = \begin{bmatrix} \Delta x \\ \Delta y \\ \Delta z \end{bmatrix} \]

最後，可以得到鐳射雷達座標與相機座標（世界座標系下）關係

\[ \begin{bmatrix} X_c` \\ Y_c` \\ Z_c` \\ 1 \end{bmatrix} = \begin{bmatrix} R & T \\ 0 & 1 \end{bmatrix} \begin{bmatrix} X_L` \\ Y_L` \\ Z_L` \\ 1 \end{bmatrix} \\ \]

例如下面是一個變換矩陣

0.029548,-0.999563,-0.00120571,-0.0189618
-0.00609057,0.00102618,-0.999981,0.181019
0.999545,0.0295548,-0.00605758,-0.0807626
0,0,0,1

將點雲投影至影像

以下是一段示例程式碼，可將鐳射雷達獲得點雲投影至影像每個畫素。

vector<vector<float>> DepthQueue::pushback(pcl::PointCloud<pcl::PointXYZ> &pc)
{
    if (this->processQueue.empty())
    {
        this->_initflag = true;
    }
    Matrix4Xf pc_Matrix = pc.getMatrixXfMap();
    int cols = pc_Matrix.cols();
    Matrix3Xf transformed_points = (this->E_0 * pc_Matrix).topRows(3);
    Matrix<float, 3, MaxPointsNum> pointsBox;       // 點雲矩陣
    Matrix<float, 1, MaxPointsNum> dptBox;          // 深度矩陣
    Matrix<int, 2, MaxPointsNum> ipBox;             // 投影至影像的點陣
    pointsBox.leftCols(cols) << transformed_points;
    dptBox.leftCols(cols) << transformed_points.row(2);
    ipBox << ((this->K_0 * pointsBox).array().rowwise() * (pointsBox.row(2).array().inverse())).topRows(2).matrix().cast<int>();
    auto inside_x = (ipBox.row(0).array() >= 0 && ipBox.row(0).array() < ImageW);
    auto inside_y = (ipBox.row(1).array() >= 0 && ipBox.row(1).array() < ImageH);
    ipBox.row(0) << (inside_x).select(ipBox.row(0), MatrixXf::Constant(1, MaxPointsNum, 0));    // x軸
    ipBox.row(1) << (inside_y).select(ipBox.row(1), MatrixXf::Constant(1, MaxPointsNum, 0));    // y軸
    this->processQueue.push(ipBox);
    // 將超出佇列大小的點雲全置零
    if (this->processQueue.size() > maxQueueSize)
    {
        Matrix<int, 2, MaxPointsNum> outpoints = this->processQueue.front();
        for (int i = 0; i < MaxPointsNum; ++i)
        {
            if (this->depth[outpoints(1, i)][outpoints(0, i)] == 0.)
                continue;
            this->depth[outpoints(1, i)][outpoints(0, i)] = 0.;
        }
        this->processQueue.pop();
    }
    for (int i = 0; i < MaxPointsNum; ++i)
    {
        if (dptBox(0, i) > 0)
        {
            if ((fabs(this->depth[ipBox(1, i)][ipBox(0, i)]) > Epsilon && dptBox(0, i) < this->depth[ipBox(1, i)][ipBox(0, i)]) || fabs(this->depth[ipBox(1, i)][ipBox(0, i)]) < Epsilon)
                this->depth[ipBox(1, i)][ipBox(0, i)] = dptBox(0, i);
                // depth[y][x] -> 影像中x，y點的深度。
        }
        else
            break;
    }
    return this->depth;
}