diff --git a/README.md b/README.md
index 887a907..8e4937d 100644
--- a/README.md
+++ b/README.md
@@ -2,6 +2,8 @@
 
 ROS package for running [orb_slam_2_ros](https://github.com/appliedAI-Initiative/orb_slam_2_ros) with a [Realsense D435 RGBD camera](https://www.intelrealsense.com/depth-camera-d435/s).
 
+Package also includes launch file for optionally running [voxblox](https://github.com/ethz-asl/voxblox) mapping. 
+
 Launch files are provided for both stereo and rgbd slam modes. (`slam_stereo.launch` and `slam_rgbd.launch` respectively). 
 Stereo mode is recommended as it seems to perform much better than rgbd mode - feature depth estimates in rgbd mode appear very noisy (at least with the current configuration).
 
diff --git a/cfg/slam.rviz b/cfg/slam.rviz
index 17a800a..ec409eb 100644
--- a/cfg/slam.rviz
+++ b/cfg/slam.rviz
@@ -7,10 +7,9 @@ Panels:
         - /Global Options1
         - /Status1
         - /TF1/Frames1
-        - //orb_slam2/map_points1
         - //orb_slam2/map_points1/Autocompute Value Bounds1
       Splitter Ratio: 0.636029422
-    Tree Height: 251
+    Tree Height: 479
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
@@ -39,7 +38,7 @@ Visualization Manager:
       Cell Size: 1
       Class: rviz/Grid
       Color: 160; 160; 164
-      Enabled: true
+      Enabled: false
       Line Style:
         Line Width: 0.0299999993
         Value: Lines
@@ -52,7 +51,7 @@ Visualization Manager:
       Plane: XY
       Plane Cell Count: 100
       Reference Frame: <Fixed Frame>
-      Value: true
+      Value: false
     - Alpha: 1
       Class: rviz/RobotModel
       Collision Enabled: false
@@ -71,6 +70,38 @@ Visualization Manager:
           Alpha: 1
           Show Axes: false
           Show Trail: false
+        camera_color_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_color_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_depth_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_depth_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_infra1_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_infra1_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_infra2_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_infra2_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
         camera_link:
           Alpha: 1
           Show Axes: false
@@ -87,9 +118,9 @@ Visualization Manager:
       Frame Timeout: 15
       Frames:
         All Enabled: false
-        camera_accel_frame:
+        base_link:
           Value: true
-        camera_accel_optical_frame:
+        camera_bottom_screw_frame:
           Value: true
         camera_color_frame:
           Value: true
@@ -99,10 +130,6 @@ Visualization Manager:
           Value: true
         camera_depth_optical_frame:
           Value: true
-        camera_gyro_frame:
-          Value: true
-        camera_gyro_optical_frame:
-          Value: true
         camera_infra1_frame:
           Value: true
         camera_infra1_optical_frame:
@@ -113,13 +140,28 @@ Visualization Manager:
           Value: true
         camera_link:
           Value: true
+        map:
+          Value: true
       Marker Scale: 0.100000001
       Name: TF
       Show Arrows: true
       Show Axes: true
       Show Names: true
       Tree:
-        {}
+        map:
+          camera_link:
+            camera_color_frame:
+              camera_color_optical_frame:
+                {}
+            camera_depth_frame:
+              camera_depth_optical_frame:
+                {}
+            camera_infra1_frame:
+              camera_infra1_optical_frame:
+                {}
+            camera_infra2_frame:
+              camera_infra2_optical_frame:
+                {}
       Update Interval: 0
       Value: true
     - Alpha: 1
@@ -169,16 +211,16 @@ Visualization Manager:
       Axes Radius: 0.100000001
       Class: rviz/Pose
       Color: 255; 25; 0
-      Enabled: true
-      Head Length: 0.300000012
-      Head Radius: 0.100000001
+      Enabled: false
+      Head Length: 0.200000003
+      Head Radius: 0.200000003
       Name: /orb_slam2/pose
       Shaft Length: 1
-      Shaft Radius: 0.0500000007
+      Shaft Radius: 0.100000001
       Shape: Arrow
       Topic: /orb_slam2/pose
       Unreliable: false
-      Value: true
+      Value: false
     - Alpha: 1
       Autocompute Intensity Bounds: true
       Autocompute Value Bounds:
@@ -202,7 +244,7 @@ Visualization Manager:
       Queue Size: 10
       Selectable: true
       Size (Pixels): 3
-      Size (m): 0.0199999996
+      Size (m): 0.00999999978
       Style: Spheres
       Topic: /orb_slam2/map_points
       Unreliable: false
@@ -210,13 +252,19 @@ Visualization Manager:
       Use rainbow: true
       Value: true
     - Class: jsk_rviz_plugin/TFTrajectory
-      Enabled: true
+      Enabled: false
       Name: TFTrajectory
-      Value: true
+      Value: false
       color: 25; 255; 240
       duration: 60
       frame: camera_link
       line_width: 0.100000001
+    - Class: voxblox_rviz_plugin/VoxbloxMesh
+      Enabled: false
+      Name: VoxbloxMesh
+      Topic: /voxblox_node/mesh
+      Unreliable: false
+      Value: false
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
@@ -241,26 +289,26 @@ Visualization Manager:
   Value: true
   Views:
     Current:
-      Class: rviz/ThirdPersonFollower
-      Distance: 7.94418192
+      Class: rviz/XYOrbit
+      Distance: 2.24058247
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.0599999987
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: 3.53536606
-        Y: -5.72843647
-        Z: 0
-      Focal Shape Fixed Size: false
+        X: 0.894681334
+        Y: -0.621096551
+        Z: -1.43051147e-06
+      Focal Shape Fixed Size: true
       Focal Shape Size: 0.0500000007
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.00999999978
-      Pitch: 0.330397516
-      Target Frame: camera_link
-      Value: ThirdPersonFollower (rviz)
-      Yaw: 4.41541862
+      Pitch: 1.31979549
+      Target Frame: <Fixed Frame>
+      Value: XYOrbit (rviz)
+      Yaw: 3.95860338
     Saved: ~
 Window Geometry:
   /orb_slam2/debug_image:
@@ -270,7 +318,7 @@ Window Geometry:
   Height: 1016
   Hide Left Dock: false
   Hide Right Dock: false
-  QMainWindow State: 000000ff00000000fd00000004000000000000023200000360fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003a00000183000000c600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000002c002f006f00720062005f0073006c0061006d0032002f00640065006200750067005f0069006d00610067006501000001c3000001d70000001600ffffff000000010000010f00000360fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003a000003600000009e00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d00650100000000000007800000024400fffffffb0000000800540069006d00650100000000000004500000000000000000000004330000036000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd00000004000000000000015600000360fc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003a00000267000000c600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000002c002f006f00720062005f0073006c0061006d0032002f00640065006200750067005f0069006d00610067006501000002a7000000f30000001600fffffffb0000000a0049006d0061006700650000000264000001360000000000000000fb0000000c00430061006d0065007200610100000384000000160000000000000000000000010000010f00000360fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003a000003600000009e00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d00650100000000000007800000024400fffffffb0000000800540069006d006501000000000000045000000000000000000000050f0000036000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
diff --git a/dependencies.rosinstall b/dependencies.rosinstall
index c518235..990bb36 100644
--- a/dependencies.rosinstall
+++ b/dependencies.rosinstall
@@ -1,6 +1,7 @@
-- git:
-    local-name: orb_slam_2_ros
-    uri: https://github.com/appliedAI-Initiative/orb_slam_2_ros.git
+# camera driver and orb slam 2:
 - git:
     local-name: realsense-ros
     uri: https://github.com/IntelRealSense/realsense-ros.git
+- git:
+    local-name: orb_slam_2_ros
+    uri: https://github.com/appliedAI-Initiative/orb_slam_2_ros.git
diff --git a/dependencies_voxblox.rosinstall b/dependencies_voxblox.rosinstall
new file mode 100644
index 0000000..2b10b8e
--- /dev/null
+++ b/dependencies_voxblox.rosinstall
@@ -0,0 +1,37 @@
+# all following packages are for voxblox
+- git:
+    local-name: voxblox
+    uri: https://github.com/ethz-asl/voxblox.git
+- git:
+    local-name: catkin_simple
+    uri: https://github.com/catkin/catkin_simple.git
+- git:
+    local-name: eigen_catkin
+    uri: https://github.com/ethz-asl/eigen_catkin.git
+- git:
+    local-name: eigen_checks
+    uri: https://github.com/ethz-asl/eigen_checks.git
+- git:
+    local-name: gflags_catkin
+    uri: https://github.com/ethz-asl/gflags_catkin.git
+- git:
+    local-name: glog_catkin
+    uri: https://github.com/ethz-asl/glog_catkin.git
+- git:
+    local-name: minkindr
+    uri: https://github.com/ethz-asl/minkindr.git
+- git:
+    local-name: minkindr_ros
+    uri: https://github.com/ethz-asl/minkindr_ros.git
+- git:
+    local-name: protobuf_catkin
+    uri: https://github.com/ethz-asl/protobuf_catkin.git
+- git:
+    local-name: catkin_boost_python_buildtool
+    uri: https://github.com/ethz-asl/catkin_boost_python_buildtool
+- git:
+    local-name: numpy_eigen
+    uri: https://github.com/ethz-asl/numpy_eigen.git
+
+
+
diff --git a/launch/slam_rgbd.launch b/launch/slam_rgbd.launch
index 9ad51f9..1c95a6e 100644
--- a/launch/slam_rgbd.launch
+++ b/launch/slam_rgbd.launch
@@ -5,6 +5,9 @@
     <arg name="load_map" default="false" doc="if true, prior map will be loaded on startup" />
     <arg name="map_file" default="map.bin" doc="initial map to load, if 'load_map' is set true"/>
     <arg name="localize_only" default="false" doc="run orb_slam2 in localization mode"/>
+    <arg name="enable_voxblox" default="true" doc="if true, will run voxblox"/>
+
+    <param name="/camera/stereo_module/emitter_enabled" value="1" />
 
     <group if="$(eval input_log_file == '')">
         <param name="use_sim_time" value="false"/>
@@ -15,14 +18,20 @@
     </group>
     <group if="$(eval input_log_file != '')">
         <param name="use_sim_time" value="true"/>
-        <node pkg="rosbag" type="play" name="bag_play" args="$(arg input_log_file)"/>
+        <node pkg="rosbag" type="play" name="bag_play" args="$(arg input_log_file) --clock"/>
+        <!-- in playback we need to republish urdf (normally the driver does this) -->
+        <param name="robot_description" command="$(find xacro)/xacro --inorder '$(find realsense2_description)/urdf/test_d435_camera.urdf.xacro' use_nominal_extrinsics:=true" />
+        <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
     </group>
 
     <include file="$(find d435_orb_slam_2)/launch/orb_slam2_d435_rgbd.launch">
         <arg name="load_map" value="$(arg load_map)" />
         <arg name="map_file" value="$(arg map_file)" />
+        <arg name="localize_only" value="$(arg localize_only)" />
     </include>
 
+    <include if="$(arg enable_voxblox)" file="$(find d435_orb_slam_2)/launch/voxblox.launch"/>
+
     <node if="$(arg record_log)" name="logging" pkg="rosbag" type="record" args="-o /tmp/d435_rgbd_log /camera/color/image_raw /camera/aligned_depth_to_color/image_raw /camera/color/camera_info"/>
 
     <node pkg="rviz" type="rviz" name="rviz" args="-d $(find d435_orb_slam_2)/cfg/slam.rviz" respawn="true"/>
diff --git a/launch/slam_stereo.launch b/launch/slam_stereo.launch
index 4e4269f..acdc18f 100644
--- a/launch/slam_stereo.launch
+++ b/launch/slam_stereo.launch
@@ -5,6 +5,7 @@
     <arg name="load_map" default="false" doc="if true, prior map will be loaded on startup" />
     <arg name="map_file" default="map.bin" doc="initial map to load, if 'load_map' is set true"/>
     <arg name="localize_only" default="false" doc="run orb_slam2 in localization mode"/>
+    <arg name="enable_voxblox" default="true" doc="if true, will run voxblox"/>
     
     <param name="/camera/stereo_module/emitter_enabled" value="0" />
 
@@ -16,15 +17,21 @@
     </group>
     <group if="$(eval input_log_file != '')">
         <param name="use_sim_time" value="true"/>
-        <node pkg="rosbag" type="play" name="bag_play" args="$(arg input_log_file)"/>
+        <node pkg="rosbag" type="play" name="bag_play" args="$(arg input_log_file) --clock"/>
+        <!-- in playback we need to republish urdf (normally the driver does this) -->
+        <param name="robot_description" command="$(find xacro)/xacro --inorder '$(find realsense2_description)/urdf/test_d435_camera.urdf.xacro' use_nominal_extrinsics:=true" />
+        <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
     </group>
 
     <include file="$(find d435_orb_slam_2)/launch/orb_slam2_d435_stereo.launch">
         <arg name="load_map" value="$(arg load_map)" />
         <arg name="map_file" value="$(arg map_file)" />
+        <arg name="localize_only" value="$(arg localize_only)" />
     </include>
 
-    <node if="$(arg record_log)" name="logging" pkg="rosbag" type="record" args="-o /tmp/d435_stereo_log /camera/infra1/image_rect_raw /camera/infra2/image_rect_raw /camera/infra1/camera_info"/>
+    <include if="$(arg enable_voxblox)" file="$(find d435_orb_slam_2)/launch/voxblox.launch"/>
+
+    <node if="$(arg record_log)" name="logging" pkg="rosbag" type="record" args="-o /tmp/d435_stereo_log /camera/infra1/image_rect_raw /camera/infra2/image_rect_raw /camera/infra1/camera_info /camera/depth/color/points"/>
 
     <node pkg="rviz" type="rviz" name="rviz" args="-d $(find d435_orb_slam_2)/cfg/slam.rviz" respawn="true"/>
 </launch>
\ No newline at end of file
diff --git a/launch/voxblox.launch b/launch/voxblox.launch
new file mode 100644
index 0000000..2a91652
--- /dev/null
+++ b/launch/voxblox.launch
@@ -0,0 +1,20 @@
+<launch>
+  <arg name="voxel_size" default="0.04"/>
+  <node name="voxblox_node" pkg="voxblox_ros" type="tsdf_server" output="screen" args="-alsologtostderr" clear_params="true">
+    <remap from="pointcloud" to="/camera/depth/color/points"/>
+    <param name="tsdf_voxel_size" value="$(arg voxel_size)" />
+    <param name="tsdf_voxels_per_side" value="16" />
+    <param name="voxel_carving_enabled" value="true" />
+    <param name="color_mode" value="color" />
+    <param name="use_tf_transforms" value="true" />
+    <param name="world_frame" value="map"/>
+    <param name="update_mesh_every_n_sec" value="1.0" />
+    <param name="min_time_between_msgs_sec" value="0.0" />
+    <param name="method" value="fast" />
+    <param name="use_const_weight" value="false" />
+    <param name="allow_clear" value="true" />
+    <param name="verbose" value="false" />
+    <param name="mesh_filename" value="/tmp/$(anon voxblox_mesh).ply" />
+  </node>
+
+</launch>
diff --git a/setup.sh b/setup.sh
index b7ff64f..2feda4b 100755
--- a/setup.sh
+++ b/setup.sh
@@ -11,15 +11,21 @@ ROS_DISTRO="kinetic"
 SRC_DIR=${WS_DIR}/src
 mkdir -p ${SRC_DIR}
 
+#system deps
+sudo apt install python-wstool python-catkin-tools ros-kinetic-cmake-modules protobuf-compiler autoconf
+
 #create catkin workspace
 catkin init -w ${WS_DIR}
 catkin config --extend /opt/ros/${ROS_DISTRO} -w ${WS_DIR}
+catkin config --cmake-args -DCMAKE_BUILD_TYPE=Release -w ${WS_DIR}
+catkin config --merge-devel -w ${WS_DIR}
 
 #use wstool to clone source packages
 wstool init ${SRC_DIR}
-wstool set d435_orb_slam --git https://github.com/tim-fan/d435_orb_slam.git -y -t ${SRC_DIR}
+wstool set d435_orb_slam_2 --git https://github.com/tim-fan/d435_orb_slam_2.git -y -t ${SRC_DIR}
 wstool update -t ${SRC_DIR}
-wstool merge ${SRC_DIR}/d435_orb_slam/dependencies.rosinstall -t ${SRC_DIR}
+wstool merge ${SRC_DIR}/d435_orb_slam_2/dependencies.rosinstall -t ${SRC_DIR}
+wstool merge ${SRC_DIR}/d435_orb_slam_2/dependencies_voxblox.rosinstall -t ${SRC_DIR}
 wstool update -t ${SRC_DIR}
 
 #install ros deps