Surfel-Based Dense RGB-D Reconstruction with Global and Local …kaess/pub/Yang19icra.pdf · 2019-03-09 · Surfel-Based Dense RGB-D Reconstruction with Global and Local Consistency

Surfel-Based Dense RGB-D Reconstruction with Global and LocalConsistency

Yi Yang, Wei Dong, and Michael Kaess

Abstract— Achieving high surface reconstruction accuracy indense mapping has been a desirable target for both roboticsand vision communities. In the robotics literature, simultaneouslocalization and mapping (SLAM) systems use RGB-D camerasto reconstruct a dense map of the environment. They leveragethe depth input to provide accurate local pose estimationand a locally consistent model. However, drift in the posetracking over time leads to misalignments and artifacts. Onthe other hand, offline computer vision methods, such as thepipeline that combines structure-from-motion (SfM) and multi-view stereo (MVS), estimate the camera poses by performingbatch optimization. These methods achieve global consistency,but suffer from heavy computation loads. We propose a novelapproach that integrates both methods to achieve locally andglobally consistent reconstruction. First, we estimate poses ofkeyframes in the offline SfM pipeline to provide strong globalconstraints at relatively low cost. Afterwards, we computeodometry between frames driven by off-the-shelf SLAM systemswith high local accuracy. We fuse the two pose estimationsusing factor graph optimization to generate accurate cameraposes for dense reconstruction. Experiments on real-world andsynthetic datasets demonstrate that our approach producesmore accurate models comparing to existing dense SLAMsystems, while achieving significant speedup with respect tostate-of-the-art SfM-MVS pipelines.

I. INTRODUCTION

Dense 3D reconstruction with accurate geometry is desiredfor different applications such as infrastructure inspection,indoor robot navigation, and virtual reality. The two keydemands for a reconstruction algorithm to fulfill are: (1)accurate modelling of the global geometry of a large scaleenvironment in terms of global geometric consistency, and(2) detailed scene description such as locally consistentshape, texture, and color. While many efforts in SfM anddense mapping in SLAM have been devoted to address bothrequirements [1]–[5], they often fail in one of the criteria.

Specifically, in computer vision methods such as SfM, theinitial structure and camera poses are computed using featurepoints. Later, bundle adjustment (BA) is applied to optimizethe overall structure and poses by minimizing the reprojec-tion error. One of the challenges in SfM is the presence ofoutliers in BA. In reality, the outliers from either the mis-matched features or the incorrect pose initialization producelarge error. BA distributes this error across all structures andposes, and results in inaccurate camera pose estimations. Inaddition, the reconstruction systems usually reject the imageinput in a featureless region, because feature-based poseestimation sometimes fails at these regions. On the other

The authors are with the Robotics Institute, Carnegie Mel-lon University, Pittsburgh, PA 15213, USA. {yiy4, weidong,kaess}@andrew.cmu.edu

(a) Augmented-ICL-lr1 [6] top view via splattedrendering

(b) TUM fr3 [8] point cloud office scene

Fig. 1. Reconstructions results for a synthetic and a real sequence

hand, systems that are dedicated to generating detailed localgeometry and accurate short-term odometry in a room-sizescene, such as ElasticFusion [5], often fail at large scaleoperations due to tracking failures. Other offline or onlinedense RGB-D reconstruction systems such as [6] and [7]produce fairly accurate models, but often at the cost ofintensive computation and time.

In the paper, we propose a method that combines theadvantages from SfM and SLAM to achieve high fidelityand accuracy in both local and global geometry in a large-scale 3D reconstruction using an RGB-D camera, as shownin Fig. I. This is achieved by using a factor graph-based

3D Landmark2D FeatureCorrespondence

<latexit sha1_base64="sThDlhVk4Xg7JmwruIPzAVepvZw=">AAACH3icbVBNSyNBFOzx29F143r00hgET2EmC7t7FJXFgwcFo0ImhDc9L5MmPd1D9xshBP+JF/+KFw+KiDf/jZ2Yg18FDdVV79FdlZZKOoqil2Bmdm5+YXFpOVxZ/bH2s7b+68yZygpsCaOMvUjBoZIaWyRJ4UVpEYpU4Xk62B/755donTT6lIYldgrItexJAeSlbu1PYiHPMbMy71P4+4Afgc4KsIMkCZsH/D8CVRb9Zd9Yi640OkMtsFurR41oAv6VxFNSZ1Mcd2vPSWZEVaAmocC5dhyV1BmBJSkUXoVJ5bAEMYAc255qKNB1RpN8V3zbKxnvGeuPJj5R32+MoHBuWKR+sgDqu8/eWPzOa1fU+9cZSV1W5FO9PdSrFCfDx2XxTFoUpIaegLDS/5WLPlgQ5CsNfQnx58hfyVmzEUeN+KRZ392b1rHENtkW22Ex+8t22SE7Zi0m2DW7ZffsIbgJ7oLH4OltdCaY7mywDwheXgGWVaIU</latexit><latexit sha1_base64="sThDlhVk4Xg7JmwruIPzAVepvZw=">AAACH3icbVBNSyNBFOzx29F143r00hgET2EmC7t7FJXFgwcFo0ImhDc9L5MmPd1D9xshBP+JF/+KFw+KiDf/jZ2Yg18FDdVV79FdlZZKOoqil2Bmdm5+YXFpOVxZ/bH2s7b+68yZygpsCaOMvUjBoZIaWyRJ4UVpEYpU4Xk62B/755donTT6lIYldgrItexJAeSlbu1PYiHPMbMy71P4+4Afgc4KsIMkCZsH/D8CVRb9Zd9Yi640OkMtsFurR41oAv6VxFNSZ1Mcd2vPSWZEVaAmocC5dhyV1BmBJSkUXoVJ5bAEMYAc255qKNB1RpN8V3zbKxnvGeuPJj5R32+MoHBuWKR+sgDqu8/eWPzOa1fU+9cZSV1W5FO9PdSrFCfDx2XxTFoUpIaegLDS/5WLPlgQ5CsNfQnx58hfyVmzEUeN+KRZ392b1rHENtkW22Ex+8t22SE7Zi0m2DW7ZffsIbgJ7oLH4OltdCaY7mywDwheXgGWVaIU</latexit><latexit sha1_base64="sThDlhVk4Xg7JmwruIPzAVepvZw=">AAACH3icbVBNSyNBFOzx29F143r00hgET2EmC7t7FJXFgwcFo0ImhDc9L5MmPd1D9xshBP+JF/+KFw+KiDf/jZ2Yg18FDdVV79FdlZZKOoqil2Bmdm5+YXFpOVxZ/bH2s7b+68yZygpsCaOMvUjBoZIaWyRJ4UVpEYpU4Xk62B/755donTT6lIYldgrItexJAeSlbu1PYiHPMbMy71P4+4Afgc4KsIMkCZsH/D8CVRb9Zd9Yi640OkMtsFurR41oAv6VxFNSZ1Mcd2vPSWZEVaAmocC5dhyV1BmBJSkUXoVJ5bAEMYAc255qKNB1RpN8V3zbKxnvGeuPJj5R32+MoHBuWKR+sgDqu8/eWPzOa1fU+9cZSV1W5FO9PdSrFCfDx2XxTFoUpIaegLDS/5WLPlgQ5CsNfQnx58hfyVmzEUeN+KRZ392b1rHENtkW22Ex+8t22SE7Zi0m2DW7ZffsIbgJ7oLH4OltdCaY7mywDwheXgGWVaIU</latexit><latexit sha1_base64="sThDlhVk4Xg7JmwruIPzAVepvZw=">AAACH3icbVBNSyNBFOzx29F143r00hgET2EmC7t7FJXFgwcFo0ImhDc9L5MmPd1D9xshBP+JF/+KFw+KiDf/jZ2Yg18FDdVV79FdlZZKOoqil2Bmdm5+YXFpOVxZ/bH2s7b+68yZygpsCaOMvUjBoZIaWyRJ4UVpEYpU4Xk62B/755donTT6lIYldgrItexJAeSlbu1PYiHPMbMy71P4+4Afgc4KsIMkCZsH/D8CVRb9Zd9Yi640OkMtsFurR41oAv6VxFNSZ1Mcd2vPSWZEVaAmocC5dhyV1BmBJSkUXoVJ5bAEMYAc255qKNB1RpN8V3zbKxnvGeuPJj5R32+MoHBuWKR+sgDqu8/eWPzOa1fU+9cZSV1W5FO9PdSrFCfDx2XxTFoUpIaegLDS/5WLPlgQ5CsNfQnx58hfyVmzEUeN+KRZ392b1rHENtkW22Ex+8t22SE7Zi0m2DW7ZffsIbgJ7oLH4OltdCaY7mywDwheXgGWVaIU</latexit>





3D PointRGB-D PixelRe-projection

<latexit sha1_base64="a17jbCcUbFH+OzAL/O8FhW7dKxg=">AAACHHicbVDLSgMxFM34rOOr6tJNsAhuLDN1oUupgi6r2Ad0imTS22lsJhmSjFhKP8SNv+LGhSJuXAj+jZm2C229EDj3nHu5OSdMONPG876dufmFxaXl3Iq7ura+sZnf2q5pmSoKVSq5VI2QaOBMQNUww6GRKCBxyKEe9s4yvX4PSjMpbkw/gVZMIsE6jBJjqdv8UaBIFEFbsahr3KNzXJFMmCBwry/Kh7ZjD8CzDg4TJe+AjrcKXtEbFZ4F/gQU0KQqt/nPoC1pGoMwlBOtm76XmNaAKMMoh6EbpBoSQnskgqaFgsSgW4ORuSHet0wbd6SyTxg8Yn9vDEisdT8O7WRMTFdPaxn5n9ZMTeekNWAiSQ0IOj7USTk2EmdJ4TZT1i/vW0CoYvavmHaJItTYPF0bgj9teRbUSkXfK/pXpcJpeRJHDu2iPXSAfHSMTtElqqAqougRPaNX9OY8OS/Ou/MxHp1zJjs76E85Xz9Z46BJ</latexit><latexit sha1_base64="a17jbCcUbFH+OzAL/O8FhW7dKxg=">AAACHHicbVDLSgMxFM34rOOr6tJNsAhuLDN1oUupgi6r2Ad0imTS22lsJhmSjFhKP8SNv+LGhSJuXAj+jZm2C229EDj3nHu5OSdMONPG876dufmFxaXl3Iq7ura+sZnf2q5pmSoKVSq5VI2QaOBMQNUww6GRKCBxyKEe9s4yvX4PSjMpbkw/gVZMIsE6jBJjqdv8UaBIFEFbsahr3KNzXJFMmCBwry/Kh7ZjD8CzDg4TJe+AjrcKXtEbFZ4F/gQU0KQqt/nPoC1pGoMwlBOtm76XmNaAKMMoh6EbpBoSQnskgqaFgsSgW4ORuSHet0wbd6SyTxg8Yn9vDEisdT8O7WRMTFdPaxn5n9ZMTeekNWAiSQ0IOj7USTk2EmdJ4TZT1i/vW0CoYvavmHaJItTYPF0bgj9teRbUSkXfK/pXpcJpeRJHDu2iPXSAfHSMTtElqqAqougRPaNX9OY8OS/Ou/MxHp1zJjs76E85Xz9Z46BJ</latexit><latexit sha1_base64="a17jbCcUbFH+OzAL/O8FhW7dKxg=">AAACHHicbVDLSgMxFM34rOOr6tJNsAhuLDN1oUupgi6r2Ad0imTS22lsJhmSjFhKP8SNv+LGhSJuXAj+jZm2C229EDj3nHu5OSdMONPG876dufmFxaXl3Iq7ura+sZnf2q5pmSoKVSq5VI2QaOBMQNUww6GRKCBxyKEe9s4yvX4PSjMpbkw/gVZMIsE6jBJjqdv8UaBIFEFbsahr3KNzXJFMmCBwry/Kh7ZjD8CzDg4TJe+AjrcKXtEbFZ4F/gQU0KQqt/nPoC1pGoMwlBOtm76XmNaAKMMoh6EbpBoSQnskgqaFgsSgW4ORuSHet0wbd6SyTxg8Yn9vDEisdT8O7WRMTFdPaxn5n9ZMTeekNWAiSQ0IOj7USTk2EmdJ4TZT1i/vW0CoYvavmHaJItTYPF0bgj9teRbUSkXfK/pXpcJpeRJHDu2iPXSAfHSMTtElqqAqougRPaNX9OY8OS/Ou/MxHp1zJjs76E85Xz9Z46BJ</latexit><latexit sha1_base64="a17jbCcUbFH+OzAL/O8FhW7dKxg=">AAACHHicbVDLSgMxFM34rOOr6tJNsAhuLDN1oUupgi6r2Ad0imTS22lsJhmSjFhKP8SNv+LGhSJuXAj+jZm2C229EDj3nHu5OSdMONPG876dufmFxaXl3Iq7ura+sZnf2q5pmSoKVSq5VI2QaOBMQNUww6GRKCBxyKEe9s4yvX4PSjMpbkw/gVZMIsE6jBJjqdv8UaBIFEFbsahr3KNzXJFMmCBwry/Kh7ZjD8CzDg4TJe+AjrcKXtEbFZ4F/gQU0KQqt/nPoC1pGoMwlBOtm76XmNaAKMMoh6EbpBoSQnskgqaFgsSgW4ORuSHet0wbd6SyTxg8Yn9vDEisdT8O7WRMTFdPaxn5n9ZMTeekNWAiSQ0IOj7USTk2EmdJ4TZT1i/vW0CoYvavmHaJItTYPF0bgj9teRbUSkXfK/pXpcJpeRJHDu2iPXSAfHSMTtElqqAqougRPaNX9OY8OS/Ou/MxHp1zJjs76E85Xz9Z46BJ</latexit>





Frame-to-frame OdometryBatch Keyframe Pose EstimationPose Factor Graph

