2D&3D融合

概述

截止目前为止，我们学习了机器人学，学习了2D和3D视觉算法。我们也学习了2D相机(图像数据的来源)和3D相机(点云数据的来源)工作原理。

实际上，我们最终要做的，是一个手眼机器人系统。在这个系统里，相机与机器人构成了两个非常关键的部分，它们之间需要密切配合，因此，它们之间的关系，也就非常重要。确定相机与机器人之间的关系，这是手眼标定要解决的问题。

另一方面，在很多场合，为了增强算法的鲁棒性，我们通常同时使用图像数据与点云数据，这又涉及到2D与3D配准的问题。

相机配准

由之前学习的相机内参相关知识：

$$Z_c\begin{bmatrix} u \\ v \\ 1 \end{bmatrix} = \begin{bmatrix} f_x & 0 & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} X_c \\ Y_c \\ Z_c \end{bmatrix}$$

可得：

$Z_c \cdot u = f_x\cdot X_c + c_x \cdot Z_c$

$Z_c \cdot v = f_y\cdot Y_c + c_y \cdot Z_c$

从而可以将图像$(u, v)$位置的点转换到相机坐标系$(X_c, Y_c)$：

$X_c = \dfrac {(u - c_x) \cdot Z_c} {f_x}$

$Y_c = \dfrac {(v - c_y) \cdot Z_c} {f_y}$

将彩色图和深度图合并成点云

• 方式一：

通过双重循环遍历

/**
     * 将彩色图和深度图合并成点云
     * @param matrix 相机内参矩阵3x3
     * @param rgb    彩色图
     * @param depth  深度图
     * @param cloud  输出点云
     */
static void convert(Mat &matrix, Mat &rgb, Mat &depth, PointCloud::Ptr &cloud) {
    double camera_fx = matrix.at<double>(0, 0);
    double camera_fy = matrix.at<double>(1, 1);
    double camera_cx = matrix.at<double>(0, 2);
    double camera_cy = matrix.at<double>(1, 2);

    cout << "fx: " << camera_fx << endl;
    cout << "fy: " << camera_fy << endl;
    cout << "cx: " << camera_cx << endl;
    cout << "cy: " << camera_cy << endl;

    // 遍历深度图
    for (int v = 0; v < depth.rows; v++)
        for (int u = 0; u < depth.cols; u++) {
            // 获取深度图中(m,n)处的值
            ushort d = depth.ptr<ushort>(v)[u];
            // d 可能没有值，若如此，跳过此点
            if (isnan(d) && abs(d) < 0.0001)
                continue;
            // d 存在值，则向点云增加一个点
            PointT p;

            // 计算这个点的空间坐标
            p.z = double(d) / 1000; //单位是米
            p.x = (u - camera_cx) * p.z / camera_fx;
            p.y = (v - camera_cy) * p.z / camera_fy;

            // 从rgb图像中获取它的颜色
            // rgb是三通道的BGR格式图，所以按下面的顺序获取颜色
            Vec3b bgr = rgb.at<Vec3b>(v, u);
            p.b = bgr[0];
            p.g = bgr[1];
            p.r = bgr[2];

            // 把p加入到点云中
            cloud->points.push_back(p);
            //cout << cloud->points.size() << endl;
        }


    // 设置并保存点云
    cloud->height = 1;
    cloud->width = cloud->points.size();
    cout << "point cloud size = " << cloud->points.size() << endl;
    cloud->is_dense = false;
}
int main(){
    cv::Mat cameraMatrix; // 从文件加载相机内参
    cv::Mat rgb;         // 从相机得到RGB彩色图
    cv::Mat depth;       // 从相机得到depth深度图
    PointCloud::Ptr pCloud = PointCloud::Ptr(new PointCloud);
    convert(cameraMatrix, rgb, depth, pCloud);
}

• 方式二：

通过指针遍历，并提前准备好计算矩阵

#include <iostream>
#include <opencv2/opencv.hpp>
#include <sstream>
#include <cstdlib>
#include <pcl/io/io.h>

using namespace std;
using namespace cv;

float qnan_ = std::numeric_limits<float>::quiet_NaN();
const char *cameraInCailFile = "./assets/3DCameraInCailResult.xml";

Eigen::Matrix<float, 1920, 1> colmap;
Eigen::Matrix<float, 1080, 1> rowmap;
//const short w = 512, h = 424;
const short w = 1920, h = 1080;

void prepareMake3D(const double cx, const double cy,
                   const double fx, const double fy) {
    float *pm1 = colmap.data();
    float *pm2 = rowmap.data();
    for (int i = 0; i < w; i++) {
        *pm1++ = (i - cx + 0.5) / fx;
    }
    for (int i = 0; i < h; i++) {
        *pm2++ = (i - cy + 0.5) / fy;
    }
}
/**
 * 根据内参，合并RGB彩色图和深度图到点云
 * @param cloud
 * @param depthMat
 * @param rgbMat
 */
void getCloud(pcl::PointCloud<pcl::PointXYZRGB>::Ptr &cloud, Mat &depthMat, Mat &rgbMat) {
    const float *itD0 = (float *) depthMat.ptr();
    const char *itRGB0 = (char *) rgbMat.ptr();

    if (cloud->size() != w * h)
        cloud->resize(w * h);


    pcl::PointXYZRGB *itP = &cloud->points[0];
    bool is_dense = true;

    for (size_t y = 0; y < h; ++y) {

        const unsigned int offset = y * w;
        const float *itD = itD0 + offset;
        const char *itRGB = itRGB0 + offset * 4;
        const float dy = rowmap(y);

        for (size_t x = 0; x < w; ++x, ++itP, ++itD, itRGB += 4) {
            const float depth_value = *itD / 1000.0f;

            if (!isnan(depth_value) && abs(depth_value) >= 0.0001) {

                const float rx = colmap(x) * depth_value;
                const float ry = dy * depth_value;
                itP->z = depth_value;
                itP->x = rx;
                itP->y = ry;

                itP->b = itRGB[0];
                itP->g = itRGB[1];
                itP->r = itRGB[2];
            } else {
                itP->z = qnan_;
                itP->x = qnan_;
                itP->y = qnan_;

                itP->b = qnan_;
                itP->g = qnan_;
                itP->r = qnan_;
                is_dense = false;
            }
        }
    }
    cloud->is_dense = is_dense;
}

int main(){
    Mat cameraMatrix = cv::Mat_<double>(3, 3);
    FileStorage paramFs(cameraInCailFile, FileStorage::READ);
    paramFs["cameraMatrix"] >> cameraMatrix;
    // 内参数据
    double fx = cameraMatrix.at<double>(0, 0);
    double fy = cameraMatrix.at<double>(1, 1);
    double cx = cameraMatrix.at<double>(0, 2);
    double cy = cameraMatrix.at<double>(1, 2);
    // 提前准备计算所需参数
    prepareMake3D(cx, cy, fx, fy);

    cv::Mat rgbMat;      // 从相机得到RGB彩色图
    cv::Mat depthMat;        // 从相机得到depth深度图
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>());
    getCloud(cloud, depthMat, rgbMat)
}