在上一篇中已经完成了从图像到点云的转换，但是只针对单个相机，在这一篇中将再进一步，从两个相机拍摄图像，然后做点云的生成及拼接。
相机标定的原理我们上一篇章中也提到了一些（内参矩阵、外参矩阵），而相机标定需要用到的信息包含有内部参数（焦距、光学中心等）、外部参数（旋转、平移矩阵）以及畸变系数。
其中畸变主要指的是径向畸变和切向畸变，径向畸变导致直线呈曲线状，距离图像中心越远，径向失真越大；切向畸变发生是因为摄像透镜没有与成像平面完全平行，图像中的某些区域看起来可能比预期的要近。
更为详细的原理步骤叙述可以参考：https://blog.csdn.net/lql0716/article/details/71973318。
因为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>
还可以利用Matlab相机标定获取相机的标定信息。
从上一步中可以得到从相机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;
}
//平移矩阵
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点云库相关的操作和通用算法。