相机标定及点云拼接

前言

在上一篇中已经完成了从图像到点云的转换，但是只针对单个相机，在这一篇中将再进一步，从两个相机拍摄图像，然后做点云的生成及拼接。

一、相机标定原理

相机标定的原理我们上一篇章中也提到了一些（内参矩阵、外参矩阵），而相机标定需要用到的信息包含有内部参数（焦距、光学中心等）、外部参数（旋转、平移矩阵）以及畸变系数。

其中畸变主要指的是径向畸变和切向畸变，径向畸变导致直线呈曲线状，距离图像中心越远，径向失真越大；切向畸变发生是因为摄像透镜没有与成像平面完全平行，图像中的某些区域看起来可能比预期的要近。

更为详细的原理步骤叙述可以参考：https://blog.csdn.net/lql0716/article/details/71973318。

二、关键代码

2.1 相机标定代码1

因为Azure-Kinect-Sensor-SDK中有提供Kinect标定代码，这里可以直接参考。

这里就简单说一下大致步骤：
(1) 使用相机在近距离、远距离和多个指向角度拍摄标定板图像
运行以下命令以捕获相机0的视频：

k4arecorder --device 0 -c 3072p -d PASSIVE_IR -l 20 ./device0/device0.mkv

运行以下命令以捕获相机1的视频：

k4arecorder --device 1 -c 3072p -d PASSIVE_IR -l 20 ./device1/device1.mkv

(2) 使用ffmpeg获取视频帧:

ffmpeg -i ./device0/device0.mkv -vf fps=1 ./device0/color_%d.jpg
ffmpeg -i ./device1/device1.mkv -vf fps=1 ./device1/color_%d.jpg

(3) 从每个相机拍摄的图像中产生标定文件calib.yaml：

python calibrate.py --dataset-dir ./device0 --template ./PlaneFiles/plane.json
python calibrate.py --dataset-dir ./device1 --template ./PlaneFiles/plane.json

(4) 同时拍摄固定形态的标定板（一个相机一张即可），产生双目标定的信息calibration_blob.json：

python register.py --img-a  <full_path_img_b> --img-b <full_path_img_b> --template ./PlaneFiles/plane.json --calib-a <full_path_calibration_a> --calib-b <full_path_calibration_b> --out-dir <out_dir>

2.2 相机标定代码2

还可以利用Matlab相机标定获取相机的标定信息。

2.3 点云拼接代码

从上一步中可以得到从相机1到相机0的旋转矩阵和平移矩阵（我这里利用的是从Matlab中获取到的标定信息），因此点云根据这两个矩阵进行变换即可，将两个相机的点云数据变换到同一个世界坐标系下。

这里仅供参考的关键代码：

class Reconstruction{
private:
    vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d>> _poses;//相机标定外参
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr _pointcloud; //三维重建拼接点云

    int _camera_num; //相机数目
    double _cx; //光心
    double _cy;
    double _fx; //焦距
    double _fy; 
    double _depthScale;

public:
    //构造函数
    Reconstruction(){
        _pointcloud.reset( new pcl::PointCloud<pcl::PointXYZRGB>() );
        //读取相机内参标定
        ifstream ifs("../parameter1.txt"); //parameter.txt存储相机内参信息
        ifs >> _camera_num >> _cx >> _cy >> _fx >> _fy >> _depthScale;
        ifs.close();
        //读取相机外参标定
        ifstream fin("../poses1.txt"); //poses.txt存储的是两个相机的旋转和平移矩阵信息
        double data[12] = {0};

        for (int i = 0; i < _camera_num; i++) {
            for (auto &d:data){
                fin >> d;
            }
            //平移矩阵![请添加图片描述](https://img-blog.csdnimg.cn/e136bb94f133467d82110251da4c64cd.png)

            Eigen::Vector3d translation( data[0], data[1], data[2]);
            //旋转矩阵
            Eigen::Matrix3d rotation_matrix = Eigen::Matrix3d::Identity();
            rotation_matrix << data[3], data[4], data[5], data[6], data[7], data[8], data[9], data[10], data[11];
            //构造变换矩阵
            Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
            T.rotate( rotation_matrix );
            T.pretranslate(-rotation_matrix * translation);
            _poses.push_back( T );
            cout << _poses.back().matrix() << endl;
        }
        fin.close();
    }
  	//点云生成
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr ImageToPointcloud(cv::Mat& color, cv::Mat& depth){
	    pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointcloud( new pcl::PointCloud<pcl::PointXYZRGB>() );
	    for (int v = 0; v < depth.rows; v++){
	        for (int u = 0; u < depth.cols; u++){
	            unsigned int d = depth.ptr<unsigned short>(v)[u];
	            pcl::PointXYZRGB point;
	
	            point.z = double(d) / _depthScale;
	            point.x = (u - _cx) * point.z / _fx;  // _cx, _cy是摄像头光学中心
	            point.y = (v - _cy) * point.z / _fy;  // _fx, _fy是摄像头焦距
	            
	            point.b = color.data[v*color.step+u*color.channels()];
	            point.g = color.data[v*color.step+u*color.channels() + 1];
	            point.r = color.data[v*color.step+u*color.channels() + 2];
	
	            pointcloud->points.push_back(point);
	        }
	    }
	    pointcloud->height = 1;
	    pointcloud->width = pointcloud->points.size();
	    pointcloud->is_dense = false;
	
	    return pointcloud;
	}
	//点云拼接
    void pointCloudRegistration(vector<cv::Mat> colorImgs_, vector<cv::Mat> depthImgs_){
        //读取每个相机的color图和depth图
        for (int i = 0; i < _camera_num; i++) {
            cv::Mat color = colorImgs_[i];
            cv::Mat depth = depthImgs_[i];
            Eigen::Isometry3d T = _poses[i];
            
            pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>());
            pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointcloud(new pcl::PointCloud<pcl::PointXYZRGB>());
            cloud = ImageToPointcloud(i, color, depth);
            pcl::transformPointCloud(*cloud, *pointcloud, T.matrix()); //利用变换矩阵转换点云
         
            pcl::io::savePCDFileBinary("../data/camera" + to_string(i) + ".pcd", *pointcloud );
            *_pointcloud += *pointcloud;
        }

        pcl::io::savePCDFileBinary("../data/source_model.pcd", *_pointcloud );
    }
};

三、结果展示

相机0和相机1的color图：

相机0和相机1产生的点云：

拼接后的点云：

总结

这一篇只是完成了简单的点云拼接操作（其实就是两个点云叠加），最关键的部分还是在相机标定那一块，如何得到标定信息。此外，从拼接的结果来看，拼接后的点云之间还是有缝隙，我尝试标定了好几次，缝隙依然存在，也没有改善。观察Kinect所拍摄的深度图发现，人体的边缘部分存在着黑边，感觉是因为边缘误差太大或者是人体的边缘深度没有探测到，后面再看看怎么改进。因为Azure KInect DK拍摄图片形成的点云比较稠密，后续会用PCL进行处理，顺便学习PCL点云库相关的操作和通用算法。