实验室研究方向

RGB-D SLAM 章国锋 浙江大学 CAD&CG 国家重点实验室

RGB-D 相机跟踪 RGB-D Sensor 双目 结构光 TOF 相机跟踪 特征点法 ICP RGB-D 对齐

RGB-D 相机跟踪 RGB-D 传感器 Kinect, Xtion, ZED, Intel RealSense, Kinect Xtion 图片来自微软 华硕官网

RGB-D 传感器 按工作原理分类 ; 双目方案 ZED 结构光方案 Kinect v1 Xtion Intel RealSense Camera SR300 TOF 方案 Kinect v2 https://www.pcmag.com/news/356997/microsoft-is-ending-kinect-production

双目 立体匹配

结构光 主动投影已知图案的方法来实现快速鲁棒的匹配特征点 陈彦军, 左旺孟, 王宽全, 等. 结构光编码方法综述 [J]. 小型微型计算机系统, 2010 (9): 1856-1863.

TOF 测量发射与反射红外信号的相位延迟, 计算每个 sensor 像素到目标物体的距离 https://my.oschina.net/abcijkxyz/blog/788647

RGB-D 传感器 各方案的优劣 双目 : 成本最低, 但深度信息依赖纯软件算法得出, 算法复杂度高, 计算性能要求高, 受光照等影响 结构光 : 技术成熟, 深度图像分辨率可以做得比较高, 但容易受光照影响, 室外环境基本不能使用 TOF: 抗干扰性能好, 视角更宽, 深度图像分辨率较低, 不适合高精度场合 受环境影响小, 传感器芯片并不成熟, 成本很高

RGB-D 传感器 使用 RGB-D 传感器优点是不需要计算特征点和描述子, 就可以直接得到稠密或半稠密的深度图 框架也相对传统 SLAM 简单, 可分为前端 RGB-D 相机跟踪与后端模型重建

RGB-D 相机跟踪 特征点法 包含了传统的 RGB 信息, 也可以使用特征点匹配求解相机位姿 一般场景都能提供丰富的特征点, 场景适应性较好, 能够利用特征点进行重定位 缺点是特征点计算法耗时 ; 特征点利用到的信息太少, 丢失了图像中的大部分信息和深度信息, 在弱纹理环境下特征少等

RGB-D 相机跟踪 直接法 包含了传统的 RGB 信息, 也可以使用直接法求解相机位姿 优点是不需要计算特征描述子, 可以得到稠密或半稠密的地图 ; 在特征缺失时也可正常使用 缺点灰度不变假设在实际环境中不一定成立, 要求像机运动速度不能太快, 不能自动曝光等

RGB-D 相机跟踪 迭代最邻近算法 (ICP) 该算法重复进行选择对应关系点对, 计算最优刚体变换, 然后应用变换, 再寻找对应关系点对, 计算新的最优变换, 直到满足正确配准的收敛精度要求 充分利用了深度图点云信息, 利用点云的几何结构特征, 不依靠 RGB 特征与光度

ICP(Iterative Closest Point) 目标 给定两组点云 P = {p 1, p 2,, p m }, Q = {q 1, q 2,, q n }, 求解两组点云之间的变换 T 使得下式得到最优解 cost 是代价函数, 度量匹配的误差 http://pointclouds.org/documentation/tutorials/interactive_icp.php#interactive-icp

ICP 步骤 固定相对变换 T, 求解最优的对应关系 A, 这一步即所谓的数据关联 (data association) 固定匹配关系 A, 求解最优的相对变换 T:

ICP 取样 : 选择一个或两个网格上的部分点作为样本 匹配 : 建立样本点的数据关联关系 置权 : 给关联样本点对设置权重 过滤 : 对不符合条件的关联点对做删除 误差度量 : 给关联点对设置误差度量 最优化 : 最优化误差度量

ICP Variants 取样 Uniform Sampling Random Sampling Normal Space Sampling 选择样本点使得样本点法向散布最大

ICP Variants Data association Closet point K-D Tree Normal shooting https://en.wikipedia.org/wiki/k-d_tree

ICP Variants Data association project project and walk x u d, vd, 1 = KMX s K 是相机内参数,X s 是待求匹配点的三维空间坐 标,x 是匹配点像素坐标,M 是两帧之间的位姿变 换, 在 ICP 算法中通过不断迭代求得 https://blog.csdn.net/fuxingyin/article/details/51425721

ICP Variants 权值设置 Constant weight Linear with distance Compatibility of normals Uncertainty 基于相机噪声的权值设置

ICP Variants 过滤准则 距离超过固定门限的关联点对删除 距离最大的 n% 关联点对删除 距离超过 2.5 倍标准差的关联点对删除

ICP Variants 误差度量 point-to-point point-to-plane Low K L. Linear least-squares optimization for point-to-plane icp surface registration[j]. Chapel Hill, University of North Carolina, 2004, 4: 1.

ICP 缺点 对位姿初值很敏感, 需要有较好的初值 只利用了深度信息, 没有利用 RGB 信息

