RGBD融合原理及实践

前言

好久没更新博客了，主要是因为懒，最近有些得闲，决定纪录下之前的工作。RT，RGBD数据融合其实就是将3D摄像机的RGB与Depth数据做融合显示的过程，做法也不难理解，就是将depth camera与rgb camera的像素对应起来即可。

原理部分

原理部分主要借鉴这篇 博文, 详细的公式在这就不作重复了，贴张图吧。

从上面的博客或图片可以看出，关键先找到两个camera的外参矩阵RT，一开始我是按照博客的来用GML Camera Calibration Toolbox进行内外参矩阵，然后利用公式求出RT，但实际测试下来我尝试在同一场景下同时采集双目摄像头的几组正面棋盘，获得它们的外参得出的RT都不正常，这里的RT很重要，直接影响到后面计算对应像素！ 后面我还是转用大杀器matlab calibration toolbox，虽然比GML标定要麻烦，每张图都要手动选四个参考角点，但胜在它稳定、精度高啊，我基本走一次流程下来得到的RT就比较准确了。所以，标个内参的话可以用GML，比较快搞掂，但需要双目标定时还是用回matlab吧，哈哈。 哦，对了，matlab出来的旋转矩阵是om，需要做一个罗格变换成标准的3x3矩阵，toolbox里自带了接口，直接用即可。

Rotation vector:             om = [ 0.05129   0.00136  -0.02893 ] ? [ 0.03624   0.03419  0.00277 ]
>> rodrigues(om) 
ans =
    0.9996    0.0289    0.0006
   -0.0289    0.9983   -0.0513
   -0.0021    0.0512    0.9987

实践

来到实践部分，写了一个简单的脚本做验证，主要是验证下标定出的RT是否正确可用。（注意代码里IR即指depth camera）

from numpy import *
import numpy as np
# import matplotlib.pylab as plt


#双目内参
# ir camera
# 408.72767  0           332.18622
# 0        410.38278    227.32216
# 0        0             1 
# 
# rgb camera
# 438.63884  0      337.13761
# 0          440.86391   254.91443
# 0         0             1
#  


ir_in = np.loadtxt("ir_matlab_intrinsic.txt")
rgb_in = np.loadtxt("rgb_matlab_intrinsic.txt")

# RT矩阵
R = np.array([[0.9996,    0.0289,    0.0006], [-0.0289,    0.9983,   -0.0513], [-0.0021,    0.0512,    0.9987]])
T = np.array([[-54.58182],   [2.11322],  [-0.64764]])

print (R)
print (T)
# ir 内参逆阵
ir_in_I = linalg.inv(ir_in)
print (ir_in_I)

# 建立ir像面坐标，900指某一点的深度900mm，注意是 Zc [x  y 1]
pixel_depth = 900
test = np.array([[129 * pixel_depth], [302 * pixel_depth], [pixel_depth]])

print ("---- Pir ---")
P_ir = np.dot(ir_in_I, test)
print (P_ir)
P_rgb = np.dot(R, P_ir) + T
print ("---- P rgb ---")
print (P_rgb)
p_rgb = np.dot(rgb_in, P_rgb)

print (p_rgb)
print (p_rgb / p_rgb[2])

上面代码主要验证IR camera坐标 (129, 302)与RGB对应的坐标是多少，最终输出的p_rgb x y分量即为对应的rgb坐标值，实际测试下来还是蛮准确的。OK，验证完事后，可以用C++实现下上面的脚本，对每个pixel都做这样的转换处理，即可以得到rgb depth camera对应关系，也就是说做到这两者的数据融合咯。 注意最好用eigen这样的三方库，直接用opencv的矩阵运算实在太慢了（其实主要是cv::Mat 动态变量分配空间比较耗时，反复调用的话延时完全不可接受）。 下图为实际的融合效果，初步来看效果还是不错的。（请忽视右上角的那几道条纹，那是因为rgb摄像头在日光灯下产生了条纹）