[转载]windows下realsense数据处理基础（一）

realsense的开发资料主要集中linux下，linux对图像的处理确实有很多优势，比如大名鼎鼎的TensorFlow，OrbSlam2都是很难移植到windows下的。一些时候，开发者和用户都比较熟悉windows，加上个人比较习惯vs2015开发环境，所以文章的介绍都是基于windows环境的。

在基础（一）部分，主要梳理了如何获取原始帧数据（图中绿色部分），以及如何利用原始数据生成其它数据的方法（图中蓝色部分）。

1 环境说明

工具和库

	选择原因	移植性	下载地址
Qt 5.9.4	兼容多版本windows GUI友好	可移植
vs 2015专业版	开发环境友好	不可
OpenCV 2.4.13.5	图像处理库	可移植	点击下载
librealsense 2.11.0	数据采集	可移植	点击下载 配置说明
PCL 1.8.1	点云处理库	可移植	点击下载 配置说明
VTK 8.0.0	点云显示库	可移植	源码 编译安装说明 x64 编译版 替换方法

硬件&驱动

测试以sr300为例，librealsense2的推荐sr300和400系列，代码中可以发现有对r200,f200控制的相关代码，测试发现不支持f200。

sr300驱动下载 （windows）。

安装目录

opencv C:\opencv\opencv_2413\build

pcl1.8.1 C:\PCL1.8.1

librealsense C:\Program Files (x86)\Intel RealSense SDK 2.0

修改安装目录，需修改对应的系统环境变量。

系统环境变量&Path

Path

VS2015工程环境

点此下载

备注

2 realsense原始数据采集

2.1 realsense原始数据

sr300有2个物理传感器ir_sensor和color_sensor，每个物理传感器都有各自的二维像素平面坐标系和三维点云坐标系。这个两个坐标系通过外参数关联。

librealsense2的数据接口提供设备信息、传感器属性、帧数据三类原始数据。

设备信息。设备信息包括设备名称、设备序列号、固件版本等信息。

传感器属性。传感器属性包括缩放比例因子（深度值和米之间的换算关系）、内参数（分辨率、中心点位置、视角、畸变矫正方法、矫正参数）、外参数（两个传感器的坐标系的映射关系）。 depth和ir是同一个物理传感器，所以两者坐标系相同，内外参相同。intel规定color传感器不存在畸变，所以color内参数中k1,k2,k3,q1,q2均为0。

帧数据。原始设备的帧数据有三种：

物理传感器	frame数据	数据格式及分辨率	数据类型
ir_sensor	ir_frame	640*480 CV_8UC1	rs2::frame
ir_sensor	depth_frame	640*480 CV_16UC1	rs2::frame
color_sensor	color_frame	1920*1080 CV_8UC3	rs2::frame

sr300帧数据似乎并没有做到绝对的时间同步，所以intel采用了时间近似匹配策略，并在帧数据中提供了时间戳，从观测情况下，误差大约在3ms左右。

对帧数据质量的评估，可使用Depth Quality Tool工具。

2.2 源数据采集demo

// 获取设备信息
rs2::context ctx;
auto devicelist = ctx.query_devices();
rs2::device dev = *devicelist.begin(); // 单个设备
cout << "RS2_CAMERA_NAME： " << dev.get_info(RS2_CAMERA_INFO_NAME) << endl;
cout << "RS2_CAMERA_SERIAL_NUMBER： " << dev.get_info(RS2_SERIAL_NUMBER) << endl;
cout << "RS2_FIRMWARE_VERSION： " << dev.get_info(RS2_FIRMWARE_VERSION) << endl;

// 配置需要采集的帧数据
rs2::config cfg;
cfg.enable_stream(RS2_STREAM_INFRARED, 640, 480, RS2_FORMAT_Y8, 30);
cfg.enable_stream(RS2_STREAM_DEPTH, 640, 480, RS2_FORMAT_Z16, 30);
cfg.enable_stream(RS2_STREAM_COLOR, 640, 480, RS2_FORMAT_BGR8, 30);

// 启动采集设备
profile = pipe.start(cfg);

// 获取profile，包括深度的比例因子和传感器的内外参数
auto sensor = profile->get_device().first<rs2::depth_sensor>();
double _depth_scale = sensor.get_depth_scale();
auto _stream_depth = profile->get_stream(RS2_STREAM_DEPTH).as<rs2::video_stream_profile>();
auto _stream_color = profile->get_stream(RS2_STREAM_COLOR).as<rs2::video_stream_profile>();
auto _stream_ir = profile->get_stream(RS2_STREAM_INFRARED).as<rs2::video_stream_profile>();
rs2_intrinsics _intrinsics_depth = _stream_depth.get_intrinsics();
rs2_intrinsics _intrinsics_color = _stream_color.get_intrinsics();
rs2_extrinsics _extrinsics_depth2color = _stream_depth.get_extrinsics_to(_stream_color);