RGB-D 对齐 通过最小化逆深度误差和光度误差来求解两帧之间的相对相机姿态 几何误差 E z = x i ρ z 1 z X j Z j x j z X j 代表点 X j 在第 i 帧上的深度,Z j (x j ) 代表第 j 帧的深度图上点 X j 的投影位置 x j 对应的深度 ρ z 是相应的鲁棒化函数

RGB-D 对齐 相对于 ICP 算法,RGB-D 对齐不仅考虑到了深度信息的几何误差, 还考虑了光度误差 光度误差 E I = x i ρ I I i x i I j x j I i (x i ) 代表第 i 帧上 x i 对应的光度 ρ I 是相应的鲁棒化函数 总能量函数 arg min T E align = E z + α E I

模型表示与重建 渐进式在线重建 可以支持在线高效更新 网格表达不适合在线更新 两种常见模型表示 TSDF Surfel 一些代表性算法

TSDF 带符号距离函数 (Signed Distance Function): sdf: R 3 R 对于点 p, sdf(p) 记录了把它映射到最近表面的距离 sdf(p) 为 0 的集合即为表面 截断带符号距离函数 (Truncated Signed Distance Function) 实际上, 对于三维重建而言, 我们仅仅需要找到 SDF 为 0 的点, 因此只有表面附近的点是有用的, 因此通常会对 SDF 进行截断, 不再存储离表面较远处点的 SDF

TSDF: rendering by raycasting 渲染一个像素 x 时 从相机中心投射一条射线, 寻找它和模型的交点 这个交点的 TSDF 函数值为 0 一个简单的实现 : 采用固定步长 但是我们可以用 TSDF 来加速 : 渲染时间近似与图像的尺寸线性相关 http://flafla2.github.io/2016/10/01/raymarching.html

Surfel Surfel: 表面的一个小的面元 几何属性 位置 法向 半径... 反射属性 颜色... model: 面元的集合 Pfister H, Zwicker M, Van Baar J, et al. Surfels: Surface elements as rendering primitives[c]//proceedings of the 27th annual conference on Computer graphics and interactive techniques. ACM Press/Addison-Wesley Publishing Co., 2000: 335-342.

http://www.cs.rug.nl/~roe/courses/acg/gpurendering Surfel: splat rendering 渲染一个 Surfel 是非常简单的 : 直接单独渲染一个 Surfel 是不行的, 因为面元之间可能有重叠!

http://www.cs.rug.nl/~roe/courses/acg/gpurendering Surfel: splat rendering (cont.) 使用高斯核卷积 取加权和

Algorithms KinectFusion Kintinous InifiniTAM ElasticFusion RKD SLAM

KinectFusion

KinectFusion The first algorithm using a commodity RGBD sensor to do 3D scanning GPU based pipeline Only the depth image is used, both for tacking and model update Newcombe R A, Izadi S, Hilliges O, et al. KinectFusion: Real-time dense surface mapping and tracking[c]//mixed and augmented reality (ISMAR), 2011 10th IEEE international symposium on. IEEE, 2011: 127-136.

KinectFusion TSDF based model representation Discretize the space into uniform grid Call each grid voxel struct Voxel { float sdf; float weight; };

KinectFusion: pipeline Iterating Pose estimation Model update (depth integration) Raycasting (visualization)

KinectFusion: pose estimation Convert the depth image into Vertex map Normal map Pyramid ICP Projective data association Remove outliers by normal direction and distance Minimize point to plane distance

KinectFusion: integration (model update) For each voxel Project the voxel to image space Compute the TSDF observation d(x) Update by weighted sum http://ifp.uni-stuttgart.de/lehre/diplomarbeiten/korcz/index.html

KinectFusion: limitations Only use depth image Robust to lighting Works on dark environment But not works on structureless scene High memory consumption Each voxel takes 8 byte A 2mx2mx2m scene, takes ~0.5GB at 5mm resolution (geometry only, double if add color) Improvement Moving volume Hash table Octree...

Kintinous

Kintinous KinectFusion Huge memory consumption Suitable for very small workspace Kintinous Only part of the model is active Move inactive part out of GPU Loop closure detection and online model adjustment

Kintinous: pipeline (i) Camera moves, (ii) Red region becomes inactive, swap out (iii) Convert red region to point cloud by raycasting (iv) Blue region becomes active

Kintinous: online map adjustment The swapped out TSDF volume is called point slice Associate each point slice with Pose BOW, to find loop On loop closure, use pose graph to do pose optimization, adjust the map by deformation graph

Kintinous https://blog.csdn.net/fuxingyin/article/details/51647750

Kintinous Whelan T, Kaess M, Fallon M, et al. Kintinuous: Spatially extended kinectfusion[j]. 2012.

ElasticFusion

ElasticFusion Surfel based representation Support loop closure, online model adjustment Thomas Whelan, Stefan Leutenegger, R Salas-Moreno, Ben Glocker, Andrew Davison. Elasticfusion: Dense slam without a pose graph[c]. Robotics: Science and Systems, 2015.