<latexit sha1_base64="QfM0nGOD82H0OvdneXhU7z3oddQ=">AAACUHicbVHBTttAEB2HFqhpIcCxl1UjpF6IbC5wjKiAShwIEgGkOIrWm3GyYu21dscVlpVP5MKN7+ilhyLYOEGi0Lns03tvZ2bfxrmSloLgwWssffi4vLL6yV/7/GV9o7m5dWl1YQT2hFbaXMfcopIZ9kiSwuvcIE9jhVfxzY+ZfvULjZU6u6Ayx0HKx5lMpODkqGFzHBHeUpxUx4anuEt6N5kBdjbSKZIpp1Hkv1gOOYkJO8Vybulqi+zIkkzrXq+dtXTMBWnDTgzPJ9NhsxW0g7rYexAuQAsW1R0276ORFkWKGQnFre2HQU6DihuSQuHUjwqLORc3fIx9BzO3kR1UdSBTtuOYEUvc9ERnxGr29Y2Kp9aWaeycbvmJfavNyP9p/YKSg0Els7wgzMR8UFIoRprN0mUjaVCQKh3gwki3KxMTblwS7g98F0L49snvweVeOwza4fleq3O4iGMVvsI3+A4h7EMHfkIXeiDgDn7DX3j07r0/3lPDm1tfTtiGf6rhPwOO+7XQ</latexit><latexit sha1_base64="QfM0nGOD82H0OvdneXhU7z3oddQ=">AAACUHicbVHBTttAEB2HFqhpIcCxl1UjpF6IbC5wjKiAShwIEgGkOIrWm3GyYu21dscVlpVP5MKN7+ilhyLYOEGi0Lns03tvZ2bfxrmSloLgwWssffi4vLL6yV/7/GV9o7m5dWl1YQT2hFbaXMfcopIZ9kiSwuvcIE9jhVfxzY+ZfvULjZU6u6Ayx0HKx5lMpODkqGFzHBHeUpxUx4anuEt6N5kBdjbSKZIpp1Hkv1gOOYkJO8Vybulqi+zIkkzrXq+dtXTMBWnDTgzPJ9NhsxW0g7rYexAuQAsW1R0276ORFkWKGQnFre2HQU6DihuSQuHUjwqLORc3fIx9BzO3kR1UdSBTtuOYEUvc9ERnxGr29Y2Kp9aWaeycbvmJfavNyP9p/YKSg0Els7wgzMR8UFIoRprN0mUjaVCQKh3gwki3KxMTblwS7g98F0L49snvweVeOwza4fleq3O4iGMVvsI3+A4h7EMHfkIXeiDgDn7DX3j07r0/3lPDm1tfTtiGf6rhPwOO+7XQ</latexit><latexit sha1_base64="QfM0nGOD82H0OvdneXhU7z3oddQ=">AAACUHicbVHBTttAEB2HFqhpIcCxl1UjpF6IbC5wjKiAShwIEgGkOIrWm3GyYu21dscVlpVP5MKN7+ilhyLYOEGi0Lns03tvZ2bfxrmSloLgwWssffi4vLL6yV/7/GV9o7m5dWl1YQT2hFbaXMfcopIZ9kiSwuvcIE9jhVfxzY+ZfvULjZU6u6Ayx0HKx5lMpODkqGFzHBHeUpxUx4anuEt6N5kBdjbSKZIpp1Hkv1gOOYkJO8Vybulqi+zIkkzrXq+dtXTMBWnDTgzPJ9NhsxW0g7rYexAuQAsW1R0276ORFkWKGQnFre2HQU6DihuSQuHUjwqLORc3fIx9BzO3kR1UdSBTtuOYEUvc9ERnxGr29Y2Kp9aWaeycbvmJfavNyP9p/YKSg0Els7wgzMR8UFIoRprN0mUjaVCQKh3gwki3KxMTblwS7g98F0L49snvweVeOwza4fleq3O4iGMVvsI3+A4h7EMHfkIXeiDgDn7DX3j07r0/3lPDm1tfTtiGf6rhPwOO+7XQ</latexit><latexit sha1_base64="QfM0nGOD82H0OvdneXhU7z3oddQ=">AAACUHicbVHBTttAEB2HFqhpIcCxl1UjpF6IbC5wjKiAShwIEgGkOIrWm3GyYu21dscVlpVP5MKN7+ilhyLYOEGi0Lns03tvZ2bfxrmSloLgwWssffi4vLL6yV/7/GV9o7m5dWl1YQT2hFbaXMfcopIZ9kiSwuvcIE9jhVfxzY+ZfvULjZU6u6Ayx0HKx5lMpODkqGFzHBHeUpxUx4anuEt6N5kBdjbSKZIpp1Hkv1gOOYkJO8Vybulqi+zIkkzrXq+dtXTMBWnDTgzPJ9NhsxW0g7rYexAuQAsW1R0276ORFkWKGQnFre2HQU6DihuSQuHUjwqLORc3fIx9BzO3kR1UdSBTtuOYEUvc9ERnxGr29Y2Kp9aWaeycbvmJfavNyP9p/YKSg0Els7wgzMR8UFIoRprN0mUjaVCQKh3gwki3KxMTblwS7g98F0L49snvweVeOwza4fleq3O4iGMVvsI3+A4h7EMHfkIXeiDgDn7DX3j07r0/3lPDm1tfTtiGf6rhPwOO+7XQ</latexit>

Frame-to-frame OdometryBatch Keyframe Pose EstimationPose Factor Graph

<latexit sha1_base64="QfM0nGOD82H0OvdneXhU7z3oddQ=">AAACUHicbVHBTttAEB2HFqhpIcCxl1UjpF6IbC5wjKiAShwIEgGkOIrWm3GyYu21dscVlpVP5MKN7+ilhyLYOEGi0Lns03tvZ2bfxrmSloLgwWssffi4vLL6yV/7/GV9o7m5dWl1YQT2hFbaXMfcopIZ9kiSwuvcIE9jhVfxzY+ZfvULjZU6u6Ayx0HKx5lMpODkqGFzHBHeUpxUx4anuEt6N5kBdjbSKZIpp1Hkv1gOOYkJO8Vybulqi+zIkkzrXq+dtXTMBWnDTgzPJ9NhsxW0g7rYexAuQAsW1R0276ORFkWKGQnFre2HQU6DihuSQuHUjwqLORc3fIx9BzO3kR1UdSBTtuOYEUvc9ERnxGr29Y2Kp9aWaeycbvmJfavNyP9p/YKSg0Els7wgzMR8UFIoRprN0mUjaVCQKh3gwki3KxMTblwS7g98F0L49snvweVeOwza4fleq3O4iGMVvsI3+A4h7EMHfkIXeiDgDn7DX3j07r0/3lPDm1tfTtiGf6rhPwOO+7XQ</latexit><latexit sha1_base64="QfM0nGOD82H0OvdneXhU7z3oddQ=">AAACUHicbVHBTttAEB2HFqhpIcCxl1UjpF6IbC5wjKiAShwIEgGkOIrWm3GyYu21dscVlpVP5MKN7+ilhyLYOEGi0Lns03tvZ2bfxrmSloLgwWssffi4vLL6yV/7/GV9o7m5dWl1YQT2hFbaXMfcopIZ9kiSwuvcIE9jhVfxzY+ZfvULjZU6u6Ayx0HKx5lMpODkqGFzHBHeUpxUx4anuEt6N5kBdjbSKZIpp1Hkv1gOOYkJO8Vybulqi+zIkkzrXq+dtXTMBWnDTgzPJ9NhsxW0g7rYexAuQAsW1R0276ORFkWKGQnFre2HQU6DihuSQuHUjwqLORc3fIx9BzO3kR1UdSBTtuOYEUvc9ERnxGr29Y2Kp9aWaeycbvmJfavNyP9p/YKSg0Els7wgzMR8UFIoRprN0mUjaVCQKh3gwki3KxMTblwS7g98F0L49snvweVeOwza4fleq3O4iGMVvsI3+A4h7EMHfkIXeiDgDn7DX3j07r0/3lPDm1tfTtiGf6rhPwOO+7XQ</latexit><latexit sha1_base64="QfM0nGOD82H0OvdneXhU7z3oddQ=">AAACUHicbVHBTttAEB2HFqhpIcCxl1UjpF6IbC5wjKiAShwIEgGkOIrWm3GyYu21dscVlpVP5MKN7+ilhyLYOEGi0Lns03tvZ2bfxrmSloLgwWssffi4vLL6yV/7/GV9o7m5dWl1YQT2hFbaXMfcopIZ9kiSwuvcIE9jhVfxzY+ZfvULjZU6u6Ayx0HKx5lMpODkqGFzHBHeUpxUx4anuEt6N5kBdjbSKZIpp1Hkv1gOOYkJO8Vybulqi+zIkkzrXq+dtXTMBWnDTgzPJ9NhsxW0g7rYexAuQAsW1R0276ORFkWKGQnFre2HQU6DihuSQuHUjwqLORc3fIx9BzO3kR1UdSBTtuOYEUvc9ERnxGr29Y2Kp9aWaeycbvmJfavNyP9p/YKSg0Els7wgzMR8UFIoRprN0mUjaVCQKh3gwki3KxMTblwS7g98F0L49snvweVeOwza4fleq3O4iGMVvsI3+A4h7EMHfkIXeiDgDn7DX3j07r0/3lPDm1tfTtiGf6rhPwOO+7XQ</latexit><latexit sha1_base64="QfM0nGOD82H0OvdneXhU7z3oddQ=">AAACUHicbVHBTttAEB2HFqhpIcCxl1UjpF6IbC5wjKiAShwIEgGkOIrWm3GyYu21dscVlpVP5MKN7+ilhyLYOEGi0Lns03tvZ2bfxrmSloLgwWssffi4vLL6yV/7/GV9o7m5dWl1YQT2hFbaXMfcopIZ9kiSwuvcIE9jhVfxzY+ZfvULjZU6u6Ayx0HKx5lMpODkqGFzHBHeUpxUx4anuEt6N5kBdjbSKZIpp1Hkv1gOOYkJO8Vybulqi+zIkkzrXq+dtXTMBWnDTgzPJ9NhsxW0g7rYexAuQAsW1R0276ORFkWKGQnFre2HQU6DihuSQuHUjwqLORc3fIx9BzO3kR1UdSBTtuOYEUvc9ERnxGr29Y2Kp9aWaeycbvmJfavNyP9p/YKSg0Els7wgzMR8UFIoRprN0mUjaVCQKh3gwki3KxMTblwS7g98F0L49snvweVeOwza4fleq3O4iGMVvsI3+A4h7EMHfkIXeiDgDn7DX3j07r0/3lPDm1tfTtiGf6rhPwOO+7XQ</latexit>

……Pk

i

Pi+1

Pj−1

Pk+1j

s<latexit sha1_base64="N1eK0lTaQQAFA6yHzcECkl4oWJk=">AAAB6HicbVBNS8NAEJ3Ur1q/qh69LBbBU0lEqMeiF48t2A9oQ9lsJ+3azSbsboQS+gu8eFDEqz/Jm//GbZuDtj4YeLw3w8y8IBFcG9f9dgobm1vbO8Xd0t7+weFR+fikreNUMWyxWMSqG1CNgktsGW4EdhOFNAoEdoLJ3dzvPKHSPJYPZpqgH9GR5CFn1FipqQflilt1FyDrxMtJBXI0BuWv/jBmaYTSMEG17nluYvyMKsOZwFmpn2pMKJvQEfYslTRC7WeLQ2fkwipDEsbKljRkof6eyGik9TQKbGdEzVivenPxP6+XmvDGz7hMUoOSLReFqSAmJvOvyZArZEZMLaFMcXsrYWOqKDM2m5INwVt9eZ20r6qeW/Wa15X6bR5HEc7gHC7BgxrU4R4a0AIGCM/wCm/Oo/PivDsfy9aCk8+cwh84nz/epYz3</latexit><latexit sha1_base64="N1eK0lTaQQAFA6yHzcECkl4oWJk=">AAAB6HicbVBNS8NAEJ3Ur1q/qh69LBbBU0lEqMeiF48t2A9oQ9lsJ+3azSbsboQS+gu8eFDEqz/Jm//GbZuDtj4YeLw3w8y8IBFcG9f9dgobm1vbO8Xd0t7+weFR+fikreNUMWyxWMSqG1CNgktsGW4EdhOFNAoEdoLJ3dzvPKHSPJYPZpqgH9GR5CFn1FipqQflilt1FyDrxMtJBXI0BuWv/jBmaYTSMEG17nluYvyMKsOZwFmpn2pMKJvQEfYslTRC7WeLQ2fkwipDEsbKljRkof6eyGik9TQKbGdEzVivenPxP6+XmvDGz7hMUoOSLReFqSAmJvOvyZArZEZMLaFMcXsrYWOqKDM2m5INwVt9eZ20r6qeW/Wa15X6bR5HEc7gHC7BgxrU4R4a0AIGCM/wCm/Oo/PivDsfy9aCk8+cwh84nz/epYz3</latexit><latexit sha1_base64="N1eK0lTaQQAFA6yHzcECkl4oWJk=">AAAB6HicbVBNS8NAEJ3Ur1q/qh69LBbBU0lEqMeiF48t2A9oQ9lsJ+3azSbsboQS+gu8eFDEqz/Jm//GbZuDtj4YeLw3w8y8IBFcG9f9dgobm1vbO8Xd0t7+weFR+fikreNUMWyxWMSqG1CNgktsGW4EdhOFNAoEdoLJ3dzvPKHSPJYPZpqgH9GR5CFn1FipqQflilt1FyDrxMtJBXI0BuWv/jBmaYTSMEG17nluYvyMKsOZwFmpn2pMKJvQEfYslTRC7WeLQ2fkwipDEsbKljRkof6eyGik9TQKbGdEzVivenPxP6+XmvDGz7hMUoOSLReFqSAmJvOvyZArZEZMLaFMcXsrYWOqKDM2m5INwVt9eZ20r6qeW/Wa15X6bR5HEc7gHC7BgxrU4R4a0AIGCM/wCm/Oo/PivDsfy9aCk8+cwh84nz/epYz3</latexit><latexit sha1_base64="N1eK0lTaQQAFA6yHzcECkl4oWJk=">AAAB6HicbVBNS8NAEJ3Ur1q/qh69LBbBU0lEqMeiF48t2A9oQ9lsJ+3azSbsboQS+gu8eFDEqz/Jm//GbZuDtj4YeLw3w8y8IBFcG9f9dgobm1vbO8Xd0t7+weFR+fikreNUMWyxWMSqG1CNgktsGW4EdhOFNAoEdoLJ3dzvPKHSPJYPZpqgH9GR5CFn1FipqQflilt1FyDrxMtJBXI0BuWv/jBmaYTSMEG17nluYvyMKsOZwFmpn2pMKJvQEfYslTRC7WeLQ2fkwipDEsbKljRkof6eyGik9TQKbGdEzVivenPxP6+XmvDGz7hMUoOSLReFqSAmJvOvyZArZEZMLaFMcXsrYWOqKDM2m5INwVt9eZ20r6qeW/Wa15X6bR5HEc7gHC7BgxrU4R4a0AIGCM/wCm/Oo/PivDsfy9aCk8+cwh84nz/epYz3</latexit>

i-th Normal Frame and k-th Keyframei-th Normal FrameOdometryKey Frame Pose Prior

