标签：begin end 运算 变换 ipBox depth 坐标 bmatrix Matrix

坐标系变换

坐标系变换包括：平移，旋转。

旋转矩阵

例如，绕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;
}

标签：begin,end,运算,变换,ipBox,depth,坐标,bmatrix,Matrix
From： https://www.cnblogs.com/AlanLeeee/p/18192369