ElasticFusion The model is an unordered list of Surfels Each surfel contains: Position p Normal n Radius r Color c Confidence (how often it s observed) Crate timestamp t0, most recent update timestamp t

ElasticFusion A time window divides the model into active/inactive part A surfel is active if it is most recent update time t Only active surfels are used to do camera pose estimation and depth map fusion

ElasticFusion At each frame, try to register active and inactive part to find a local loop closure Only active surfels are used to do camera pose estimation and depth map fusion Use place recognition to find global loop closure On loop closure, correct model by non-rigid transformation

Deformation Graph Problem: non-rigidly align two models

Deformation Graph Construct a graph in space Each node represents an affine transformation The affine transformation should: Satisfies the alignment constraint (constraint term) As rigid as possible (rotation term) Neighboring node has similar transformations (regulation term)

ElasticFusion: system pipeline RGB-D image preprocess Camera pose estimation Depth image fusion Try find local loop closure Try find global loop closure Refine the map using deformation graph (in a separate thread)

ElasticFusion: camera pose estimation Render the active part into a colored depth image (splat rendering) using last camera pose Register the rendered image with current image by ICP Hierarchical, coarse to fine Projective data association Optimize both geometric error and photometric error

ElasticFusion: depth image fusion A surfel is stable if its confidence is above the threshold Render the active part into an index map, the index map is used to associate surfels in active model with current depth image For each point in current image If a stable correspondence surfel found, then update the surfel by weighted sum else create a new surfel, add it to map

ElasticFusion: local loop detection Render the active, inactive part using latest camera pose into color depth image Try to register the two images If a high quality registration found, then build a deformation graph to align the two part

ElasticFusion: global loop detection Ferns: encode an RGB-D image as a string of codes Maintain a fern database On each frame, query the database to find the closure candidates Align current frame with the candidates If a high quality alignment found, then use the constraints to build deformation graph, and update the model

Robust Keyframe-based Dense SLAM with an RGB-D Camera https://arxiv.org/abs/1711.05166

RKD-SLAM 系统框架 非常快速鲁棒的基于 RGB-D 的跟踪方法 ( 单 CPU 下约 70-200 fps) 非常快速的增量集束调整算法 非常高效的基于关键帧的深度表达和融合方法 支持快速运动 回路闭合 重定位和长时间运行

Efficient Incremental BA 提出了一个非常高效的 Incremental Schur complement 计算方法 ; 采用 Preconditioned Conjugated Gradient 进行求解, 比 Factorization 的方法要快 ; 速度比 isam2 快一个数量级

Efficient Incremental BA 与 isam2 的对比 运行时间 Reprojection Error

Integration & De-integration 使用 de-integration 可以将模型复原到 integration 前 + -

Re-integration 原模型 Dai A, Nießner M, Zollhöfer M, et al. Bundlefusion: Real-time globally consistent 3d reconstruction using on-the-fly surface reintegration[j]. ACM Transactions on Graphics (TOG), 2017, 36(4): 76a.

Re-integration 使用错误姿态 integrate 后的模型 Dai A, Nießner M, Zollhöfer M, et al. Bundlefusion: Real-time globally consistent 3d reconstruction using on-the-fly surface reintegration[j]. ACM Transactions on Graphics (TOG), 2017, 36(4): 76a.

Re-integration 使用 de-integrate 还原模型 Dai A, Nießner M, Zollhöfer M, et al. Bundlefusion: Real-time globally consistent 3d reconstruction using on-the-fly surface reintegration[j]. ACM Transactions on Graphics (TOG), 2017, 36(4): 76a.

Re-integration 使用正确姿态 integrate 后的模型 Dai A, Nießner M, Zollhöfer M, et al. Bundlefusion: Real-time globally consistent 3d reconstruction using on-the-fly surface reintegration[j]. ACM Transactions on Graphics (TOG), 2017, 36(4): 76a.

Keyframe-based Fusion 对于新来的一帧 F i 如果是关键帧, 则 integrate 到 TSDF 如非关键帧, 则选出重合度最大的关键帧 F ki 进行 de-integrate 然后将该帧深度 fuse 到 F ki 上 然后将 fuse 后的关键帧 re-integrate 到 TSDF

Keyframe-based Fusion 当关键帧的姿态发生改变 (EIBA 优化后 ) 根据 EIBA 的优化结果, 对姿态改变的关键帧进行 re-integration. 维护一个关键帧更新队列 优先更新姿态改变最大的关键帧 ; 每个时刻只对固定数量的关键帧进行 re-integration, 没有更新的关键帧会在放在后面的时刻更新

Comparison of ATE RMSE on all of the sequences on TUM RGB-D Benchmark

Reconstruction Result

Comparations without/with Re-integration

鲁棒处理快速运动

在线的回路闭合和三维表面调整

推荐开源系统 Kintinuous https://github.com/mp3guy/kintinuous ElasticFusion https://github.com/mp3guy/elasticfusion BundleFusion https://github.com/niessner/bundlefusion

Thank you!