<latexit sha1_base64="i2f/E+AAJ6eKaTDIzzDZwfY2W8g=">AAACcnicdVFda9RAFJ2kVevWj9Xii4JOXQQfdEn6oo+lQikIuoLbFjbLMpncZIedjzBzI4aQH9C/55u/wpf+gE62gdpWL8xwOGfOvTNn0lIKh1H0Owg3Nu/cvbd1f7D94OGjx8MnT4+dqSyHKTfS2NOUOZBCwxQFSjgtLTCVSjhJV586/eQHWCeM/o51CXPFCi1ywRl6ajE8SywrCsisKJY4EO8ThJ/Y4JJ+MVYxSQ8tU0CZzlq6uhI/Q513Qpsk//N00qXwNTMK0NZXjLf3jSfG+c0KY9vFcBSNo3XR2yDuwYj0NVkMfyWZ4ZUCjVwy52ZxVOK8YRYFl9AOkspByfiKFTDzUPt5bt6sI2vpG89kNDfWL410zf7taJhyrlapP6kYLt1NrSP/pc0qzD/OG6HLCkHzy0F5JSka2uVPM2GBo6w9YNwKf1fKl8wyjv6XBj6E+OaTb4PjvXEcjeNve6P9gz6OLfKCvCZvSUw+kH1yRCZkSjj5EzwLXgavgvPwebgb9tmFQe/ZIdcqfHcBMsq+bA==</latexit><latexit sha1_base64="i2f/E+AAJ6eKaTDIzzDZwfY2W8g=">AAACcnicdVFda9RAFJ2kVevWj9Xii4JOXQQfdEn6oo+lQikIuoLbFjbLMpncZIedjzBzI4aQH9C/55u/wpf+gE62gdpWL8xwOGfOvTNn0lIKh1H0Owg3Nu/cvbd1f7D94OGjx8MnT4+dqSyHKTfS2NOUOZBCwxQFSjgtLTCVSjhJV586/eQHWCeM/o51CXPFCi1ywRl6ajE8SywrCsisKJY4EO8ThJ/Y4JJ+MVYxSQ8tU0CZzlq6uhI/Q513Qpsk//N00qXwNTMK0NZXjLf3jSfG+c0KY9vFcBSNo3XR2yDuwYj0NVkMfyWZ4ZUCjVwy52ZxVOK8YRYFl9AOkspByfiKFTDzUPt5bt6sI2vpG89kNDfWL410zf7taJhyrlapP6kYLt1NrSP/pc0qzD/OG6HLCkHzy0F5JSka2uVPM2GBo6w9YNwKf1fKl8wyjv6XBj6E+OaTb4PjvXEcjeNve6P9gz6OLfKCvCZvSUw+kH1yRCZkSjj5EzwLXgavgvPwebgb9tmFQe/ZIdcqfHcBMsq+bA==</latexit><latexit sha1_base64="i2f/E+AAJ6eKaTDIzzDZwfY2W8g=">AAACcnicdVFda9RAFJ2kVevWj9Xii4JOXQQfdEn6oo+lQikIuoLbFjbLMpncZIedjzBzI4aQH9C/55u/wpf+gE62gdpWL8xwOGfOvTNn0lIKh1H0Owg3Nu/cvbd1f7D94OGjx8MnT4+dqSyHKTfS2NOUOZBCwxQFSjgtLTCVSjhJV586/eQHWCeM/o51CXPFCi1ywRl6ajE8SywrCsisKJY4EO8ThJ/Y4JJ+MVYxSQ8tU0CZzlq6uhI/Q513Qpsk//N00qXwNTMK0NZXjLf3jSfG+c0KY9vFcBSNo3XR2yDuwYj0NVkMfyWZ4ZUCjVwy52ZxVOK8YRYFl9AOkspByfiKFTDzUPt5bt6sI2vpG89kNDfWL410zf7taJhyrlapP6kYLt1NrSP/pc0qzD/OG6HLCkHzy0F5JSka2uVPM2GBo6w9YNwKf1fKl8wyjv6XBj6E+OaTb4PjvXEcjeNve6P9gz6OLfKCvCZvSUw+kH1yRCZkSjj5EzwLXgavgvPwebgb9tmFQe/ZIdcqfHcBMsq+bA==</latexit><latexit sha1_base64="i2f/E+AAJ6eKaTDIzzDZwfY2W8g=">AAACcnicdVFda9RAFJ2kVevWj9Xii4JOXQQfdEn6oo+lQikIuoLbFjbLMpncZIedjzBzI4aQH9C/55u/wpf+gE62gdpWL8xwOGfOvTNn0lIKh1H0Owg3Nu/cvbd1f7D94OGjx8MnT4+dqSyHKTfS2NOUOZBCwxQFSjgtLTCVSjhJV586/eQHWCeM/o51CXPFCi1ywRl6ajE8SywrCsisKJY4EO8ThJ/Y4JJ+MVYxSQ8tU0CZzlq6uhI/Q513Qpsk//N00qXwNTMK0NZXjLf3jSfG+c0KY9vFcBSNo3XR2yDuwYj0NVkMfyWZ4ZUCjVwy52ZxVOK8YRYFl9AOkspByfiKFTDzUPt5bt6sI2vpG89kNDfWL410zf7taJhyrlapP6kYLt1NrSP/pc0qzD/OG6HLCkHzy0F5JSka2uVPM2GBo6w9YNwKf1fKl8wyjv6XBj6E+OaTb4PjvXEcjeNve6P9gz6OLfKCvCZvSUw+kH1yRCZkSjj5EzwLXgavgvPwebgb9tmFQe/ZIdcqfHcBMsq+bA==</latexit>

i-th Normal Frame and k-th Keyframei-th Normal FrameOdometryKey Frame Pose Prior

<latexit sha1_base64="i2f/E+AAJ6eKaTDIzzDZwfY2W8g=">AAACcnicdVFda9RAFJ2kVevWj9Xii4JOXQQfdEn6oo+lQikIuoLbFjbLMpncZIedjzBzI4aQH9C/55u/wpf+gE62gdpWL8xwOGfOvTNn0lIKh1H0Owg3Nu/cvbd1f7D94OGjx8MnT4+dqSyHKTfS2NOUOZBCwxQFSjgtLTCVSjhJV586/eQHWCeM/o51CXPFCi1ywRl6ajE8SywrCsisKJY4EO8ThJ/Y4JJ+MVYxSQ8tU0CZzlq6uhI/Q513Qpsk//N00qXwNTMK0NZXjLf3jSfG+c0KY9vFcBSNo3XR2yDuwYj0NVkMfyWZ4ZUCjVwy52ZxVOK8YRYFl9AOkspByfiKFTDzUPt5bt6sI2vpG89kNDfWL410zf7taJhyrlapP6kYLt1NrSP/pc0qzD/OG6HLCkHzy0F5JSka2uVPM2GBo6w9YNwKf1fKl8wyjv6XBj6E+OaTb4PjvXEcjeNve6P9gz6OLfKCvCZvSUw+kH1yRCZkSjj5EzwLXgavgvPwebgb9tmFQe/ZIdcqfHcBMsq+bA==</latexit><latexit sha1_base64="i2f/E+AAJ6eKaTDIzzDZwfY2W8g=">AAACcnicdVFda9RAFJ2kVevWj9Xii4JOXQQfdEn6oo+lQikIuoLbFjbLMpncZIedjzBzI4aQH9C/55u/wpf+gE62gdpWL8xwOGfOvTNn0lIKh1H0Owg3Nu/cvbd1f7D94OGjx8MnT4+dqSyHKTfS2NOUOZBCwxQFSjgtLTCVSjhJV586/eQHWCeM/o51CXPFCi1ywRl6ajE8SywrCsisKJY4EO8ThJ/Y4JJ+MVYxSQ8tU0CZzlq6uhI/Q513Qpsk//N00qXwNTMK0NZXjLf3jSfG+c0KY9vFcBSNo3XR2yDuwYj0NVkMfyWZ4ZUCjVwy52ZxVOK8YRYFl9AOkspByfiKFTDzUPt5bt6sI2vpG89kNDfWL410zf7taJhyrlapP6kYLt1NrSP/pc0qzD/OG6HLCkHzy0F5JSka2uVPM2GBo6w9YNwKf1fKl8wyjv6XBj6E+OaTb4PjvXEcjeNve6P9gz6OLfKCvCZvSUw+kH1yRCZkSjj5EzwLXgavgvPwebgb9tmFQe/ZIdcqfHcBMsq+bA==</latexit><latexit sha1_base64="i2f/E+AAJ6eKaTDIzzDZwfY2W8g=">AAACcnicdVFda9RAFJ2kVevWj9Xii4JOXQQfdEn6oo+lQikIuoLbFjbLMpncZIedjzBzI4aQH9C/55u/wpf+gE62gdpWL8xwOGfOvTNn0lIKh1H0Owg3Nu/cvbd1f7D94OGjx8MnT4+dqSyHKTfS2NOUOZBCwxQFSjgtLTCVSjhJV586/eQHWCeM/o51CXPFCi1ywRl6ajE8SywrCsisKJY4EO8ThJ/Y4JJ+MVYxSQ8tU0CZzlq6uhI/Q513Qpsk//N00qXwNTMK0NZXjLf3jSfG+c0KY9vFcBSNo3XR2yDuwYj0NVkMfyWZ4ZUCjVwy52ZxVOK8YRYFl9AOkspByfiKFTDzUPt5bt6sI2vpG89kNDfWL410zf7taJhyrlapP6kYLt1NrSP/pc0qzD/OG6HLCkHzy0F5JSka2uVPM2GBo6w9YNwKf1fKl8wyjv6XBj6E+OaTb4PjvXEcjeNve6P9gz6OLfKCvCZvSUw+kH1yRCZkSjj5EzwLXgavgvPwebgb9tmFQe/ZIdcqfHcBMsq+bA==</latexit><latexit sha1_base64="i2f/E+AAJ6eKaTDIzzDZwfY2W8g=">AAACcnicdVFda9RAFJ2kVevWj9Xii4JOXQQfdEn6oo+lQikIuoLbFjbLMpncZIedjzBzI4aQH9C/55u/wpf+gE62gdpWL8xwOGfOvTNn0lIKh1H0Owg3Nu/cvbd1f7D94OGjx8MnT4+dqSyHKTfS2NOUOZBCwxQFSjgtLTCVSjhJV586/eQHWCeM/o51CXPFCi1ywRl6ajE8SywrCsisKJY4EO8ThJ/Y4JJ+MVYxSQ8tU0CZzlq6uhI/Q513Qpsk//N00qXwNTMK0NZXjLf3jSfG+c0KY9vFcBSNo3XR2yDuwYj0NVkMfyWZ4ZUCjVwy52ZxVOK8YRYFl9AOkspByfiKFTDzUPt5bt6sI2vpG89kNDfWL410zf7taJhyrlapP6kYLt1NrSP/pc0qzD/OG6HLCkHzy0F5JSka2uVPM2GBo6w9YNwKf1fKl8wyjv6XBj6E+OaTb4PjvXEcjeNve6P9gz6OLfKCvCZvSUw+kH1yRCZkSjj5EzwLXgavgvPwebgb9tmFQe/ZIdcqfHcBMsq+bA==</latexit>

Scale Prior<latexit sha1_base64="3joNh4mSsVbGvtTv8CFqg4esrH8=">AAAB8nicbVBNS8NAEJ3Ur1q/qh69LBbBU0l60WPRi8eK9gPSUDbbTbt0kw27E6GE/gwvHhTx6q/x5r9x2+agrQ8GHu/NMDMvTKUw6LrfTmljc2t7p7xb2ds/ODyqHp90jMo0422mpNK9kBouRcLbKFDyXqo5jUPJu+Hkdu53n7g2QiWPOE15ENNRIiLBKFrJf2BUctLSQulBtebW3QXIOvEKUoMCrUH1qz9ULIt5gkxSY3zPTTHIqUbBJJ9V+pnhKWUTOuK+pQmNuQnyxckzcmGVIYmUtpUgWai/J3IaGzONQ9sZUxybVW8u/uf5GUbXQS6SNEOesOWiKJMEFZn/T4ZCc4ZyagllWthbCRtTTRnalCo2BG/15XXSadQ9t+7dN2rNmyKOMpzBOVyCB1fQhDtoQRsYKHiGV3hz0Hlx3p2PZWvJKWZO4Q+czx/jPpD6</latexit><latexit sha1_base64="3joNh4mSsVbGvtTv8CFqg4esrH8=">AAAB8nicbVBNS8NAEJ3Ur1q/qh69LBbBU0l60WPRi8eK9gPSUDbbTbt0kw27E6GE/gwvHhTx6q/x5r9x2+agrQ8GHu/NMDMvTKUw6LrfTmljc2t7p7xb2ds/ODyqHp90jMo0422mpNK9kBouRcLbKFDyXqo5jUPJu+Hkdu53n7g2QiWPOE15ENNRIiLBKFrJf2BUctLSQulBtebW3QXIOvEKUoMCrUH1qz9ULIt5gkxSY3zPTTHIqUbBJJ9V+pnhKWUTOuK+pQmNuQnyxckzcmGVIYmUtpUgWai/J3IaGzONQ9sZUxybVW8u/uf5GUbXQS6SNEOesOWiKJMEFZn/T4ZCc4ZyagllWthbCRtTTRnalCo2BG/15XXSadQ9t+7dN2rNmyKOMpzBOVyCB1fQhDtoQRsYKHiGV3hz0Hlx3p2PZWvJKWZO4Q+czx/jPpD6</latexit><latexit sha1_base64="3joNh4mSsVbGvtTv8CFqg4esrH8=">AAAB8nicbVBNS8NAEJ3Ur1q/qh69LBbBU0l60WPRi8eK9gPSUDbbTbt0kw27E6GE/gwvHhTx6q/x5r9x2+agrQ8GHu/NMDMvTKUw6LrfTmljc2t7p7xb2ds/ODyqHp90jMo0422mpNK9kBouRcLbKFDyXqo5jUPJu+Hkdu53n7g2QiWPOE15ENNRIiLBKFrJf2BUctLSQulBtebW3QXIOvEKUoMCrUH1qz9ULIt5gkxSY3zPTTHIqUbBJJ9V+pnhKWUTOuK+pQmNuQnyxckzcmGVIYmUtpUgWai/J3IaGzONQ9sZUxybVW8u/uf5GUbXQS6SNEOesOWiKJMEFZn/T4ZCc4ZyagllWthbCRtTTRnalCo2BG/15XXSadQ9t+7dN2rNmyKOMpzBOVyCB1fQhDtoQRsYKHiGV3hz0Hlx3p2PZWvJKWZO4Q+czx/jPpD6</latexit><latexit sha1_base64="3joNh4mSsVbGvtTv8CFqg4esrH8=">AAAB8nicbVBNS8NAEJ3Ur1q/qh69LBbBU0l60WPRi8eK9gPSUDbbTbt0kw27E6GE/gwvHhTx6q/x5r9x2+agrQ8GHu/NMDMvTKUw6LrfTmljc2t7p7xb2ds/ODyqHp90jMo0422mpNK9kBouRcLbKFDyXqo5jUPJu+Hkdu53n7g2QiWPOE15ENNRIiLBKFrJf2BUctLSQulBtebW3QXIOvEKUoMCrUH1qz9ULIt5gkxSY3zPTTHIqUbBJJ9V+pnhKWUTOuK+pQmNuQnyxckzcmGVIYmUtpUgWai/J3IaGzONQ9sZUxybVW8u/uf5GUbXQS6SNEOesOWiKJMEFZn/T4ZCc4ZyagllWthbCRtTTRnalCo2BG/15XXSadQ9t+7dN2rNmyKOMpzBOVyCB1fQhDtoQRsYKHiGV3hz0Hlx3p2PZWvJKWZO4Q+czx/jPpD6</latexit> Pose Scale