// 获取帧数据
while (1)
{
    rs2::frameset frames = pipe.wait_for_frames();          // Wait for next set of frames from the camera
    rs2::frame Rs_ir_pixel = frames.get_infrared_frame();
    rs2::frame Rs_ir_depth = frames.get_depth_frame();
    rs2::frame Rs_color_pixel = frames.get_color_frame();
    // 添加自己的帧数据处理、显示代码
}

3 librealsense扩展数据

扩展数据是由librealsense2库处理后生成的数据， 如图绿色部分，这部分数据基本可以通过调用librealsense2库提供的功能实现。扩展数据不包括手势、人脸等分析结果，这部分功能在intel早期的闭源sdk中提供，并且已经不再更新。

3.1 三个基本转换函数

为了实现这个功能， librealsense2在rsutil.h中定义了三个单像素/单点转换函数。一些高级的转换功能都是在这个3个函数的基础上实现。

逆投影函数（rs2_deproject_pixel_to_point）

将像素平面上的点投影到三维空间。输入点的像素坐标、对应的深度值、传感器内参数，输出点的三维坐标。这个过程考虑了镜头的失真，使用修改的BROWN_CONRADY模型矫正。

投影函数（ rs2_project_point_to_pixel ）

将三维空间的点投影到像素平面。输入点的三维坐标，传感器内参数，输出二维像素平面坐标。这个坐标不是一个离散坐标，也就是说位于目标像素平面，和目标像素图有相同的坐标系，但没有和平面上的像素点形成一一对应关系，且投影会在二维平面上形成空洞（hole）。

三维点坐标系变换函数（rs2_transform_point_to_point）

将点坐标从一个传感器三维坐标系到另一个传感器三维坐标系。简单，不解释。

三个函数的原型

static void rs2_deproject_pixel_to_point(float point[3], const struct rs2_intrinsics * intrin, const float pixel[2], float depth)

static void rs2_transform_point_to_point(float to_point[3], const struct rs2_extrinsics * extrin, const float from_point[3])

static void rs2_project_point_to_pixel(float pixel[2], const struct rs2_intrinsics * intrin, const float point[3])

3.2 获取Rs_ir_points

Rs_ir_points的获取方法就是对调用deproject对Rs_ir_depth迭代。

实现效果。用vtk显示ir_points 是这样的

实现代码

rs2::pointcloud pc2;

rs2::points ir_points = pc2.calculate(rs_ir_depth);

3.3 获取Rs_color_depth

正常情况下，获得Rs_color_depth途径比较复杂。仅靠三个基本函数并不能完全实现，需要自己补充一些算法。librealsense2中直接提供了rs2::align对象， 如图可见，该方法可以将深度图像可以从一个传感器坐标系转换到另一个传感器坐标系。例如，通过该方法可以将Rs_ir_depth对齐到Rs_color_depth，对齐可以理解为在新的坐标系下建立一组点云描述原点云同一个形状，新旧直接不存在一一对应关系。在align.cpp的align_images()中可以看到算法的实现代码。

实现效果。三幅图像分别为rs_color_pixel，rs_ir_depth，rs_color_depth。

实现方法

rs2::align align(RS2_STREAM_COLOR); // 选择要对齐的数据流

auto aligned_frames = align.process(frames); // rs2::frameset frames

rs2::frame color_depth_frame = aligned_frames.get_depth_frame();

此算法存在一些问题，在color分辨率为640*480时会有轻微变形，在分辨率为1920*1080时有严重变形。

获取Rs_color_points

从前面的分析可以看出有两种方法获得Rs_color_points。

方法一 Rs_ir_points经坐标系转换得到。

方法二 通过Rs_color_depth生成。

这两种点云描述的形状一致，但是点不重合。方法一生成的点云与ir传感器像素一一对应。方法二生成的点云与color传感器像素一一对应。方法二中包含的计算误差会多一些。方法一的点云着色需要先构建三角网，然后采用纹理贴图的融合点云和rs_color_pixel，方法二的点云可以直接将color中像素颜色映射到点云的每个点上。

方法一的效果

换个角度是这样的

方法一的实现可参考rs_sample的pointcloude工程。

方法二的效果是这样的

换个角度是这样的。

方法二 代码

rs2::align align(RS2_STREAM_COLOR);

auto aligned_frames = align.process(frames); // rs2::frameset frames

rs2::frame rs_color_depth = aligned_frames.get_depth_frame();

rs2::pointcloud pc1;

rs2::points rs_color_points = pc1.calculate(rs_color_depth);

4 rs像素图转opencv Mat

注意下较早版本的opencv可能不支持，转换时要选择正确的Mat深度格式