<latexit sha1_base64="gAna7ADJoNoAQoqgarb5NoReu5k=">AAAB8XicbVA9SwNBEJ3zM8avqKXNYhCswl0aLYM2lhHNByZH2NvMJUv2do/dPSGE/AsbC0Vs/Td2/hs3yRWa+GDg8d4MM/OiVHBjff/bW1vf2NzaLuwUd/f2Dw5LR8dNozLNsMGUULodUYOCS2xYbgW2U400iQS2otHNzG89oTZcyQc7TjFM6EDymDNqnfRYVwbJPaMCe6WyX/HnIKskyEkZctR7pa9uX7EsQWmZoMZ0Aj+14YRqy5nAabGbGUwpG9EBdhyVNEETTuYXT8m5U/okVtqVtGSu/p6Y0MSYcRK5zoTaoVn2ZuJ/Xiez8VU44TLNLEq2WBRnglhFZu+TPtfIrBg7Qpnm7lbChlRTZl1IRRdCsPzyKmlWK4FfCe6q5dp1HkcBTuEMLiCAS6jBLdShAQwkPMMrvHnGe/HevY9F65qXz5zAH3ifPwl4kHs=</latexit><latexit sha1_base64="gAna7ADJoNoAQoqgarb5NoReu5k=">AAAB8XicbVA9SwNBEJ3zM8avqKXNYhCswl0aLYM2lhHNByZH2NvMJUv2do/dPSGE/AsbC0Vs/Td2/hs3yRWa+GDg8d4MM/OiVHBjff/bW1vf2NzaLuwUd/f2Dw5LR8dNozLNsMGUULodUYOCS2xYbgW2U400iQS2otHNzG89oTZcyQc7TjFM6EDymDNqnfRYVwbJPaMCe6WyX/HnIKskyEkZctR7pa9uX7EsQWmZoMZ0Aj+14YRqy5nAabGbGUwpG9EBdhyVNEETTuYXT8m5U/okVtqVtGSu/p6Y0MSYcRK5zoTaoVn2ZuJ/Xiez8VU44TLNLEq2WBRnglhFZu+TPtfIrBg7Qpnm7lbChlRTZl1IRRdCsPzyKmlWK4FfCe6q5dp1HkcBTuEMLiCAS6jBLdShAQwkPMMrvHnGe/HevY9F65qXz5zAH3ifPwl4kHs=</latexit><latexit sha1_base64="gAna7ADJoNoAQoqgarb5NoReu5k=">AAAB8XicbVA9SwNBEJ3zM8avqKXNYhCswl0aLYM2lhHNByZH2NvMJUv2do/dPSGE/AsbC0Vs/Td2/hs3yRWa+GDg8d4MM/OiVHBjff/bW1vf2NzaLuwUd/f2Dw5LR8dNozLNsMGUULodUYOCS2xYbgW2U400iQS2otHNzG89oTZcyQc7TjFM6EDymDNqnfRYVwbJPaMCe6WyX/HnIKskyEkZctR7pa9uX7EsQWmZoMZ0Aj+14YRqy5nAabGbGUwpG9EBdhyVNEETTuYXT8m5U/okVtqVtGSu/p6Y0MSYcRK5zoTaoVn2ZuJ/Xiez8VU44TLNLEq2WBRnglhFZu+TPtfIrBg7Qpnm7lbChlRTZl1IRRdCsPzyKmlWK4FfCe6q5dp1HkcBTuEMLiCAS6jBLdShAQwkPMMrvHnGe/HevY9F65qXz5zAH3ifPwl4kHs=</latexit><latexit sha1_base64="gAna7ADJoNoAQoqgarb5NoReu5k=">AAAB8XicbVA9SwNBEJ3zM8avqKXNYhCswl0aLYM2lhHNByZH2NvMJUv2do/dPSGE/AsbC0Vs/Td2/hs3yRWa+GDg8d4MM/OiVHBjff/bW1vf2NzaLuwUd/f2Dw5LR8dNozLNsMGUULodUYOCS2xYbgW2U400iQS2otHNzG89oTZcyQc7TjFM6EDymDNqnfRYVwbJPaMCe6WyX/HnIKskyEkZctR7pa9uX7EsQWmZoMZ0Aj+14YRqy5nAabGbGUwpG9EBdhyVNEETTuYXT8m5U/okVtqVtGSu/p6Y0MSYcRK5zoTaoVn2ZuJ/Xiez8VU44TLNLEq2WBRnglhFZu+TPtfIrBg7Qpnm7lbChlRTZl1IRRdCsPzyKmlWK4FfCe6q5dp1HkcBTuEMLiCAS6jBLdShAQwkPMMrvHnGe/HevY9F65qXz5zAH3ifPwl4kHs=</latexit>


Pi

Pi+��1

Pi+�<latexit sha1_base64="EnKwfV6UGaSjXbhYA79/ZOI4rFE=">AAACKXicdVBdS8MwFE39nPWr6qMvwSEI4mhF0MehLz5OcB+wlpKm6RaWpiVJhVH6d3zxr/iioKiv/hHTrQ9z0wOBk3PuTe49QcqoVLb9aSwtr6yurdc2zM2t7Z1da2+/I5NMYNLGCUtEL0CSMMpJW1HFSC8VBMUBI91gdFP63QciJE34vRqnxIvRgNOIYqS05FtNN0ZqGER5q/Cp65oz15yeuky/FKIzp/jPKnyrbjfsCeAicSpSBxVavvXqhgnOYsIVZkjKvmOnysuRUBQzUphuJkmK8AgNSF9TjmIivXyyaQGPtRLCKBH6cAUn6mxHjmIpx3GgK8tp5bxXin95/UxFV15OeZopwvH0oyhjUCWwjA2GVBCs2FgThAXVs0I8RAJhpcM1dQjO/MqLpHPecOyGc3dRb15XcdTAITgCJ8ABl6AJbkELtAEGj+AZvIF348l4MT6Mr2npklH1HIBfML5/AIt9p2c=</latexit><latexit sha1_base64="EnKwfV6UGaSjXbhYA79/ZOI4rFE=">AAACKXicdVBdS8MwFE39nPWr6qMvwSEI4mhF0MehLz5OcB+wlpKm6RaWpiVJhVH6d3zxr/iioKiv/hHTrQ9z0wOBk3PuTe49QcqoVLb9aSwtr6yurdc2zM2t7Z1da2+/I5NMYNLGCUtEL0CSMMpJW1HFSC8VBMUBI91gdFP63QciJE34vRqnxIvRgNOIYqS05FtNN0ZqGER5q/Cp65oz15yeuky/FKIzp/jPKnyrbjfsCeAicSpSBxVavvXqhgnOYsIVZkjKvmOnysuRUBQzUphuJkmK8AgNSF9TjmIivXyyaQGPtRLCKBH6cAUn6mxHjmIpx3GgK8tp5bxXin95/UxFV15OeZopwvH0oyhjUCWwjA2GVBCs2FgThAXVs0I8RAJhpcM1dQjO/MqLpHPecOyGc3dRb15XcdTAITgCJ8ABl6AJbkELtAEGj+AZvIF348l4MT6Mr2npklH1HIBfML5/AIt9p2c=</latexit><latexit sha1_base64="EnKwfV6UGaSjXbhYA79/ZOI4rFE=">AAACKXicdVBdS8MwFE39nPWr6qMvwSEI4mhF0MehLz5OcB+wlpKm6RaWpiVJhVH6d3zxr/iioKiv/hHTrQ9z0wOBk3PuTe49QcqoVLb9aSwtr6yurdc2zM2t7Z1da2+/I5NMYNLGCUtEL0CSMMpJW1HFSC8VBMUBI91gdFP63QciJE34vRqnxIvRgNOIYqS05FtNN0ZqGER5q/Cp65oz15yeuky/FKIzp/jPKnyrbjfsCeAicSpSBxVavvXqhgnOYsIVZkjKvmOnysuRUBQzUphuJkmK8AgNSF9TjmIivXyyaQGPtRLCKBH6cAUn6mxHjmIpx3GgK8tp5bxXin95/UxFV15OeZopwvH0oyhjUCWwjA2GVBCs2FgThAXVs0I8RAJhpcM1dQjO/MqLpHPecOyGc3dRb15XcdTAITgCJ8ABl6AJbkELtAEGj+AZvIF348l4MT6Mr2npklH1HIBfML5/AIt9p2c=</latexit><latexit sha1_base64="EnKwfV6UGaSjXbhYA79/ZOI4rFE=">AAACKXicdVBdS8MwFE39nPWr6qMvwSEI4mhF0MehLz5OcB+wlpKm6RaWpiVJhVH6d3zxr/iioKiv/hHTrQ9z0wOBk3PuTe49QcqoVLb9aSwtr6yurdc2zM2t7Z1da2+/I5NMYNLGCUtEL0CSMMpJW1HFSC8VBMUBI91gdFP63QciJE34vRqnxIvRgNOIYqS05FtNN0ZqGER5q/Cp65oz15yeuky/FKIzp/jPKnyrbjfsCeAicSpSBxVavvXqhgnOYsIVZkjKvmOnysuRUBQzUphuJkmK8AgNSF9TjmIivXyyaQGPtRLCKBH6cAUn6mxHjmIpx3GgK8tp5bxXin95/UxFV15OeZopwvH0oyhjUCWwjA2GVBCs2FgThAXVs0I8RAJhpcM1dQjO/MqLpHPecOyGc3dRb15XcdTAITgCJ8ABl6AJbkELtAEGj+AZvIF348l4MT6Mr2npklH1HIBfML5/AIt9p2c=</latexit>

Pi

Pi+��1


Pi

Pi+��1


Pi

Pi+��1


Pi

Pi+��1


…………

Keyframe PoseFrame Pose

<latexit sha1_base64="Cvl4/CqS+pjYO7XMJ88Zw5FzxP4=">AAACA3icbVDLSsNAFJ34rPEVdaebwSK4Kkk3uiwKIripYB/QhjKZ3rRDJ5MwMxFCKLjxV9y4UMStP+HOv3HaBtHWAwPnnnMvd+4JEs6Udt0va2l5ZXVtvbRhb25t7+w6e/tNFaeSQoPGPJbtgCjgTEBDM82hnUggUcChFYwuJ37rHqRisbjTWQJ+RAaChYwSbaSec3gDWShJBLgeK+h27aufoueU3Yo7BV4kXkHKqEC953x2+zFNIxCacqJUx3MT7edEakY5jO1uqiAhdEQG0DFUmEXKz6c3jPGJUfo4jKV5QuOp+nsiJ5FSWRSYzojooZr3JuJ/XifV4bmfM5GkGgSdLQpTjnWMJ4HgPpNANc8MIVQy81dMh0QSqk1stgnBmz95kTSrFc+teLfVcu2iiKOEjtAxOkUeOkM1dI3qqIEoekBP6AW9Wo/Ws/Vmvc9al6xi5gD9gfXxDdhflvw=</latexit><latexit sha1_base64="Cvl4/CqS+pjYO7XMJ88Zw5FzxP4=">AAACA3icbVDLSsNAFJ34rPEVdaebwSK4Kkk3uiwKIripYB/QhjKZ3rRDJ5MwMxFCKLjxV9y4UMStP+HOv3HaBtHWAwPnnnMvd+4JEs6Udt0va2l5ZXVtvbRhb25t7+w6e/tNFaeSQoPGPJbtgCjgTEBDM82hnUggUcChFYwuJ37rHqRisbjTWQJ+RAaChYwSbaSec3gDWShJBLgeK+h27aufoueU3Yo7BV4kXkHKqEC953x2+zFNIxCacqJUx3MT7edEakY5jO1uqiAhdEQG0DFUmEXKz6c3jPGJUfo4jKV5QuOp+nsiJ5FSWRSYzojooZr3JuJ/XifV4bmfM5GkGgSdLQpTjnWMJ4HgPpNANc8MIVQy81dMh0QSqk1stgnBmz95kTSrFc+teLfVcu2iiKOEjtAxOkUeOkM1dI3qqIEoekBP6AW9Wo/Ws/Vmvc9al6xi5gD9gfXxDdhflvw=</latexit><latexit sha1_base64="Cvl4/CqS+pjYO7XMJ88Zw5FzxP4=">AAACA3icbVDLSsNAFJ34rPEVdaebwSK4Kkk3uiwKIripYB/QhjKZ3rRDJ5MwMxFCKLjxV9y4UMStP+HOv3HaBtHWAwPnnnMvd+4JEs6Udt0va2l5ZXVtvbRhb25t7+w6e/tNFaeSQoPGPJbtgCjgTEBDM82hnUggUcChFYwuJ37rHqRisbjTWQJ+RAaChYwSbaSec3gDWShJBLgeK+h27aufoueU3Yo7BV4kXkHKqEC953x2+zFNIxCacqJUx3MT7edEakY5jO1uqiAhdEQG0DFUmEXKz6c3jPGJUfo4jKV5QuOp+nsiJ5FSWRSYzojooZr3JuJ/XifV4bmfM5GkGgSdLQpTjnWMJ4HgPpNANc8MIVQy81dMh0QSqk1stgnBmz95kTSrFc+teLfVcu2iiKOEjtAxOkUeOkM1dI3qqIEoekBP6AW9Wo/Ws/Vmvc9al6xi5gD9gfXxDdhflvw=</latexit><latexit sha1_base64="Cvl4/CqS+pjYO7XMJ88Zw5FzxP4=">AAACA3icbVDLSsNAFJ34rPEVdaebwSK4Kkk3uiwKIripYB/QhjKZ3rRDJ5MwMxFCKLjxV9y4UMStP+HOv3HaBtHWAwPnnnMvd+4JEs6Udt0va2l5ZXVtvbRhb25t7+w6e/tNFaeSQoPGPJbtgCjgTEBDM82hnUggUcChFYwuJ37rHqRisbjTWQJ+RAaChYwSbaSec3gDWShJBLgeK+h27aufoueU3Yo7BV4kXkHKqEC953x2+zFNIxCacqJUx3MT7edEakY5jO1uqiAhdEQG0DFUmEXKz6c3jPGJUfo4jKV5QuOp+nsiJ5FSWRSYzojooZr3JuJ/XifV4bmfM5GkGgSdLQpTjnWMJ4HgPpNANc8MIVQy81dMh0QSqk1stgnBmz95kTSrFc+teLfVcu2iiKOEjtAxOkUeOkM1dI3qqIEoekBP6AW9Wo/Ws/Vmvc9al6xi5gD9gfXxDdhflvw=</latexit>