实现代码

Mat ir_depth(Size(640, 480), CV_16UC1, (void*)ir_depth_frame.get_data(), Mat::AUTO_STEP);

Mat color_pixel(Size(640, 480), CV_8UC3, (void*)color_pixel_frame.get_data(), Mat::AUTO_STEP);

Mat color_depth(Size(640, 480), CV_16UC1, (void*)color_depth_frame.get_data(), Mat::AUTO_STEP);

5 rs点云转PCL点云

这里贴一个基于方法二获得的Rs_color_points融合Rs_color_pixel转Pcl_pointcloud的代码。

void rspoints_to_pcl(const rs2::points& rs_points, pcl::PointCloud<pcl::PointXYZRGBA>::Ptr pcl_cloud, cv::Mat rs_color_pixel)

{

auto sp = rs_points.get_profile().as<rs2::video_stream_profile>();

pcl_cloud->width = sp.width();

pcl_cloud->height = sp.height();

pcl_cloud->is_dense = false;

pcl_cloud->points.resize(rs_points.size());

auto ptr = rs_points.get_vertices(); // rs_color_point

MatIterator_<Vec3b> it, end;

it = rs_color_pixel.begin<Vec3b>(); // rs_color_pixel 已经转换为cv::Mat

for (auto& p : pcl_cloud->points)

{

p.x = ptr->x;

p.y = ptr->y;

p.z = ptr->z;

p.b = (*it)[0];

p.g = (*it)[1];

p.r = (*it)[2];

ptr++;

it++;

}

}

6 Mat转QImage显示

装QImage是为了在Qt中显示，转换方法网上有许多，因为有三个格式，需要写一个判断函数。

代码实现可参考下面的代码

QImage mat2QImage(cv::Mat& image)

{

QImage img;

if (image.channels()==3) {

cvtColor(image, image, CV_BGR2RGB);

img = QImage((const unsigned char *)(image.data), image.cols, image.rows,

image.cols*image.channels(), QImage::Format_RGB888);

} else if (image.channels()==1) {

img = QImage((const unsigned char *)(image.data), image.cols, image.rows,

image.cols*image.channels(), QImage::Format_ARGB32);

} else {

img = QImage((const unsigned char *)(image.data), image.cols, image.rows,

image.cols*image.channels(), QImage::Format_RGB888);

}

return img;

}

QImage ImageLabel::qImageFrom16UC1(const cv::Mat & image)

{

if (image.type()!=CV_16UC1)

{ //unsupported type

return QImage();

}

QImage qimg(image.cols, image.rows, QImage::Format_RGB32);

//normalize(image, image, 0, 255, CV_MINMAX); // 图像归一化

for (int h=0; h<image.rows; h++)

{

const unsigned short * row = image.ptr<unsigned short>(h);

unsigned * qrow = reinterpret_cast<unsigned *>(qimg.scanLine(h));

for (register int w=0; w<image.cols; w++)

{

unsigned char pix = row[w]>>6;

qrow[w] = qRgb(pix, pix, pix);

}

}

return qimg;

}

7 PCL点云显示

在配置时需要注意CameraPosition

7.1 界面设计

在QT窗口上拖一个QVTKWidget，取名vtk_view1。

7.2 配置

ui->vtk_view1->setGeometry(0, 504, 640, 480);

_pcl_cloud1.reset(new PointCloudT); // 创建PCL点云对象

//_pcl_cloud1->points.resize(200); // 配置PCL点云容器大小

_pcl_viewer1.reset(new pcl::visualization::PCLVisualizer("_pcl_viewer1", false)); // 创建PCL渲染器

_pcl_viewer1->setupInteractor(ui->vtk_view1->GetInteractor(), ui->vtk_view1->GetRenderWindow()); // 配置vtk控件的交互器

_pcl_viewer1->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "cloud"); // 配置PCL渲染器点大小

// _pcl_viewer1->addCoordinateSystem(1.0); // 配置PCL渲染器坐标系

_pcl_viewer1->setCameraPosition(0, 0, -1, 0, -1,0); // 前3个点是观察位置（0,0,-1）表示观察者在z轴负方向，后3个值是上方向（0，-1，0），表示y轴向下。

_pcl_viewer1->resetCamera(); // 配置PCL渲染器相机

_pcl_viewer1->addPointCloud(_pcl_cloud1, "cloud1"); // 关联PCL渲染器和PCL点云

ui->vtk_view1->SetRenderWindow(_pcl_viewer1->getRenderWindow()); // 关联vtk控件的PCL渲染器

ui->vtk_view1->update();

7.3 帧刷新显示

_pcl_viewer2->updatePointCloud(_pcl_cloud2, "cloud2");

ui->vtk_view2->update();

OK，基本数据处理就这么多吧，下次整整滤波。