Keyframe PoseFrame Pose

<latexit sha1_base64="Cvl4/CqS+pjYO7XMJ88Zw5FzxP4=">AAACA3icbVDLSsNAFJ34rPEVdaebwSK4Kkk3uiwKIripYB/QhjKZ3rRDJ5MwMxFCKLjxV9y4UMStP+HOv3HaBtHWAwPnnnMvd+4JEs6Udt0va2l5ZXVtvbRhb25t7+w6e/tNFaeSQoPGPJbtgCjgTEBDM82hnUggUcChFYwuJ37rHqRisbjTWQJ+RAaChYwSbaSec3gDWShJBLgeK+h27aufoueU3Yo7BV4kXkHKqEC953x2+zFNIxCacqJUx3MT7edEakY5jO1uqiAhdEQG0DFUmEXKz6c3jPGJUfo4jKV5QuOp+nsiJ5FSWRSYzojooZr3JuJ/XifV4bmfM5GkGgSdLQpTjnWMJ4HgPpNANc8MIVQy81dMh0QSqk1stgnBmz95kTSrFc+teLfVcu2iiKOEjtAxOkUeOkM1dI3qqIEoekBP6AW9Wo/Ws/Vmvc9al6xi5gD9gfXxDdhflvw=</latexit><latexit sha1_base64="Cvl4/CqS+pjYO7XMJ88Zw5FzxP4=">AAACA3icbVDLSsNAFJ34rPEVdaebwSK4Kkk3uiwKIripYB/QhjKZ3rRDJ5MwMxFCKLjxV9y4UMStP+HOv3HaBtHWAwPnnnMvd+4JEs6Udt0va2l5ZXVtvbRhb25t7+w6e/tNFaeSQoPGPJbtgCjgTEBDM82hnUggUcChFYwuJ37rHqRisbjTWQJ+RAaChYwSbaSec3gDWShJBLgeK+h27aufoueU3Yo7BV4kXkHKqEC953x2+zFNIxCacqJUx3MT7edEakY5jO1uqiAhdEQG0DFUmEXKz6c3jPGJUfo4jKV5QuOp+nsiJ5FSWRSYzojooZr3JuJ/XifV4bmfM5GkGgSdLQpTjnWMJ4HgPpNANc8MIVQy81dMh0QSqk1stgnBmz95kTSrFc+teLfVcu2iiKOEjtAxOkUeOkM1dI3qqIEoekBP6AW9Wo/Ws/Vmvc9al6xi5gD9gfXxDdhflvw=</latexit><latexit sha1_base64="Cvl4/CqS+pjYO7XMJ88Zw5FzxP4=">AAACA3icbVDLSsNAFJ34rPEVdaebwSK4Kkk3uiwKIripYB/QhjKZ3rRDJ5MwMxFCKLjxV9y4UMStP+HOv3HaBtHWAwPnnnMvd+4JEs6Udt0va2l5ZXVtvbRhb25t7+w6e/tNFaeSQoPGPJbtgCjgTEBDM82hnUggUcChFYwuJ37rHqRisbjTWQJ+RAaChYwSbaSec3gDWShJBLgeK+h27aufoueU3Yo7BV4kXkHKqEC953x2+zFNIxCacqJUx3MT7edEakY5jO1uqiAhdEQG0DFUmEXKz6c3jPGJUfo4jKV5QuOp+nsiJ5FSWRSYzojooZr3JuJ/XifV4bmfM5GkGgSdLQpTjnWMJ4HgPpNANc8MIVQy81dMh0QSqk1stgnBmz95kTSrFc+teLfVcu2iiKOEjtAxOkUeOkM1dI3qqIEoekBP6AW9Wo/Ws/Vmvc9al6xi5gD9gfXxDdhflvw=</latexit><latexit sha1_base64="Cvl4/CqS+pjYO7XMJ88Zw5FzxP4=">AAACA3icbVDLSsNAFJ34rPEVdaebwSK4Kkk3uiwKIripYB/QhjKZ3rRDJ5MwMxFCKLjxV9y4UMStP+HOv3HaBtHWAwPnnnMvd+4JEs6Udt0va2l5ZXVtvbRhb25t7+w6e/tNFaeSQoPGPJbtgCjgTEBDM82hnUggUcChFYwuJ37rHqRisbjTWQJ+RAaChYwSbaSec3gDWShJBLgeK+h27aufoueU3Yo7BV4kXkHKqEC953x2+zFNIxCacqJUx3MT7edEakY5jO1uqiAhdEQG0DFUmEXKz6c3jPGJUfo4jKV5QuOp+nsiJ5FSWRSYzojooZr3JuJ/XifV4bmfM5GkGgSdLQpTjnWMJ4HgPpNANc8MIVQy81dMh0QSqk1stgnBmz95kTSrFc+teLfVcu2iiKOEjtAxOkUeOkM1dI3qqIEoekBP6AW9Wo/Ws/Vmvc9al6xi5gD9gfXxDdhflvw=</latexit>

⇠ij<latexit sha1_base64="8Bz3RkOn6DeQE+uf+qy+2QYiwmM=">AAAB73icbVBNS8NAEJ3Ur1q/qh69LBbBU0lE0GPRi8cK9gPaUDbbTbt2s4m7E7GE/gkvHhTx6t/x5r9x2+agrQ8GHu/NMDMvSKQw6LrfTmFldW19o7hZ2tre2d0r7x80TZxqxhsslrFuB9RwKRRvoEDJ24nmNAokbwWj66nfeuTaiFjd4TjhfkQHSoSCUbRSu/skepm4n/TKFbfqzkCWiZeTCuSo98pf3X7M0ogrZJIa0/HcBP2MahRM8kmpmxqeUDaiA96xVNGIGz+b3TshJ1bpkzDWthSSmfp7IqORMeMosJ0RxaFZ9Kbif14nxfDSz4RKUuSKzReFqSQYk+nzpC80ZyjHllCmhb2VsCHVlKGNqGRD8BZfXibNs6rnVr3b80rtKo+jCEdwDKfgwQXU4Abq0AAGEp7hFd6cB+fFeXc+5q0FJ585hD9wPn8AZr+QMQ==</latexit><latexit sha1_base64="8Bz3RkOn6DeQE+uf+qy+2QYiwmM=">AAAB73icbVBNS8NAEJ3Ur1q/qh69LBbBU0lE0GPRi8cK9gPaUDbbTbt2s4m7E7GE/gkvHhTx6t/x5r9x2+agrQ8GHu/NMDMvSKQw6LrfTmFldW19o7hZ2tre2d0r7x80TZxqxhsslrFuB9RwKRRvoEDJ24nmNAokbwWj66nfeuTaiFjd4TjhfkQHSoSCUbRSu/skepm4n/TKFbfqzkCWiZeTCuSo98pf3X7M0ogrZJIa0/HcBP2MahRM8kmpmxqeUDaiA96xVNGIGz+b3TshJ1bpkzDWthSSmfp7IqORMeMosJ0RxaFZ9Kbif14nxfDSz4RKUuSKzReFqSQYk+nzpC80ZyjHllCmhb2VsCHVlKGNqGRD8BZfXibNs6rnVr3b80rtKo+jCEdwDKfgwQXU4Abq0AAGEp7hFd6cB+fFeXc+5q0FJ585hD9wPn8AZr+QMQ==</latexit><latexit sha1_base64="8Bz3RkOn6DeQE+uf+qy+2QYiwmM=">AAAB73icbVBNS8NAEJ3Ur1q/qh69LBbBU0lE0GPRi8cK9gPaUDbbTbt2s4m7E7GE/gkvHhTx6t/x5r9x2+agrQ8GHu/NMDMvSKQw6LrfTmFldW19o7hZ2tre2d0r7x80TZxqxhsslrFuB9RwKRRvoEDJ24nmNAokbwWj66nfeuTaiFjd4TjhfkQHSoSCUbRSu/skepm4n/TKFbfqzkCWiZeTCuSo98pf3X7M0ogrZJIa0/HcBP2MahRM8kmpmxqeUDaiA96xVNGIGz+b3TshJ1bpkzDWthSSmfp7IqORMeMosJ0RxaFZ9Kbif14nxfDSz4RKUuSKzReFqSQYk+nzpC80ZyjHllCmhb2VsCHVlKGNqGRD8BZfXibNs6rnVr3b80rtKo+jCEdwDKfgwQXU4Abq0AAGEp7hFd6cB+fFeXc+5q0FJ585hD9wPn8AZr+QMQ==</latexit><latexit sha1_base64="8Bz3RkOn6DeQE+uf+qy+2QYiwmM=">AAAB73icbVBNS8NAEJ3Ur1q/qh69LBbBU0lE0GPRi8cK9gPaUDbbTbt2s4m7E7GE/gkvHhTx6t/x5r9x2+agrQ8GHu/NMDMvSKQw6LrfTmFldW19o7hZ2tre2d0r7x80TZxqxhsslrFuB9RwKRRvoEDJ24nmNAokbwWj66nfeuTaiFjd4TjhfkQHSoSCUbRSu/skepm4n/TKFbfqzkCWiZeTCuSo98pf3X7M0ogrZJIa0/HcBP2MahRM8kmpmxqeUDaiA96xVNGIGz+b3TshJ1bpkzDWthSSmfp7IqORMeMosJ0RxaFZ9Kbif14nxfDSz4RKUuSKzReFqSQYk+nzpC80ZyjHllCmhb2VsCHVlKGNqGRD8BZfXibNs6rnVr3b80rtKo+jCEdwDKfgwQXU4Abq0AAGEp7hFd6cB+fFeXc+5q0FJ585hD9wPn8AZr+QMQ==</latexit>

Frame-to-model Odometry<latexit sha1_base64="mwsn+GDToYcBz/N19D1ZZVsPlho=">AAACCXicbVDJSgNBEO1xN26jHr00BsFLwowXPQYF8aaCWSAJoaenJjZ2Tw/dNWIYcvXir3jxoIhX/8Cbf2NnObg9KHi8V0VVvSiTwmIQfHozs3PzC4tLy6WV1bX1DX9zq2F1bjjUuZbatCJmQYoU6ihQQiszwFQkoRndnIz85i0YK3R6hYMMuor1U5EIztBJPZ92EO4wSopTwxRUUFeUjkHS81grQDMY9vxyUA3GoH9JOCVlMsVFz//oxJrnClLkklnbDoMMuwUzKLiEYamTW8gYv2F9aDuaurW2W4w/GdI9p8Q00cZVinSsfp8omLJ2oCLXqRhe29/eSPzPa+eYHHULkWY5Qsoni5JcUtR0FAuNhQGOcuAI40a4Wym/ZoZxdOGVXAjh75f/ksZBNQyq4eVBuXY8jWOJ7JBdsk9Cckhq5IxckDrh5J48kmfy4j14T96r9zZpnfGmM9vkB7z3L+2FmnY=</latexit><latexit sha1_base64="mwsn+GDToYcBz/N19D1ZZVsPlho=">AAACCXicbVDJSgNBEO1xN26jHr00BsFLwowXPQYF8aaCWSAJoaenJjZ2Tw/dNWIYcvXir3jxoIhX/8Cbf2NnObg9KHi8V0VVvSiTwmIQfHozs3PzC4tLy6WV1bX1DX9zq2F1bjjUuZbatCJmQYoU6ihQQiszwFQkoRndnIz85i0YK3R6hYMMuor1U5EIztBJPZ92EO4wSopTwxRUUFeUjkHS81grQDMY9vxyUA3GoH9JOCVlMsVFz//oxJrnClLkklnbDoMMuwUzKLiEYamTW8gYv2F9aDuaurW2W4w/GdI9p8Q00cZVinSsfp8omLJ2oCLXqRhe29/eSPzPa+eYHHULkWY5Qsoni5JcUtR0FAuNhQGOcuAI40a4Wym/ZoZxdOGVXAjh75f/ksZBNQyq4eVBuXY8jWOJ7JBdsk9Cckhq5IxckDrh5J48kmfy4j14T96r9zZpnfGmM9vkB7z3L+2FmnY=</latexit><latexit sha1_base64="mwsn+GDToYcBz/N19D1ZZVsPlho=">AAACCXicbVDJSgNBEO1xN26jHr00BsFLwowXPQYF8aaCWSAJoaenJjZ2Tw/dNWIYcvXir3jxoIhX/8Cbf2NnObg9KHi8V0VVvSiTwmIQfHozs3PzC4tLy6WV1bX1DX9zq2F1bjjUuZbatCJmQYoU6ihQQiszwFQkoRndnIz85i0YK3R6hYMMuor1U5EIztBJPZ92EO4wSopTwxRUUFeUjkHS81grQDMY9vxyUA3GoH9JOCVlMsVFz//oxJrnClLkklnbDoMMuwUzKLiEYamTW8gYv2F9aDuaurW2W4w/GdI9p8Q00cZVinSsfp8omLJ2oCLXqRhe29/eSPzPa+eYHHULkWY5Qsoni5JcUtR0FAuNhQGOcuAI40a4Wym/ZoZxdOGVXAjh75f/ksZBNQyq4eVBuXY8jWOJ7JBdsk9Cckhq5IxckDrh5J48kmfy4j14T96r9zZpnfGmM9vkB7z3L+2FmnY=</latexit><latexit sha1_base64="mwsn+GDToYcBz/N19D1ZZVsPlho=">AAACCXicbVDJSgNBEO1xN26jHr00BsFLwowXPQYF8aaCWSAJoaenJjZ2Tw/dNWIYcvXir3jxoIhX/8Cbf2NnObg9KHi8V0VVvSiTwmIQfHozs3PzC4tLy6WV1bX1DX9zq2F1bjjUuZbatCJmQYoU6ihQQiszwFQkoRndnIz85i0YK3R6hYMMuor1U5EIztBJPZ92EO4wSopTwxRUUFeUjkHS81grQDMY9vxyUA3GoH9JOCVlMsVFz//oxJrnClLkklnbDoMMuwUzKLiEYamTW8gYv2F9aDuaurW2W4w/GdI9p8Q00cZVinSsfp8omLJ2oCLXqRhe29/eSPzPa+eYHHULkWY5Qsoni5JcUtR0FAuNhQGOcuAI40a4Wym/ZoZxdOGVXAjh75f/ksZBNQyq4eVBuXY8jWOJ7JBdsk9Cckhq5IxckDrh5J48kmfy4j14T96r9zZpnfGmM9vkB7z3L+2FmnY=</latexit>

Fig. 2. Frame-to-model odometry is solved using joint ICP and photometric optimization, as shown in the picture in the top left. The keyframe pose priorsare solved using SfM, as shown in the top right. The factor graph in the bottom demonstrates how to combine the keyframe priors with the odometry.

optimization [9] of the camera poses obtained from bothSfM and a dense mapping SLAM system. Specifically, a fewframes are selected from the color images as keyframes, andprocessed using SfM algorithms such as OpenMVG [1], [10],Theia [11] or COLMAP [12]. Then, keyframes poses areinterpolated and combined with the RGB-D camera trackingfrom ElasticFusion [5], a state-of-the-art dense mappingsystem, to recover the overall trajectory via factor graphoptimization. Finally, an online surfel-based fusion methodused in ElasticFusion is applied to generate the final modelbased on the optimized camera poses. Our method uses theRGB-D odometry to enhance the local geometry consistency,and SfM to maximize the global geometry consistency.

There are two main contributions of this paper:

• We introduce the framework to combine SfM andSLAM in a factor graph formulation. Although ourmethod requires offline pose optimization, it producesmore accurate model geometry comparing to the onlinemethods, and is much less time-consuming comparingto other offline reconstruction systems.

• We detail our methods in combining the result from SfMand SLAM. We conduct both real world experimentsand tests on a synthetic dataset to provide a thoroughanalysis of the system runtime and performance. Ourevaluation includes comparisons to both offline SfM-MVS pipeline and online dense mapping. The resultsshow that our method outperforms both online densemapping and offline 3D reconstructions in various met-rics.

II. RELATED WORK

The problem of both local and global geometric consis-tency has been addressed in many large scale offline SfM-MVS pipelines and online dense mapping systems. Oneof the most popular approaches is based on the idea ofdivide-and-conquer. Zhu et al. [13] and Yao et al. [14] haveintroduced the technique of locally readjusting the cameraposes to improve the reconstruction quality. This methodis effective in terms of smoothing out the rough surfacesand recovering more accurate local geometry. However,these methods still suffer from pre-defined heuristics suchas the size of local camera group. Built upon these ideas,many state-of-the-art SfM pipelines such as COLMAP bySchonberger et al. [12] and OpenMVG by Moulon et al. [1]have developed complete systems that apply local refinementto recover better local reconstruction quality. These state-of-the-art systems provide globally accurate camera poseestimation. However, structure estimation from multi-viewgeometry is usually prone to outliers, and procedures suchas patch matching in MVS are time-consuming. In addition,the resulting mesh from SfM-MVS pipeline usually requirespost-processing that is not only time-consuming but alsolabor intensive.

As RGB-D sensors become popular, many offline systemsare developed to use the depth information to generatehighly accurate 3D model. For example, Zhou and Koltun[15] propose to construct small fragments of a large scene,and then use volumetric registration to combine these smallfragments. Also, Zhou et al. [16] apply elastic regularizationto merge all fragments. Apart from these methods, Choi et

Fig. 3. System block diagram. The top of each block shows the sub-components for each system, and the bottom of the block shows the output ofthe corresponding system. From a set of RGB-D images, a set of keyframes K is selected, and processed using the SfM pipeline. The resulting set ofkeyframe poses Pk and the corresponding covariance Σk are generated; these are treated as strong priors in the later optimization. Frame-to-model trackingprovides odometry using joint optimization of ICP and photometric information. In preparation to the joint optimization of prior and odometry, keyframesare interpolated to provide a good initial value for the optimization. We optimize the poses incrementally as the new odometry becomes available, andfuse the new surfels from each frame into the existing model.

al. [6] apply a similar idea of constructing many overlappingsmall fragments, and then use Iterative Closest Point (ICP)to register the fragments together to complete a full scene.Recently, Open3D [17] introduces various algorithms toreconstruct the 3D environment with RGB-D information.However, these systems still face problems such as requiringmanual alignment of the unregistered camera frames fromseparated fragments, and poor initialization of the cameraposes could lead to incorrect convergence of the cameraposes. These works produce accurate models, but at a cost ofhours or even days of processing time. Different from thesemethods that use small fragment registration, our systemfocuses on providing accurate camera poses in dense recon-struction. It does not involve the computationally intensiveregistration.

Another category of the dense reconstruction is SLAMwith RGB-D sensor. As a subject of great interest, systemssuch as KinectFusion [3], Kintinuous [4], InifiniTAM [18],[19] focus on the surface reconstruction of the model us-ing Truncated Signed Distance Function (TSDF) [20] thatstores the signed distance from every voxel to its nearestsurface. Although these systems are able to achieve highperformance in a room-size reconstruction, they rely on thedepth information to conduct frame-to-model ICP, which isbrittle when the depth measurements are subject to significantsensor noise. As a result, they are vulnerable to accumulateddrift. ElasticFusion [5] incorporated the color informationin the frame-to-model tracking to minimize the effect ofICP drift. However, the ICP drift still exists when the sceneis large. Another approach applied by the state-of-the-artBundleFusion [7] is the SIFT feature correspondence searchand BA that is similar to the works such as ORB-SLAM[21]. Although BundleFusion achieves superior performancein indoor reconstruction, it is computationally demanding,

requiring two high-end GPUs to run in real-time. Flash-Fusion [22] applies a similar approach to use ORB [23]feature correspondence and keyframe integration withoutGPU. It achieves a similar performance to BundleFusion withmuch lower computation needs, but its keyframe integrationstrategy leads to a lower surface area coverage comparing toother dense mapping systems; it substantially sub-samplesthe frames and voxels. A common characteristic in all of thedense mapping methods in SLAM is that they often fail at alarger scene due to incorrect odometry estimation or incorrectloop closures. These failures often lead to misalignmentsin the final reconstruction. However, they provide highlyaccurate estimation of the change of poses between frames.We take advantage of the accurate short-term odometryestimation from the SLAM methods to achieve consistentlocal geometry.

III. GLOBALLY CONSISTENT POSE ESTIMATION USINGFACTOR GRAPH

Given a set of N RGB-D images captured by a commericalRGB-D camera, keyframe camera pose priors are obtainedthrough an SfM pipeline. Later, the camera poses are jointlyoptimized by the prior and odometry from the dense tracking.The optimized camera poses are used in the final reconstruc-tion. Fig. 3 shows an illustration of the system architecture.

A. Prior and Odometry Estimation

Keyframe Pose Estimation: The set of color images C isevenly down-sampled with an interval of λ. The resultingcolor maps are regarded as keyframes K ⊂ C. Using the state-of-the-art system COLMAP [12], we estimate the i-th framepose Pi ∈ SE(3) and its corresponding covariance ΣPi . Theresulting camera pose is regarded as the camera pose priorin the optimization in Sec. III-B. We refer to Schonberger et

al. [2] and Moulon et al. [1] for a detail description of theSfM method in systems such as COLMAP and OpenMVG.


Pi

Pj

Ptk<latexit sha1_base64="eBcKxpqhypOv0m/nezB8GO7D6Qg=">AAACF3icbVDLSsNAFJ3UV42vqEs3g0VwFRIRdFl047KCfUATwmQ6acdOJmFmIpSQv3Djr7hxoYhb3fk3Ttos+vDAwLnn3Mvce8KUUakc59eora1vbG7Vt82d3b39A+vwqCOTTGDSxglLRC9EkjDKSVtRxUgvFQTFISPdcHxb+t0nIiRN+IOapMSP0ZDTiGKktBRYthcjNQqjvFUE1PPMufJxscxVMC4Cq+HYzhRwlbgVaYAKrcD68QYJzmLCFWZIyr7rpMrPkVAUM1KYXiZJivAYDUlfU45iIv18elcBz7QygFEi9OMKTtX5iRzFUk7iUHeWe8plrxT/8/qZiq79nPI0U4Tj2UdRxqBKYBkSHFBBsGITTRAWVO8K8QgJhJWO0tQhuMsnr5LOhe06tnt/2WjeVHHUwQk4BefABVegCe5AC7QBBs/gFbyDD+PFeDM+ja9Za82oZo7BAozvP+kyoGM=</latexit><latexit sha1_base64="eBcKxpqhypOv0m/nezB8GO7D6Qg=">AAACF3icbVDLSsNAFJ3UV42vqEs3g0VwFRIRdFl047KCfUATwmQ6acdOJmFmIpSQv3Djr7hxoYhb3fk3Ttos+vDAwLnn3Mvce8KUUakc59eora1vbG7Vt82d3b39A+vwqCOTTGDSxglLRC9EkjDKSVtRxUgvFQTFISPdcHxb+t0nIiRN+IOapMSP0ZDTiGKktBRYthcjNQqjvFUE1PPMufJxscxVMC4Cq+HYzhRwlbgVaYAKrcD68QYJzmLCFWZIyr7rpMrPkVAUM1KYXiZJivAYDUlfU45iIv18elcBz7QygFEi9OMKTtX5iRzFUk7iUHeWe8plrxT/8/qZiq79nPI0U4Tj2UdRxqBKYBkSHFBBsGITTRAWVO8K8QgJhJWO0tQhuMsnr5LOhe06tnt/2WjeVHHUwQk4BefABVegCe5AC7QBBs/gFbyDD+PFeDM+ja9Za82oZo7BAozvP+kyoGM=</latexit><latexit sha1_base64="eBcKxpqhypOv0m/nezB8GO7D6Qg=">AAACF3icbVDLSsNAFJ3UV42vqEs3g0VwFRIRdFl047KCfUATwmQ6acdOJmFmIpSQv3Djr7hxoYhb3fk3Ttos+vDAwLnn3Mvce8KUUakc59eora1vbG7Vt82d3b39A+vwqCOTTGDSxglLRC9EkjDKSVtRxUgvFQTFISPdcHxb+t0nIiRN+IOapMSP0ZDTiGKktBRYthcjNQqjvFUE1PPMufJxscxVMC4Cq+HYzhRwlbgVaYAKrcD68QYJzmLCFWZIyr7rpMrPkVAUM1KYXiZJivAYDUlfU45iIv18elcBz7QygFEi9OMKTtX5iRzFUk7iUHeWe8plrxT/8/qZiq79nPI0U4Tj2UdRxqBKYBkSHFBBsGITTRAWVO8K8QgJhJWO0tQhuMsnr5LOhe06tnt/2WjeVHHUwQk4BefABVegCe5AC7QBBs/gFbyDD+PFeDM+ja9Za82oZo7BAozvP+kyoGM=</latexit><latexit sha1_base64="eBcKxpqhypOv0m/nezB8GO7D6Qg=">AAACF3icbVDLSsNAFJ3UV42vqEs3g0VwFRIRdFl047KCfUATwmQ6acdOJmFmIpSQv3Djr7hxoYhb3fk3Ttos+vDAwLnn3Mvce8KUUakc59eora1vbG7Vt82d3b39A+vwqCOTTGDSxglLRC9EkjDKSVtRxUgvFQTFISPdcHxb+t0nIiRN+IOapMSP0ZDTiGKktBRYthcjNQqjvFUE1PPMufJxscxVMC4Cq+HYzhRwlbgVaYAKrcD68QYJzmLCFWZIyr7rpMrPkVAUM1KYXiZJivAYDUlfU45iIv18elcBz7QygFEi9OMKTtX5iRzFUk7iUHeWe8plrxT/8/qZiq79nPI0U4Tj2UdRxqBKYBkSHFBBsGITTRAWVO8K8QgJhJWO0tQhuMsnr5LOhe06tnt/2WjeVHHUwQk4BefABVegCe5AC7QBBs/gFbyDD+PFeDM+ja9Za82oZo7BAozvP+kyoGM=</latexit>

· · ·<latexit sha1_base64="C/ThE3LO0bsU2xdfe5suNCnJ9S4=">AAAB7XicbVBNS8NAEJ3Ur1q/qh69LBbBU0lE0GPRi8cK9gPaUDabTbt2kw27E6GE/gcvHhTx6v/x5r9x2+agrQ8GHu/NMDMvSKUw6LrfTmltfWNzq7xd2dnd2z+oHh61jco04y2mpNLdgBouRcJbKFDybqo5jQPJO8H4duZ3nrg2QiUPOEm5H9NhIiLBKFqp3WehQjOo1ty6OwdZJV5BalCgOah+9UPFspgnyCQ1pue5Kfo51SiY5NNKPzM8pWxMh7xnaUJjbvx8fu2UnFklJJHSthIkc/X3RE5jYyZxYDtjiiOz7M3E/7xehtG1n4skzZAnbLEoyiRBRWavk1BozlBOLKFMC3srYSOqKUMbUMWG4C2/vEraF3XPrXv3l7XGTRFHGU7gFM7BgytowB00oQUMHuEZXuHNUc6L8+58LFpLTjFzDH/gfP4ArlePLw==</latexit><latexit sha1_base64="C/ThE3LO0bsU2xdfe5suNCnJ9S4=">AAAB7XicbVBNS8NAEJ3Ur1q/qh69LBbBU0lE0GPRi8cK9gPaUDabTbt2kw27E6GE/gcvHhTx6v/x5r9x2+agrQ8GHu/NMDMvSKUw6LrfTmltfWNzq7xd2dnd2z+oHh61jco04y2mpNLdgBouRcJbKFDybqo5jQPJO8H4duZ3nrg2QiUPOEm5H9NhIiLBKFqp3WehQjOo1ty6OwdZJV5BalCgOah+9UPFspgnyCQ1pue5Kfo51SiY5NNKPzM8pWxMh7xnaUJjbvx8fu2UnFklJJHSthIkc/X3RE5jYyZxYDtjiiOz7M3E/7xehtG1n4skzZAnbLEoyiRBRWavk1BozlBOLKFMC3srYSOqKUMbUMWG4C2/vEraF3XPrXv3l7XGTRFHGU7gFM7BgytowB00oQUMHuEZXuHNUc6L8+58LFpLTjFzDH/gfP4ArlePLw==</latexit><latexit sha1_base64="C/ThE3LO0bsU2xdfe5suNCnJ9S4=">AAAB7XicbVBNS8NAEJ3Ur1q/qh69LBbBU0lE0GPRi8cK9gPaUDabTbt2kw27E6GE/gcvHhTx6v/x5r9x2+agrQ8GHu/NMDMvSKUw6LrfTmltfWNzq7xd2dnd2z+oHh61jco04y2mpNLdgBouRcJbKFDybqo5jQPJO8H4duZ3nrg2QiUPOEm5H9NhIiLBKFqp3WehQjOo1ty6OwdZJV5BalCgOah+9UPFspgnyCQ1pue5Kfo51SiY5NNKPzM8pWxMh7xnaUJjbvx8fu2UnFklJJHSthIkc/X3RE5jYyZxYDtjiiOz7M3E/7xehtG1n4skzZAnbLEoyiRBRWavk1BozlBOLKFMC3srYSOqKUMbUMWG4C2/vEraF3XPrXv3l7XGTRFHGU7gFM7BgytowB00oQUMHuEZXuHNUc6L8+58LFpLTjFzDH/gfP4ArlePLw==</latexit><latexit sha1_base64="C/ThE3LO0bsU2xdfe5suNCnJ9S4=">AAAB7XicbVBNS8NAEJ3Ur1q/qh69LBbBU0lE0GPRi8cK9gPaUDabTbt2kw27E6GE/gcvHhTx6v/x5r9x2+agrQ8GHu/NMDMvSKUw6LrfTmltfWNzq7xd2dnd2z+oHh61jco04y2mpNLdgBouRcJbKFDybqo5jQPJO8H4duZ3nrg2QiUPOEm5H9NhIiLBKFqp3WehQjOo1ty6OwdZJV5BalCgOah+9UPFspgnyCQ1pue5Kfo51SiY5NNKPzM8pWxMh7xnaUJjbvx8fu2UnFklJJHSthIkc/X3RE5jYyZxYDtjiiOz7M3E/7xehtG1n4skzZAnbLEoyiRBRWavk1BozlBOLKFMC3srYSOqKUMbUMWG4C2/vEraF3XPrXv3l7XGTRFHGU7gFM7BgytowB00oQUMHuEZXuHNUc6L8+58LFpLTjFzDH/gfP4ArlePLw==</latexit> · · ·<latexit sha1_base64="C/ThE3LO0bsU2xdfe5suNCnJ9S4=">AAAB7XicbVBNS8NAEJ3Ur1q/qh69LBbBU0lE0GPRi8cK9gPaUDabTbt2kw27E6GE/gcvHhTx6v/x5r9x2+agrQ8GHu/NMDMvSKUw6LrfTmltfWNzq7xd2dnd2z+oHh61jco04y2mpNLdgBouRcJbKFDybqo5jQPJO8H4duZ3nrg2QiUPOEm5H9NhIiLBKFqp3WehQjOo1ty6OwdZJV5BalCgOah+9UPFspgnyCQ1pue5Kfo51SiY5NNKPzM8pWxMh7xnaUJjbvx8fu2UnFklJJHSthIkc/X3RE5jYyZxYDtjiiOz7M3E/7xehtG1n4skzZAnbLEoyiRBRWavk1BozlBOLKFMC3srYSOqKUMbUMWG4C2/vEraF3XPrXv3l7XGTRFHGU7gFM7BgytowB00oQUMHuEZXuHNUc6L8+58LFpLTjFzDH/gfP4ArlePLw==</latexit><latexit sha1_base64="C/ThE3LO0bsU2xdfe5suNCnJ9S4=">AAAB7XicbVBNS8NAEJ3Ur1q/qh69LBbBU0lE0GPRi8cK9gPaUDabTbt2kw27E6GE/gcvHhTx6v/x5r9x2+agrQ8GHu/NMDMvSKUw6LrfTmltfWNzq7xd2dnd2z+oHh61jco04y2mpNLdgBouRcJbKFDybqo5jQPJO8H4duZ3nrg2QiUPOEm5H9NhIiLBKFqp3WehQjOo1ty6OwdZJV5BalCgOah+9UPFspgnyCQ1pue5Kfo51SiY5NNKPzM8pWxMh7xnaUJjbvx8fu2UnFklJJHSthIkc/X3RE5jYyZxYDtjiiOz7M3E/7xehtG1n4skzZAnbLEoyiRBRWavk1BozlBOLKFMC3srYSOqKUMbUMWG4C2/vEraF3XPrXv3l7XGTRFHGU7gFM7BgytowB00oQUMHuEZXuHNUc6L8+58LFpLTjFzDH/gfP4ArlePLw==</latexit><latexit sha1_base64="C/ThE3LO0bsU2xdfe5suNCnJ9S4=">AAAB7XicbVBNS8NAEJ3Ur1q/qh69LBbBU0lE0GPRi8cK9gPaUDabTbt2kw27E6GE/gcvHhTx6v/x5r9x2+agrQ8GHu/NMDMvSKUw6LrfTmltfWNzq7xd2dnd2z+oHh61jco04y2mpNLdgBouRcJbKFDybqo5jQPJO8H4duZ3nrg2QiUPOEm5H9NhIiLBKFqp3WehQjOo1ty6OwdZJV5BalCgOah+9UPFspgnyCQ1pue5Kfo51SiY5NNKPzM8pWxMh7xnaUJjbvx8fu2UnFklJJHSthIkc/X3RE5jYyZxYDtjiiOz7M3E/7xehtG1n4skzZAnbLEoyiRBRWavk1BozlBOLKFMC3srYSOqKUMbUMWG4C2/vEraF3XPrXv3l7XGTRFHGU7gFM7BgytowB00oQUMHuEZXuHNUc6L8+58LFpLTjFzDH/gfP4ArlePLw==</latexit><latexit sha1_base64="C/ThE3LO0bsU2xdfe5suNCnJ9S4=">AAAB7XicbVBNS8NAEJ3Ur1q/qh69LBbBU0lE0GPRi8cK9gPaUDabTbt2kw27E6GE/gcvHhTx6v/x5r9x2+agrQ8GHu/NMDMvSKUw6LrfTmltfWNzq7xd2dnd2z+oHh61jco04y2mpNLdgBouRcJbKFDybqo5jQPJO8H4duZ3nrg2QiUPOEm5H9NhIiLBKFqp3WehQjOo1ty6OwdZJV5BalCgOah+9UPFspgnyCQ1pue5Kfo51SiY5NNKPzM8pWxMh7xnaUJjbvx8fu2UnFklJJHSthIkc/X3RE5jYyZxYDtjiiOz7M3E/7xehtG1n4skzZAnbLEoyiRBRWavk1BozlBOLKFMC3srYSOqKUMbUMWG4C2/vEraF3XPrXv3l7XGTRFHGU7gFM7BgytowB00oQUMHuEZXuHNUc6L8+58LFpLTjFzDH/gfP4ArlePLw==</latexit>

Pi+1

Pi+�

Pi+��1<latexit sha1_base64="SeQY9dk4/d10G1HVWodF+17+Tb4=">AAACLXicdVDLSsNAFL3xWesr6tJNtAgFsSRudFnUhcsK9gFNCJPJpB06eTAzEUrID7nxF/wEEVxUxK1f4N5J24Vt9cDA4Zx779x7vIRRIU1zpC0tr6yurZc2yptb2zu7+t5+S8Qpx6SJYxbzjocEYTQiTUklI52EExR6jLS9wXXhtx8IFzSO7uUwIU6IehENKEZSSa5+Y4dI9r0ga+RuRk+t3LbLs5LN1DQf/WucWbmrV8yaOYaxSKwpqdSPqt/PANBw9Vfbj3EakkhihoToWmYinQxxSTEjedlOBUkQHqAe6SoaoZAIJxtfmxsnSvGNIObqRdIYq787MhQKMQw9VVnsK+a9QvzL66YyuHQyGiWpJBGefBSkzJCxUURn+JQTLNlQEYQ5VbsauI84wlIFXFYhWPMnL5LWec0ya9adSuMKJijBIRxDFSy4gDrcQgOagOERXmAE79qT9qZ9aJ+T0iVt2nMAM9C+fgCOU6sd</latexit><latexit sha1_base64="9bovXoWfBlL5j9dx++26TCpob/Q=">AAACLXicdVDLSsNAFJ3UV42vqEs30SJUxJK40WVRFy4r2Ac0IUwmk3ZwMgkzE6GEfIP/4cZ/8AtEcFERt36BeydtF7bVAwOHc+69c+/xE0qEtKyhVlpYXFpeKa/qa+sbm1vG9k5LxClHuIliGvOODwWmhOGmJJLiTsIxjHyK2/7dZeG37zEXJGa3cpBgN4I9RkKCoFSSZ1w5EZR9P8wauZeRYzt3HH1acqiaFsB/jRM794yKVbNGMOeJPSGV+n71+/nBOWp4xqsTxCiNMJOIQiG6tpVIN4NcEkRxrjupwAlEd7CHu4oyGGHhZqNrc/NQKYEZxlw9Js2R+rsjg5EQg8hXlcW+YtYrxL+8birDczcjLEklZmj8UZhSU8ZmEZ0ZEI6RpANFIOJE7WqiPuQQSRWwrkKwZ0+eJ63Tmm3V7BuVxgUYowz2wAGoAhucgTq4Bg3QBAg8ghcwBO/ak/amfWif49KSNunZBVPQvn4AAj+sMw==</latexit><latexit sha1_base64="9bovXoWfBlL5j9dx++26TCpob/Q=">AAACLXicdVDLSsNAFJ3UV42vqEs30SJUxJK40WVRFy4r2Ac0IUwmk3ZwMgkzE6GEfIP/4cZ/8AtEcFERt36BeydtF7bVAwOHc+69c+/xE0qEtKyhVlpYXFpeKa/qa+sbm1vG9k5LxClHuIliGvOODwWmhOGmJJLiTsIxjHyK2/7dZeG37zEXJGa3cpBgN4I9RkKCoFSSZ1w5EZR9P8wauZeRYzt3HH1acqiaFsB/jRM794yKVbNGMOeJPSGV+n71+/nBOWp4xqsTxCiNMJOIQiG6tpVIN4NcEkRxrjupwAlEd7CHu4oyGGHhZqNrc/NQKYEZxlw9Js2R+rsjg5EQg8hXlcW+YtYrxL+8birDczcjLEklZmj8UZhSU8ZmEZ0ZEI6RpANFIOJE7WqiPuQQSRWwrkKwZ0+eJ63Tmm3V7BuVxgUYowz2wAGoAhucgTq4Bg3QBAg8ghcwBO/ak/amfWif49KSNunZBVPQvn4AAj+sMw==</latexit><latexit sha1_base64="qE11qeSNWS+pINsyjZqmG8Xkriw=">AAACLXicdVDLSsNAFJ34rPEVdelmsAiCWDJudFnUhcsK9gFNCJPJpB06eTAzEUrID7nxV0RwURG3/oaTNgvb6oGBwzn33rn3+ClnUtn2xFhZXVvf2Kxtmds7u3v71sFhRyaZILRNEp6Ino8l5SymbcUUp71UUBz5nHb90W3pd5+okCyJH9U4pW6EBzELGcFKS55150RYDf0wbxVezs5R4TjmvORwPS3A/xoXqPCsut2wp4DLBFWkDiq0POvNCRKSRTRWhGMp+8hOlZtjoRjhtDCdTNIUkxEe0L6mMY6odPPptQU81UoAw0ToFys4VX935DiSchz5urLcVy56pfiX189UeO3mLE4zRWMy+yjMOFQJLKODAROUKD7WBBPB9K6QDLHAROmATR0CWjx5mXQuG8huoAe73ryp4qiBY3ACzgACV6AJ7kELtAEBz+AVTMCH8WK8G5/G16x0xah6jsAcjO8fiC6o3w==</latexit>

Pi+1

Pi+�


Pi+1

Pi+�


Fig. 4. (Left) Illustration of the pose interpolation on manifold. (Right)3D similarity transformation.

Frame Odometry Estimation: The pose change betweenthe i-th and j-th consecutive frames ξij ∈ R6 is estimatedusing a joint frame-to-model [24] optimization. The costfunction over the relative pose joins the point-to-plane dis-tance and the photometric error, and is minimized in imagepyramids for faster convergence. For details, we refer to theoriginal ElasticFusion by Whelan et al. [5].

B. Factor Graph Representation

At the core of our proposed system is the joint optimiza-tion of the keyframe pose prior and frame-to-model odome-try. To bring these together, we propose to use the factorgraph formulation shown in Fig. 2. Specifically, given akeyframe Ki where i ∈ {1, 2, · · · bNλ c} and its correspondingpose Pi estimated from SfM, we treat the keyframe pose asthe pose prior in the factor graph. In addition, we representthe frame-to-model odometry ξij between frame i and j asan odometry factor. Since the keyframe prior and the frame-to-model odometry are independently estimated, these twokinds of factors are not aligned in the same coordinate frame.In addition, due to the scale ambiguity issue in SfM, weneed to simultaneously estimate the scale that best alignsthe prior and the odometry. The connections of the priorfactor and the odometry factor are shown in Fig. 2. Giventhis formulation, the optimal solution of the problem is themaximum a posteriori estimation of the factor graph:

X ∗ = arg minX

NK∑i=1

‖rs‖2σ2s

+

N∑i=1

‖rPi‖2ΣPi+

N∑i,j=1

∥∥rξij∥∥2

Σξij,

(1)where the set of optimal state is the set of all of the optimalposes and the optimal scale:

X ∗ = {P∗, s∗}. (2)

rs, rPi , and rξij correspond to the residuals of scale,pose prior, and odometry respectively. σ2

s , ΣPi , and Σξijcorrespond to the covariance of the scale, the pose prior, andthe odometry. This problem can be solved using Levenberg-Marquardt non-linear optimization to iterate at the lineariza-tion point X k. At the k-th iteration, the linearization can beexpressed as the following Taylor expansion:

rsi(X k + δX k+1) ≈ rsi(X k) +HksiδX k+1, (3)

rξj (X k + δX k+1) ≈ rξj (X k) +HkξjδX k+1, (4)

where we use the first order term to approximate the actualvalue. The measurement Jacobian H at the k-th iteration andthe corresponding state update can be expressed as:

Hksi =

δrsiδX

∣∣∣∣X=Xk

, Hkξij =

δrξijδX

∣∣∣∣X=Xk

, (5)

X k+1 = X k ⊕ δX k+1. (6)

We use the ⊕ operator to represent retraction.

C. Pose Initialization: Interpolation on SE(3) Manifold

Initialization is of paramount importance to solve the non-convex optimization problem. It is important to initialize thestate within close proximity of the optimal value to allowfinal convergence. If the sub-sampled interval λ is large,using the keyframe pose prior to initialize the intermediateposes is likely inaccurate, and thus might lead to wrongconvergence. We propose to use an on-manifold interpolationbased on the method shown in Zefran and Kumar [25] toinitialize the poses in between two keyframes.

Let two keyframe poses be Pi and Pi+λ correspondingto the i-th and the (i + λ)-th frames, we want to find theintermediate pose Pk corresponding to the k-th frame thatsatisfies i ≤ k ≤ i+λ. This can be achieved by first findingthe difference δP between two poses, and then map thedifference from SE(3) to se(3) represented by δξ. Given thetangent space of the Lie manifold preserves linearity, we canestimate the intermediate poses corresponds to k as:

Pk = Pi exp

(k − iλ

δξ

). (7)

D. Scale Initialization

Due to the scale ambiguity in SfM, we need to find thescale factor s ∈ R that best aligns the two sets of poses.Similar to the pose initialization in Sec. III-C, we also need tomake sure that the scale is correctly initialized. We propose touse the 3D landmark Xsfm captured by the first frame in theSfM pipeline. Assuming that the first camera frame shouldbe aligned with the global coordinate, we have the posefor the first frame expressed in the global coordinate frameto be P = I4×4. The set of first frame landmarks Xsfm

corresponds to the set of 2D pixel coordinates x1. We canextract the corresponding 3D landmarks Xslam from the firstframe depth image using the following camera projectionfunction:

Xslam = π−1(P1 = I4×4,x1), (8)

where π−1(·, ·) is the inverse camera projection that mapsthe pixel coordinate to the world coordinate. Then, basedon the method by Horn [26], we estimate a similaritytransformation between two sets of 3D points using singularvalue decomposition:

U,S,V = svd(Xsfm ⊗ Xslam), (9)

R = UV>, (10)

Xaligned = RXsfm. (11)

COLMAP-MVS ElasticFusion Redwood Proposed

Fig. 5. Heatmaps showing reconstruction error on A-ICL-lr1. The top row and the bottom row are the internal view and the top view of the room scene,respectively. Points more than 0.2m away from the ground truth are removed.

We express the initial scale as:

sI =

∑Xi∈Xslam,Xj∈Xaligned

Xi ·Xj∑Xj∈Xsfm

‖Xj‖2, (12)

where the Xsfm and Xslam are the zero-centered 3D pointsstacked in a column matrix, and ⊗ denotes the outer productbetween two matrices. Since the depth image is subject tonoise, the initial scale sI is not entirely accurate. We refinethe scale in the optimization as shown in Fig. 2.

E. Optimization and Surfel-based Model Reconstruction

Given the keyframe pose prior and the odometry areinitialized and aligned, we optimize the camera poses usingthe incoming odometry. When a new odometry measurementis obtained using the method in Sec. III-A, it is inserted intothe factor graph and incrementally optimized using iSAM2[27]. Similar to ElasticFusion, the model is generated bysurface splatting. Different from the surface deformationapproach in ElasticFusion, our method relies on the glob-ally consistent pose priors to constrain the input odometry.Therefore, we do not apply the surface loop closure and themodel deformation.

IV. EXPERIMENTAL RESULTS

A. Implementation

We implement the system based on two state-of-the-artsystems, COLMAP and ElasticFusion. COLMAP is usedas the SfM pipeline that generates the pose prior, and thetracking component in ElasticFusion is used to generateodometry. In addition, we adopt the surfel representation andthe surface fusion strategy in ElasticFusion. We implementthe proposed factor graph, Levenberg-Marquardt optimizer,

and iSAM2 [27] optimizer using the GTSAM [28] library.Experiments are conducted on a Ubuntu 16.04 desktop withan Intel i7-6700 CPU and a GeForce GTX 1070 GPU.

B. Synthetic Dataset

We test our system on the Augmented ICL-NUIM (A-ICL)dataset with synthetic noise created by Choi et al. [6]. The A-ICL sequence is based on the original ICL-NUIM dataset byHanda et al. [29]. Both ICL and A-ICL sequences are createdin the same synthetic living room scene, but A-ICL containsa longer camera trajectory (max of 2870 frames vs. max of1510 frames) and a larger coverage of the scene. We comparethe surface reconstruction accuracy against the ground truth(GT) model using CloudCompare [30], and we also comparethe Root Mean Square of Absolute Trajectory Error (ATE-RMSE) using the tools from TUM-RGBD benchmark [8].To demonstrate the performance of our system, we com-pare against the systems from three different categories: (1)offline SfM-MVS systems represented by COLMAP-MVS[12], (2) offline RGB-D reconstruction systems representedby Redwood [6], and (3) online dense mapping systemsrepresented by ElasticFusion [5] and InfiniTAMv3 [19].The surface reconstruction accuracy, system runtime, andabsolute trajectory error are used as metrics to evaluate thesystem performance. Table I shows the results of the A-ICLliving room dataset with noise. A heatmap visualization ofthe surface reconstruction accuracy is shown in Fig. 5.

Table I shows the mean and the standard deviation ofthe distance to the GT model. Although our surface recon-struction accuracy is slightly lower than the models fromCOLMAP-MVS and the Redwood, the model produced byour method has the lowest standard deviation and trajec-tory error. A collection of heatmaps illustrating the surface

TABLE IRESULTS ON THE SYNTHETIC A-ICL DATASET

Mean distance to GT model (cm) Std. Dev. distance to GT model (cm) System Runtime (min) ATE RMSE (cm)A-ICL-lr1 A-ICL-lr2 A-ICL-lr1 A-ICL-lr2 A-ICL-lr1 A-ICL-lr2 A-ICL-lr1 A-ICL-lr2

COLMAP-MVS 1.59 1.35 4.58 4.20 212.66 159.41 5.68 18.78Redwood 3.00 2.02 2.98 1.83 ∼300 ∼300 N/A N/AElasticFusion 7.71 7.78 6.91 6.34 (online) (online) 66.61 28.53InfiniTAMv3 7.33 10.25 6.39 6.87 (online) (online) 46.07 73.64Proposed 1.74 2.26 1.34 1.78 29.37 6.93 5.14 3.79

Colored model Distance heatmap

Fig. 6. Results on the real-world CoRBS dataset. Top row, Humansequence. Bottom row, Racing Car sequence. The output distance heatmapshows our method reconstructs accurate models.

reconstruction accuracy of A-ICL-lr1 is shown in Fig. 5.Furthermore, our method not only achieves a higher accuracythan the online methods, but also finishes the reconstructionin a much shorter period comparing to other offline methods.

C. Real World Dataset

We conduct additional tests on the CoRBS [31] realworld dataset with surface ground truth model collected bya 3D scanner with sub-millimeter accuracy. This dataset iscollected using a Kinect V2 hand-held RGB-D camera, andit contains a real racing car and a human-size model. Thesurface reconstruction accuracy of each object is shown inTable II, and the reconstructed models and the correspondingheatmaps are shown in Fig. IV-C.

TABLE IIRESULTS ON THE REAL-WORLD CORBS DATASET

Mean distance (cm) Std. dev. distance (cm)Racing car Human Racing car Human

ElasticFusion 6.07 52.98 4.45 49.0Proposed 1.30 1.12 1.15 1.84

V. CONCLUSION

We present a dense reconstruction system that combinesthe advantages from both SfM and SLAM to recover bothlocally and globally consistent 3D models using an RGB-D sensor. We achieve performance by extracting keyframesand obtaining their corresponding pose priors from SfM,and locally adjusting the camera using odometry from denseSLAM. Our method shows better performance than the state-of-the-art dense SLAM system for both synthetic and real-world datasets, while providing much shorter processingtime and comparable quality than other offline dense 3Dreconstruction systems. In addition to the discussion aboutSfM and SLAM, the idea of combining strong camera priorsand odometry can also be applied in situations such as GPSinput as prior, or IMU input as odometry.

For future work, we would like to develop a solutionto various parameter choices. For example, the number ofkeyframes should be automatically adjusted when the recon-struction data becomes larger, and more keyframes shouldbe extracted when the camera motion is large. In addition,we are working towards an online system that is capable ofachieving similar reconstruction accuracy.

ACKNOWLEDGMENT

We would like to thank Thomas Whelan for his help withElasticFusion. We would also like to thank Ming Hsiao, EricWestman, and Jerry Hsiung for valuable discussions.

REFERENCES

[1] P. Moulon, P. Monasse, and R. Marlet, “Adaptive structure from mo-tion with a contrario model estimation,” in Asian Conf. on ComputerVision (ACCV), Nov. 2012, pp. 257–270.

[2] J. L. Schonberger, E. Zheng, J. M. Frahm, and M. Pollefeys, “Pixel-wise view selection for unstructured multi-view stereo,” in Eur. Conf.on Computer Vision (ECCV), Oct. 2016, pp. 501–518.

[3] R. A. Newcombe, S. Izadi, O. Hilliges, D. Molyneaux, D. Kim,A. J. Davison, P. Kohli, J. Shotton, S. Hodges, and A. Fitzgibbon,“KinectFusion: Real-time dense surface mapping and tracking,” inIEEE and ACM Intl. Sym. on Mixed and Augmented Reality (ISMAR),Oct. 2011, pp. 127–136.

[4] T. Whelan, M. Kaess, M. Fallon, H. Johannsson, J. Leonard, andJ. McDonald, “Kintinuous: Spatially extended KinectFusion,” RSSWorkshop on RGB-D: Advanced Reasoning with Depth Cameras, Jul.2012.

[5] T. Whelan, S. Leutenegger, R. F. Salas-Moreno, B. Glocker, and A. J.Davison, “ElasticFusion: Dense SLAM without a pose graph,” inRobotics: Science and Systems (RSS), Rome, Italy, Jul. 2015.

[6] S. Choi, Q. Y. Zhou, and V. Koltun, “Robust reconstruction ofindoor scenes,” in Proc. IEEE Int. Conf. Computer Vision and PatternRecognition, Jun. 2015, pp. 5556–5565.

[7] A. Dai, M. Nießner, M. Zollhofer, S. Izadi, and C. Theobalt,“BundleFusion: Real-time globally consistent 3D reconstructionusing on-the-fly surface re-integration,” ACM Transactions onGraphics, vol. 36, no. 4, May 2017. [Online]. Available:http://doi.acm.org/10.1145/3072959.3054739

[8] J. Sturm, N. Engelhard, F. Endres, W. Burgard, and D. Cremers, “Abenchmark for the evaluation of RGB-D SLAM systems,” in IEEE/RSJIntl. Conf. on Intelligent Robots and Systems (IROS), Oct. 2012, pp.573–580.

[9] F. Dellaert and M. Kaess, “Factor graphs for robot perception,”Foundations and Trends in Robotics, vol. 6, no. 1-2, pp. 1–139, 2017.[Online]. Available: http://dx.doi.org/10.1561/2300000043

[10] P. Moulon and P. Monasse, “Unordered feature tracking made fastand easy,” ACM SIGGRAPH European Conference on Visual MediaProduction, p. 1, Dec. 2012.

[11] C. Sweeney, T. Hollerer, and M. Turk, “Theia: A fast and scalablestructure-from-motion library,” in Proceedings of the 23rd ACM Intl.Conf. on Multimedia, Oct. 2015, pp. 693–696.

[12] J. L. Schonberger and J.-M. Frahm, “Structure-from-motion revisited,”in Proc. IEEE Int. Conf. Computer Vision and Pattern Recognition,Jun. 2016, pp. 4104–4113.

[13] S. Zhu, T. Fang, J. Xiao, and L. Quan, “Local readjustment for high-resolution 3D reconstruction,” in Proc. IEEE Int. Conf. ComputerVision and Pattern Recognition, Jun. 2014, pp. 3938–3945.

[14] Y. Yao, S. Li, S. Zhu, H. Deng, T. Fang, and L. Quan, “Relativecamera refinement for accurate dense reconstruction,” in Intl. Conf.on 3D Vision, Oct. 2018, pp. 185–194.

[15] Q. Y. Zhou and V. Koltun, “Dense scene reconstruction with points ofinterest,” ACM Transactions on Graphics, pp. 112:1–112:8, Jul. 2013.

[16] Q. Y. Zhou, S. Miller, and V. Koltun, “Elastic fragments for densescene reconstruction,” in Intl. Conf. on Computer Vision (ICCV), Dec.2013, pp. 473–480.

[17] Q. Y. Zhou, J. Park, and V. Koltun, “Open3D: Amodern library for 3d data processing,” Computing ResearchRepository, vol. abs/1801.09847, 2018. [Online]. Available:http://arxiv.org/abs/1801.09847

[18] O. Kahler, V. A. Prisacariu, C. Y. Ren, X. Sun, P. H. S. Torr, andD. W. Murray, “Very high frame rate volumetric integration of depthimages on mobile device,” IEEE Transactions on Visualization andComputer Graphics, vol. 21, no. 11, pp. 1241–1250, Nov. 2015.

[19] V. A. Prisacariu, O. Kahler, S. Golodetz, M. Sapienza, T. Cavallari,P. H. Torr, and D. W. Murray, “InfiniTAM v3: A framework for large-scale 3D reconstruction with loop closure,” ArXiv e-prints, Aug. 2017.

[20] B. Curless and M. Levoy, “A volumetric method for building complexmodels from range images,” in SIGGRAPH, 1996, pp. 303–312.[Online]. Available: http://doi.acm.org/10.1145/237170.237269

[21] R. Mur-Artal, J. M. M. Montiel, and J. D. Tardos, “ORB-SLAM:A versatile and accurate monocular SLAM system,” IEEE Trans.Robotics, vol. 31, no. 5, pp. 1147–1163, Oct. 2015.

[22] H. L and L. F, “FlashFusion: Real-time globally consistent dense3D reconstruction using CPU computing,” in Robotics: Science andSystems (RSS), Jun. 2018.

[23] E. Rublee, V. Rabaud, K. Konolige, and G. Bradski, “ORB: Anefficient alternative to SIFT or SURF,” in Intl. Conf. on ComputerVision (ICCV), Nov. 2011, pp. 2564–2571.

[24] T. Whelan, H. Johannsson, M. Kaess, J. J. Leonard, and J. McDonald,“Robust real-time visual odometry for dense RGB-D mapping,” inIEEE Intl. Conf. on Robotics and Automation (ICRA), May 2013, pp.5724–5731.

[25] M. Zefran and V. Kumar, “Interpolation schemes for rigid bodymotions,” Computer-Aided Design, vol. 30, no. 3, pp. 179–189, Mar.1998.

[26] B. K. P. Horn, “Closed-form solution of absolute orientation usingunit quaternions,” Journal of the Optical Society America A, vol. 4,pp. 629–642, 1987.

[27] M. Kaess, H. Johannsson, R. Roberts, V. Ila, J. J. Leonard, andF. Dellaert, “iSAM2: Incremental smoothing and mapping using theBayes tree,” Intl. J. of Robotics Research, vol. 31, no. 2, pp. 216–235,2012.

[28] F. Dellaert, “Factor graphs and GTSAM: A hands-on introduction,”Georgia Tech Technical Report, 2012.

[29] A. Handa, T. Whelan, J. McDonald, and A. Davison, “A benchmarkfor RGB-D visual odometry, 3D reconstruction and SLAM,” in IEEEIntl. Conf. on Robotics and Automation (ICRA), May 2014, pp. 1524–1531.

[30] “Cloudcompare user manual,” https://www.danielgm.net/cc/doc/qCC/CloudCompare%20v2.6.1%20-%20User%20manual.pdf.

[31] O. Wasenmuller, M. Meyer, and D. Stricker, “CoRBS: ComprehensiveRGB-D benchmark for SLAM using Kinect v2,” in Winter Conf. onApplication of Computer Vision (WACV), Mar. 2016, pp. 1–7.

Documents

Surfel-Based Dense RGB-D Reconstruction with Global and Local …kaess/pub/Yang19icra.pdf · 2019-03-09 · Surfel-Based Dense RGB-D Reconstruction with Global and Local Consistency