diff --git a/.gitignore b/.gitignore
new file mode 100644
index 0000000000000000000000000000000000000000..e92b5eb4b98a945e71bcb48fa1e9e0283f4869d3
--- /dev/null
+++ b/.gitignore
@@ -0,0 +1,6 @@
+**/build/
+**/devel/
+**/logs/
+**/.catkin_tools/
+**/.catkin_workspace
+**/~.bashrc
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/assets/10_left_link3_config_step_1.png b/RWR/src/assets/10_left_link3_config_step_1.png
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/assets/10_left_link3_config_step_1.png
rename to RWR/src/assets/10_left_link3_config_step_1.png
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/assets/11_left_link3_config_step_2.png b/RWR/src/assets/11_left_link3_config_step_2.png
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/assets/11_left_link3_config_step_2.png
rename to RWR/src/assets/11_left_link3_config_step_2.png
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/assets/12_robot_back_overview.png b/RWR/src/assets/12_robot_back_overview.png
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/assets/12_robot_back_overview.png
rename to RWR/src/assets/12_robot_back_overview.png
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/assets/13_robot_back_angle_directions.png b/RWR/src/assets/13_robot_back_angle_directions.png
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/assets/13_robot_back_angle_directions.png
rename to RWR/src/assets/13_robot_back_angle_directions.png
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/assets/14_right_link1_config_step_1.png b/RWR/src/assets/14_right_link1_config_step_1.png
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/assets/14_right_link1_config_step_1.png
rename to RWR/src/assets/14_right_link1_config_step_1.png
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/assets/15_right_link1_config_step_2.png b/RWR/src/assets/15_right_link1_config_step_2.png
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/assets/15_right_link1_config_step_2.png
rename to RWR/src/assets/15_right_link1_config_step_2.png
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/assets/16_left_link1_config_step_1.png b/RWR/src/assets/16_left_link1_config_step_1.png
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/assets/16_left_link1_config_step_1.png
rename to RWR/src/assets/16_left_link1_config_step_1.png
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/assets/17_left_link1_config_step_2.png b/RWR/src/assets/17_left_link1_config_step_2.png
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/assets/17_left_link1_config_step_2.png
rename to RWR/src/assets/17_left_link1_config_step_2.png
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/assets/1_robot_right_links.png b/RWR/src/assets/1_robot_right_links.png
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/assets/1_robot_right_links.png
rename to RWR/src/assets/1_robot_right_links.png
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/assets/2_right_straight_links.png b/RWR/src/assets/2_right_straight_links.png
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/assets/2_right_straight_links.png
rename to RWR/src/assets/2_right_straight_links.png
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/assets/3_right_link_angles_example.png b/RWR/src/assets/3_right_link_angles_example.png
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/assets/3_right_link_angles_example.png
rename to RWR/src/assets/3_right_link_angles_example.png
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/assets/4_right_link2_config_step_1.png b/RWR/src/assets/4_right_link2_config_step_1.png
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/assets/4_right_link2_config_step_1.png
rename to RWR/src/assets/4_right_link2_config_step_1.png
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/assets/4_right_link2_config_step_2.png b/RWR/src/assets/4_right_link2_config_step_2.png
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/assets/4_right_link2_config_step_2.png
rename to RWR/src/assets/4_right_link2_config_step_2.png
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/assets/5_right_link3_config_step_1.png b/RWR/src/assets/5_right_link3_config_step_1.png
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/assets/5_right_link3_config_step_1.png
rename to RWR/src/assets/5_right_link3_config_step_1.png
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/assets/6_right_link3_config_step_2.png b/RWR/src/assets/6_right_link3_config_step_2.png
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/assets/6_right_link3_config_step_2.png
rename to RWR/src/assets/6_right_link3_config_step_2.png
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/assets/7_robot_left_overview.png b/RWR/src/assets/7_robot_left_overview.png
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/assets/7_robot_left_overview.png
rename to RWR/src/assets/7_robot_left_overview.png
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/assets/8_left_link2_config_step_1.png b/RWR/src/assets/8_left_link2_config_step_1.png
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/assets/8_left_link2_config_step_1.png
rename to RWR/src/assets/8_left_link2_config_step_1.png
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/assets/9_left_link2_config_step_2.png b/RWR/src/assets/9_left_link2_config_step_2.png
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/assets/9_left_link2_config_step_2.png
rename to RWR/src/assets/9_left_link2_config_step_2.png
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/assets/custom_shoulder_assembly.jpg b/RWR/src/assets/custom_shoulder_assembly.jpg
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/assets/custom_shoulder_assembly.jpg
rename to RWR/src/assets/custom_shoulder_assembly.jpg
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/assets/kinematic_coord_system.jpg b/RWR/src/assets/kinematic_coord_system.jpg
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/assets/kinematic_coord_system.jpg
rename to RWR/src/assets/kinematic_coord_system.jpg
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/assets/lidar_mount.jpg b/RWR/src/assets/lidar_mount.jpg
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/assets/lidar_mount.jpg
rename to RWR/src/assets/lidar_mount.jpg
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/assets/robot_bottom.jpg b/RWR/src/assets/robot_bottom.jpg
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/assets/robot_bottom.jpg
rename to RWR/src/assets/robot_bottom.jpg
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/assets/robot_top.jpg b/RWR/src/assets/robot_top.jpg
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/assets/robot_top.jpg
rename to RWR/src/assets/robot_top.jpg
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/assets/robot_top_no_servos.jpg b/RWR/src/assets/robot_top_no_servos.jpg
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/assets/robot_top_no_servos.jpg
rename to RWR/src/assets/robot_top_no_servos.jpg
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/assets/rpi_platform.jpg b/RWR/src/assets/rpi_platform.jpg
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/assets/rpi_platform.jpg
rename to RWR/src/assets/rpi_platform.jpg
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/assets/rviz_animation.gif b/RWR/src/assets/rviz_animation.gif
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/assets/rviz_animation.gif
rename to RWR/src/assets/rviz_animation.gif
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/assets/servo_calibration_spreadsheet.png b/RWR/src/assets/servo_calibration_spreadsheet.png
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/assets/servo_calibration_spreadsheet.png
rename to RWR/src/assets/servo_calibration_spreadsheet.png
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/assets/servo_move_prompt.png b/RWR/src/assets/servo_move_prompt.png
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/assets/servo_move_prompt.png
rename to RWR/src/assets/servo_move_prompt.png
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/assets/spot_micro_slam.gif b/RWR/src/assets/spot_micro_slam.gif
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/assets/spot_micro_slam.gif
rename to RWR/src/assets/spot_micro_slam.gif
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/assets/spot_micro_walking.gif b/RWR/src/assets/spot_micro_walking.gif
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/assets/spot_micro_walking.gif
rename to RWR/src/assets/spot_micro_walking.gif
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/assets/state_machine.png b/RWR/src/assets/state_machine.png
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/assets/state_machine.png
rename to RWR/src/assets/state_machine.png
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/assets/tf2_coord_system.jpg b/RWR/src/assets/tf2_coord_system.jpg
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/assets/tf2_coord_system.jpg
rename to RWR/src/assets/tf2_coord_system.jpg
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/assets/walking_and_slam.gif b/RWR/src/assets/walking_and_slam.gif
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/assets/walking_and_slam.gif
rename to RWR/src/assets/walking_and_slam.gif
diff --git a/sim_ws/src/assimp_catkin b/RWR/src/assimp_catkin
similarity index 100%
rename from sim_ws/src/assimp_catkin
rename to RWR/src/assimp_catkin
diff --git a/sim_ws/src/catkin_simple b/RWR/src/catkin_simple
similarity index 100%
rename from sim_ws/src/catkin_simple
rename to RWR/src/catkin_simple
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/docs/additional_hardware_description.md b/RWR/src/docs/additional_hardware_description.md
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/docs/additional_hardware_description.md
rename to RWR/src/docs/additional_hardware_description.md
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/docs/joystick_control.md b/RWR/src/docs/joystick_control.md
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/docs/joystick_control.md
rename to RWR/src/docs/joystick_control.md
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/docs/servo_calibration.md b/RWR/src/docs/servo_calibration.md
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/docs/servo_calibration.md
rename to RWR/src/docs/servo_calibration.md
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/docs/servo_calibration_spreadsheet.ods b/RWR/src/docs/servo_calibration_spreadsheet.ods
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/docs/servo_calibration_spreadsheet.ods
rename to RWR/src/docs/servo_calibration_spreadsheet.ods
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/docs/slam_information.md b/RWR/src/docs/slam_information.md
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/docs/slam_information.md
rename to RWR/src/docs/slam_information.md
diff --git a/RWR/src/eigen_catkin/.gitignore b/RWR/src/eigen_catkin/.gitignore
new file mode 100644
index 0000000000000000000000000000000000000000..0960634b4be4be52881f143b5ec048bbdaf8473d
--- /dev/null
+++ b/RWR/src/eigen_catkin/.gitignore
@@ -0,0 +1,26 @@
+# Compiled Object files
+*.slo
+*.lo
+*.o
+*.obj
+
+# Compiled Dynamic libraries
+*.so
+*.dylib
+*.dll
+
+# Compiled Static libraries
+*.lai
+*.la
+*.a
+*.lib
+
+# Executables
+*.exe
+*.out
+*.app
+*~
+
+# Bloom.
+debian
+obj*
diff --git a/RWR/src/eigen_catkin/CMakeLists.txt b/RWR/src/eigen_catkin/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..1502a2f026fba8bccf22ef39f46507803a18226a
--- /dev/null
+++ b/RWR/src/eigen_catkin/CMakeLists.txt
@@ -0,0 +1,107 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(eigen_catkin)
+
+find_package(catkin_simple REQUIRED)
+
+catkin_simple()
+
+include(ExternalProject)
+
+set(EIGEN_CATKIN_EIGEN_INCLUDE ${CATKIN_DEVEL_PREFIX}/include/eigen3)
+set(EIGEN_CATKIN_EIGEN_INCLUDE_SIGNATURE ${EIGEN_CATKIN_EIGEN_INCLUDE}/signature_of_eigen3_matrix_library)
+
+set(EIGEN_MINIMUM_VERSION 3.3.4)
+
+# If set to OFF, will compile the given version of Eigen.
+# If set to ON, will use the system version of Eigen.
+# If set to AUTO, will determine if the system version is suitable.
+set(USE_SYSTEM_EIGEN "AUTO" CACHE INTERNAL "Whether to use the system version of Eigen.")
+include(${CMAKE_CURRENT_SOURCE_DIR}/use-system-installation-of-eigen.cmake)
+
+# Check if the version is suitable, otherwise force downloading the newer one.
+if(USE_SYSTEM_EIGEN STREQUAL "AUTO")
+  # We don't use the find_package version checks here because they fail on
+  # Ubuntu 16.04, so we'll do it manually.
+  find_package(Eigen3 QUIET)
+  if(EIGEN3_FOUND)
+    # Necessary for Ubuntu 16.04's god-awful FindEigen3.cmake.
+    if((NOT (DEFINED EIGEN3_VERSION)) AND (DEFINED EIGEN3_VERSION_STRING))
+      set(EIGEN3_VERSION ${EIGEN3_VERSION_STRING})
+    endif()
+
+    # Double check what Eigen3 found, since it doesn't seem to be sufficiently
+    # strict at least on OS X. And on Ubuntu 16.04 version is anyway undefined.
+    if((${EIGEN3_VERSION} VERSION_GREATER ${EIGEN_MINIMUM_VERSION}) OR (${EIGEN3_VERSION} VERSION_EQUAL ${EIGEN_MINIMUM_VERSION}))
+      set(USE_SYSTEM_EIGEN "ON")
+      message(STATUS "Suitable Eigen version found. Minimum version: ${EIGEN_MINIMUM_VERSION} Found version: ${EIGEN3_VERSION}")
+    else()
+      set(USE_SYSTEM_EIGEN "OFF")
+      message(STATUS "Suitable Eigen version not found. Minimum version: ${EIGEN_MINIMUM_VERSION} Found version: ${EIGEN3_VERSION}")
+    endif()
+  else()
+    set(USE_SYSTEM_EIGEN "OFF")
+    message(STATUS "No suitable Eigen version found. Minimum version: ${EIGEN_MINIMUM_VERSION}")
+  endif()
+endif()
+
+if(USE_SYSTEM_EIGEN)
+  message(STATUS "eigen_catkin is configured to USE system eigen")
+  if(EXISTS ${EIGEN_CATKIN_EIGEN_INCLUDE_SIGNATURE})
+    file(RENAME ${EIGEN_CATKIN_EIGEN_INCLUDE_SIGNATURE} ${EIGEN_CATKIN_EIGEN_INCLUDE_SIGNATURE}.off)
+
+    if(EIGEN3_INCLUDE_DIR EQUAL EIGEN_CATKIN_EIGEN_INCLUDE)
+      unset(EIGEN3_INCLUDE_DIR CACHE)
+    endif()
+    if(Eigen_INCLUDE_DIRS EQUAL EIGEN_CATKIN_EIGEN_INCLUDE)
+      unset(Eigen_INCLUDE_DIRS CACHE)
+    endif()
+  endif()
+
+  # see https://github.com/ethz-asl/ethzasl_icp_mapping/issues/38 for the discussion behind the following
+  find_package(Eigen3)
+
+  # Eigen3 will not be found on Ubuntu 12.04, so we moke it
+  if(EIGEN3_FOUND)
+    set(EIGEN_CATKIN_EIGEN_INCLUDE ${EIGEN3_INCLUDE_DIR} CACHE INTERNAL "")
+    set(EIGEN_NAME "EIGEN3")
+  else()
+    # Fallback to cmake_modules
+    find_package(Eigen REQUIRED)
+    if(EIGEN_FOUND)
+      set(EIGEN_CATKIN_EIGEN_INCLUDE ${Eigen_INCLUDE_DIRS} CACHE INTERNAL "")
+      set(EIGEN_NAME "Eigen")
+    endif()
+  endif()
+
+  if(EIGEN_NAME)
+    # this way ${EIGEN_NAME} will end up in the cs_export call's DEPENDS argument.
+    list(APPEND _DEPENDS ${EIGEN_NAME})
+    message(STATUS "Found and using system Eigen in ${EIGEN_CATKIN_EIGEN_INCLUDE}")
+  else()
+    message(FATAL_ERROR "Could not find a system Eigen installation! What about turning on eigen_catkin's own Eigen installation by removing USE_SYSTEM_EIGEN.cmake or switching back to the master branch.")
+  endif()
+else()
+  message(STATUS "eigen_catkin is configured NOT to use system eigen")
+  file(MAKE_DIRECTORY ${EIGEN_CATKIN_EIGEN_INCLUDE})
+  ExternalProject_Add(eigen_src
+    UPDATE_COMMAND ""
+    URL https://gitlab.com/libeigen/eigen/-/archive/3.3.4/eigen-3.3.4.tar.bz2
+    URL_MD5 6e74a04aeab3417120f1bdef6f3b4881
+    PATCH_COMMAND patch -p0 < ${CMAKE_CURRENT_SOURCE_DIR}/StdVector.patch && patch -p0 < ${CMAKE_CURRENT_SOURCE_DIR}/DisableTests.patch && patch -p0 < ${CMAKE_CURRENT_SOURCE_DIR}/FixWarning.patch
+    CMAKE_ARGS -DCMAKE_INSTALL_PREFIX:PATH=${CATKIN_DEVEL_PREFIX} -DCMAKE_BUILD_TYPE:STRING=Release -DBUILD_TESTING=OFF
+  )
+
+  if(EXISTS ${EIGEN_CATKIN_EIGEN_INCLUDE_SIGNATURE}.off)
+    file(RENAME ${EIGEN_CATKIN_EIGEN_INCLUDE_SIGNATURE}.off ${EIGEN_CATKIN_EIGEN_INCLUDE_SIGNATURE})
+  endif()
+
+  message(STATUS "Using eigen_catkin's Eigen in ${EIGEN_CATKIN_EIGEN_INCLUDE}")
+
+  install(DIRECTORY ${EIGEN_CATKIN_EIGEN_INCLUDE}
+          DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}
+  )
+endif()
+
+cs_export(INCLUDE_DIRS ${EIGEN_CATKIN_EIGEN_INCLUDE}
+          DEPENDS ${_DEPENDS}
+          CFG_EXTRAS eigen-extras.cmake)
diff --git a/RWR/src/eigen_catkin/DisableTests.patch b/RWR/src/eigen_catkin/DisableTests.patch
new file mode 100644
index 0000000000000000000000000000000000000000..ba03f2c5a2f7a99c35e135a1604f0c850c48a040
--- /dev/null
+++ b/RWR/src/eigen_catkin/DisableTests.patch
@@ -0,0 +1,77 @@
+--- CMakeLists.txt
++++ CMakeLists.txt
+@@ -416,16 +416,15 @@ add_subdirectory(Eigen)
+
+ add_subdirectory(doc EXCLUDE_FROM_ALL)
+
+-include(EigenConfigureTesting)
++option(BUILD_TESTING "Enable creation of Eigen tests." ON)
++if(BUILD_TESTING)
++  include(EigenConfigureTesting)
+
+-# fixme, not sure this line is still needed:
+-enable_testing() # must be called from the root CMakeLists, see man page
+-
+-
+-if(EIGEN_LEAVE_TEST_IN_ALL_TARGET)
+-  add_subdirectory(test) # can't do EXCLUDE_FROM_ALL here, breaks CTest
+-else()
+-  add_subdirectory(test EXCLUDE_FROM_ALL)
++  if(EIGEN_LEAVE_TEST_IN_ALL_TARGET)
++    add_subdirectory(test) # can't do EXCLUDE_FROM_ALL here, breaks CTest
++  else()
++    add_subdirectory(test EXCLUDE_FROM_ALL)
++  endif()
+ endif()
+
+ if(EIGEN_LEAVE_TEST_IN_ALL_TARGET)
+@@ -461,7 +460,9 @@ endif(NOT WIN32)
+
+ configure_file(scripts/cdashtesting.cmake.in cdashtesting.cmake @ONLY)
+
+-ei_testing_print_summary()
++if(BUILD_TESTING)
++  ei_testing_print_summary()
++endif()
+
+ message(STATUS "")
+ message(STATUS "Configured Eigen ${EIGEN_VERSION_NUMBER}")
+diff --git a/blas/CMakeLists.txt b/blas/CMakeLists.txt
+index d0efb4188..9887d5804 100644
+--- blas/CMakeLists.txt
++++ blas/CMakeLists.txt
+@@ -45,10 +45,12 @@ install(TARGETS eigen_blas eigen_blas_static
+
+ if(EIGEN_Fortran_COMPILER_WORKS)
+
+-if(EIGEN_LEAVE_TEST_IN_ALL_TARGET)
+-  add_subdirectory(testing) # can't do EXCLUDE_FROM_ALL here, breaks CTest
+-else()
+-  add_subdirectory(testing EXCLUDE_FROM_ALL)
++if(BUILD_TESTING)
++  if(EIGEN_LEAVE_TEST_IN_ALL_TARGET)
++    add_subdirectory(testing) # can't do EXCLUDE_FROM_ALL here, breaks CTest
++  else()
++    add_subdirectory(testing EXCLUDE_FROM_ALL)
++  endif()
+ endif()
+
+ endif()
+diff --git a/unsupported/CMakeLists.txt b/unsupported/CMakeLists.txt
+index 4fef40a86..9a5666105 100644
+--- unsupported/CMakeLists.txt
++++ unsupported/CMakeLists.txt
+@@ -1,7 +1,9 @@
+ add_subdirectory(Eigen)
+ add_subdirectory(doc EXCLUDE_FROM_ALL)
+-if(EIGEN_LEAVE_TEST_IN_ALL_TARGET)
+-  add_subdirectory(test) # can't do EXCLUDE_FROM_ALL here, breaks CTest
+-else()
+-  add_subdirectory(test EXCLUDE_FROM_ALL)
++if(BUILD_TESTING)
++  if(EIGEN_LEAVE_TEST_IN_ALL_TARGET)
++    add_subdirectory(test) # can't do EXCLUDE_FROM_ALL here, breaks CTest
++  else()
++    add_subdirectory(test EXCLUDE_FROM_ALL)
++  endif()
+ endif()
diff --git a/RWR/src/eigen_catkin/FixWarning.patch b/RWR/src/eigen_catkin/FixWarning.patch
new file mode 100644
index 0000000000000000000000000000000000000000..10e1b1abda6cbbafc30bf42eacacdb2e3723b689
--- /dev/null
+++ b/RWR/src/eigen_catkin/FixWarning.patch
@@ -0,0 +1,40 @@
+--- Eigen/src/Core/AssignEvaluator.h	2019-12-25 13:19:30.927689094 +0100
++++ Eigen/src/Core/AssignEvaluator.h	2019-12-25 13:19:32.779688334 +0100
+@@ -83,11 +83,11 @@
+                        && int(OuterStride)!=Dynamic && int(OuterStride)%int(InnerPacketSize)==0
+                        && (EIGEN_UNALIGNED_VECTORIZE  || int(JointAlignment)>=int(InnerRequiredAlignment)),
+     MayLinearize = bool(StorageOrdersAgree) && (int(DstFlags) & int(SrcFlags) & LinearAccessBit),
+-    MayLinearVectorize = bool(MightVectorize) && MayLinearize && DstHasDirectAccess
++    MayLinearVectorize = bool(MightVectorize) && MayLinearize && (DstHasDirectAccess!=0)
+                        && (EIGEN_UNALIGNED_VECTORIZE || (int(DstAlignment)>=int(LinearRequiredAlignment)) || MaxSizeAtCompileTime == Dynamic),
+       /* If the destination isn't aligned, we have to do runtime checks and we don't unroll,
+          so it's only good for large enough sizes. */
+-    MaySliceVectorize  = bool(MightVectorize) && bool(DstHasDirectAccess)
++    MaySliceVectorize  = bool(MightVectorize) && (DstHasDirectAccess!=0)
+                        && (int(InnerMaxSize)==Dynamic || int(InnerMaxSize)>=(EIGEN_UNALIGNED_VECTORIZE?InnerPacketSize:(3*InnerPacketSize)))
+       /* slice vectorization can be slow, so we only want it if the slices are big, which is
+          indicated by InnerMaxSize rather than InnerSize, think of the case of a dynamic block
+--- Eigen/src/Core/products/GeneralMatrixVector.h	2017-06-15 09:10:20.000000000 +0200
++++ Eigen/src/Core/products/GeneralMatrixVector.h	2019-12-25 13:59:41.286547822 +0100
+@@ -183,8 +183,8 @@
+     alignmentPattern = AllAligned;
+   }
+
+-  const Index offset1 = (FirstAligned && alignmentStep==1)?3:1;
+-  const Index offset3 = (FirstAligned && alignmentStep==1)?1:3;
++  const Index offset1 = (FirstAligned!=0 && alignmentStep==1)?3:1;
++  const Index offset3 = (FirstAligned!=0 && alignmentStep==1)?1:3;
+
+   Index columnBound = ((cols-skipColumns)/columnsAtOnce)*columnsAtOnce + skipColumns;
+   for (Index i=skipColumns; i<columnBound; i+=columnsAtOnce)
+@@ -457,8 +457,8 @@
+     alignmentPattern = AllAligned;
+   }
+
+-  const Index offset1 = (FirstAligned && alignmentStep==1)?3:1;
+-  const Index offset3 = (FirstAligned && alignmentStep==1)?1:3;
++  const Index offset1 = (FirstAligned!=0 && alignmentStep==1)?3:1;
++  const Index offset3 = (FirstAligned!=0 && alignmentStep==1)?1:3;
+
+   Index rowBound = ((rows-skipRows)/rowsAtOnce)*rowsAtOnce + skipRows;
+   for (Index i=skipRows; i<rowBound; i+=rowsAtOnce)
diff --git a/RWR/src/eigen_catkin/LICENSE b/RWR/src/eigen_catkin/LICENSE
new file mode 100644
index 0000000000000000000000000000000000000000..40f918099e1aadaf62966e88f101eb2bbb272735
--- /dev/null
+++ b/RWR/src/eigen_catkin/LICENSE
@@ -0,0 +1,27 @@
+Copyright (c) 2014-2019, Autonomous Systems Lab, ETH Zurich
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright
+notice, this list of conditions and the following disclaimer.
+
+2. Redistributions in binary form must reproduce the above copyright
+notice, this list of conditions and the following disclaimer in the
+documentation and/or other materials provided with the distribution.
+
+3. Neither the name of the Autonomous Systems Lab, ETH Zurich nor the
+names of its contributors may be used to endorse or promote products
+derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
diff --git a/RWR/src/eigen_catkin/README.md b/RWR/src/eigen_catkin/README.md
new file mode 100644
index 0000000000000000000000000000000000000000..5abb192f57340d70345016893f56e46a6315430e
--- /dev/null
+++ b/RWR/src/eigen_catkin/README.md
@@ -0,0 +1,8 @@
+Pull in a recent version of Eigen from upstream or find a system installation
+```
+<build_depend>eigen_catkin</build_depend>
+```
+
+To debug or analyze the effects of this library on your catkin workspace : https://github.com/ethz-asl/eigen_catkin_tools.
+
+For an overview of the possible reasons for segfaults / compiler errors / other issues see this wikipage: https://github.com/ethz-asl/eigen_catkin/wiki/Eigen-Memory-Issues
diff --git a/RWR/src/eigen_catkin/StdVector.patch b/RWR/src/eigen_catkin/StdVector.patch
new file mode 100644
index 0000000000000000000000000000000000000000..81ce3b868c5db304cd96ab21a85016bef7e56ef0
--- /dev/null
+++ b/RWR/src/eigen_catkin/StdVector.patch
@@ -0,0 +1,18 @@
+--- Eigen/StdVector
++++ Eigen/StdVector
+@@ -14,6 +14,8 @@
+ #include "Core"
+ #include <vector>
+
++#if __cplusplus < 201103L
++
+ #if EIGEN_COMP_MSVC && EIGEN_OS_WIN64 && (EIGEN_MAX_STATIC_ALIGN_BYTES<=16) /* MSVC auto aligns up to 16 bytes in 64 bit builds */
+
+ #define EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(...)
+@@ -24,4 +26,6 @@
+
+ #endif
+
++#endif
++
+ #endif // EIGEN_STDVECTOR_MODULE_H
diff --git a/RWR/src/eigen_catkin/cmake/eigen-extras.cmake.in b/RWR/src/eigen_catkin/cmake/eigen-extras.cmake.in
new file mode 100644
index 0000000000000000000000000000000000000000..ff2c1c87ca545b245b807497b6b630bc4bc8da45
--- /dev/null
+++ b/RWR/src/eigen_catkin/cmake/eigen-extras.cmake.in
@@ -0,0 +1,15 @@
+if("@INSTALLSPACE@" STREQUAL "TRUE" AND "@USE_SYSTEM_EIGEN@" STREQUAL "OFF")
+  set(@PROJECT_NAME@_INCLUDE_DIRS ${@PROJECT_NAME@_INCLUDE_DIRS}/eigen3)
+endif()
+
+# The following eigen_catkin_enforce macro can be called in a depending 
+# project's CMakeLists.txt file (Below findpackage eigen_catkin or the 
+# catkin_simple() call.
+# It will ensure that eigen_catkin's lib Eigen will be included instead 
+# of any other possible candidate (like a system installation of Eigen).
+#
+# It is possible to get that right by careful treatment of workspace layer 
+# order but calling this macro is easier.
+macro(eigen_catkin_enforce)
+  include_directories(BEFORE ${@PROJECT_NAME@_INCLUDE_DIRS})
+endmacro()
diff --git a/RWR/src/eigen_catkin/package.xml b/RWR/src/eigen_catkin/package.xml
new file mode 100644
index 0000000000000000000000000000000000000000..89143dd9aa67de5bd651e4b9fcfe2a82f6fe510c
--- /dev/null
+++ b/RWR/src/eigen_catkin/package.xml
@@ -0,0 +1,14 @@
+<?xml version="1.0"?>
+<package>
+  <name>eigen_catkin</name>
+  <version>3.2.12</version>
+  <description>eigen_catkin</description>
+  <maintainer email="schneith@ethz.ch">Thomas Schneider</maintainer>
+  <author>ASL</author>
+  <license>New BSD</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <buildtool_depend>catkin_simple</buildtool_depend>
+
+  <build_depend>cmake_modules</build_depend>
+</package>
diff --git a/RWR/src/eigen_catkin/use-system-installation-of-eigen.cmake b/RWR/src/eigen_catkin/use-system-installation-of-eigen.cmake
new file mode 100644
index 0000000000000000000000000000000000000000..4bc53ae9d70c2f4d747a17b2b1c2746638ac41cb
--- /dev/null
+++ b/RWR/src/eigen_catkin/use-system-installation-of-eigen.cmake
@@ -0,0 +1 @@
+set(USE_SYSTEM_EIGEN "AUTO")
diff --git a/RWR/src/eigen_checks/.gitignore b/RWR/src/eigen_checks/.gitignore
new file mode 100644
index 0000000000000000000000000000000000000000..0def2755dff457515a8bd19d98c5cff171d9a4f5
--- /dev/null
+++ b/RWR/src/eigen_checks/.gitignore
@@ -0,0 +1,21 @@
+# Compiled Object files
+*.slo
+*.lo
+*.o
+*.obj
+
+# Compiled Dynamic libraries
+*.so
+*.dylib
+*.dll
+
+# Compiled Static libraries
+*.lai
+*.la
+*.a
+*.lib
+
+# Executables
+*.exe
+*.out
+*.app
diff --git a/RWR/src/eigen_checks/CMakeLists.txt b/RWR/src/eigen_checks/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..dec31bf0413cb712fa846464aed82c46615d76cf
--- /dev/null
+++ b/RWR/src/eigen_checks/CMakeLists.txt
@@ -0,0 +1,79 @@
+# Copyright (c) 2015, Autonomous Systems Lab, ETH Zurich
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#     * Redistributions of source code must retain the above copyright
+#       notice, this list of conditions and the following disclaimer.
+#     * Redistributions in binary form must reproduce the above copyright
+#       notice, this list of conditions and the following disclaimer in the
+#       documentation and/or other materials provided with the distribution.
+#     * Neither the name of the <organization> nor the
+#       names of its contributors may be used to endorse or promote products
+#       derived from this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+# ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+# WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+# DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
+# DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+# (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+# ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+# (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+# SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+cmake_minimum_required(VERSION 2.8.3)
+project(eigen_checks)
+
+find_package(catkin_simple REQUIRED)
+catkin_simple()
+
+add_definitions(--std=c++0x -Wall -Wextra -pedantic)
+
+cs_add_library(${PROJECT_NAME} src/eigen-checks.cc)
+
+find_package (Threads)
+catkin_add_gtest(test_gtest_near
+	test/test_gtest-near.cc)
+target_link_libraries(test_gtest_near ${PROJECT_NAME} ${CMAKE_THREAD_LIBS_INIT})
+
+catkin_add_gtest(test_gtest_equal
+  test/test_gtest-equal.cc)
+target_link_libraries(test_gtest_equal ${PROJECT_NAME} ${CMAKE_THREAD_LIBS_INIT})
+
+catkin_add_gtest(test_gtest_zero
+  test/test_gtest-zero.cc)
+target_link_libraries(test_gtest_zero ${PROJECT_NAME} ${CMAKE_THREAD_LIBS_INIT})
+
+catkin_add_gtest(test_gtest_equal_double
+  test/test_gtest-equal-double.cc)
+target_link_libraries(test_gtest_equal_double ${PROJECT_NAME} ${CMAKE_THREAD_LIBS_INIT})
+
+catkin_add_gtest(test_glog_near
+  test/test_glog-near.cc)
+target_link_libraries(test_glog_near ${PROJECT_NAME} ${CMAKE_THREAD_LIBS_INIT})
+
+catkin_add_gtest(test_glog_equal
+  test/test_glog-equal.cc)
+target_link_libraries(test_glog_equal ${PROJECT_NAME} ${CMAKE_THREAD_LIBS_INIT})
+
+catkin_add_gtest(test_glog_zero
+  test/test_glog-zero.cc)
+target_link_libraries(test_glog_zero ${PROJECT_NAME} ${CMAKE_THREAD_LIBS_INIT})
+
+catkin_add_gtest(test_glog_equal_double
+  test/test_glog-equal-double.cc)
+target_link_libraries(test_glog_equal_double ${PROJECT_NAME} ${CMAKE_THREAD_LIBS_INIT})
+
+catkin_add_gtest(test_glog-zero-sized-types
+  test/test_glog-zero-sized-types.cc)
+target_link_libraries(test_glog-zero-sized-types ${PROJECT_NAME} ${CMAKE_THREAD_LIBS_INIT})
+
+catkin_add_gtest(test_gtest-zero-sized-types
+  test/test_gtest-zero-sized-types.cc)
+target_link_libraries(test_gtest-zero-sized-types ${PROJECT_NAME} ${CMAKE_THREAD_LIBS_INIT})
+
+cs_install()
+
+cs_export(INCLUDE_DIRS include
+          CFG_EXTRAS eigen_checks-extras.cmake)
diff --git a/RWR/src/eigen_checks/README.md b/RWR/src/eigen_checks/README.md
new file mode 100644
index 0000000000000000000000000000000000000000..92e695c4e27f38f20d5ee7df07086eaa0b924e84
--- /dev/null
+++ b/RWR/src/eigen_checks/README.md
@@ -0,0 +1,123 @@
+Eigen check macros
+============
+
+A repository for glog CHECK and gtest ASSERT and EXPECT macros for Eigen types.
+
+Licensed under the 3-clause BSD license ("New BSD").
+
+```
+Copyright (c) 2015, Autonomous Systems Lab, ETH Zurich
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the <organization> nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+```
+
+## GLOG
+The Glog `CHECK()` macros are used to check errors and exit your program if they are found. These Eigen equivalents can be used to check conditions on matrices. See [the glog documentation](https://google-glog.googlecode.com/svn/trunk/doc/glog.html) for a full description. The general form of use is:
+
+```c++
+CHECK_EIGEN_MATRIX_EQUAL(MatrixA, MatrixB) << "Informative error message!";
+```
+All tests happen component-wise. They fail if the matrices are different sizes. If the matrices are the same size, the test is applied to each corresponding pair of components.
+
+To get these macros, use:
+
+```c++
+#include<eigen-checks/glog.h>
+```
+
+#### `CHECK_EIGEN_MATRIX_EQUAL(MatrixA, MatrixB)`
+
+Checks if two matrices are binary equal.
+
+#### `CHECK_EIGEN_MATRIX_EQUAL_DOUBLE(MatrixA, MatrixB)`
+
+Checks if two matrices are equal to floating-point precision
+
+#### `CHECK_EIGEN_MATRIX_NEAR(MatrixA, MatrixB, Precision)`
+
+Checks if two matrices are equal to a user-specified precision.
+
+#### `CHECK_EIGEN_MATRIX_ZERO(MatrixA, Precision)`
+
+Checks if a matrix is equal to zero to a user-specified precision.
+
+## GTEST
+The gtest macros are built to facilitate unit testing with matrix types. This library provides two pieces of functionality: an macro that defines the main function, or *entrypoint*, for a gtest invocation, and several macros for testing if matrices are similar.
+
+### GTest 
+The Gtest `EXPECT()` and `ASSERT()` macros are used to verify assumptions in your tests and print useful information in case these don't match the expected outcome. These Eigen equivalents can be used to check conditions on matrices. See [the gtest documentation](https://code.google.com/p/googletest/wiki/Primer#Assertions) for a full description. The general form of use is:
+
+```c++
+EXPECT_TRUE(EIGEN_MATRIX_EQUAL(MatrixA, MatrixB));
+EXPECT_FALSE(EIGEN_MATRIX_EQUAL(MatrixA, MatrixB));
+ASSERT_TRUE(EIGEN_MATRIX_EQUAL(MatrixA, MatrixB));
+ASSERT_FALSE(EIGEN_MATRIX_EQUAL(MatrixA, MatrixB));
+```
+
+All tests happen component-wise. They fail if the matrices are different sizes. If the matrices are the same size, the test is applied to each corresponding pair of components.
+
+To get these macros, use:
+
+```c++
+#include<eigen-checks/gtest.h>
+```
+
+#### `EXPECT_TRUE(EIGEN_MATRIX_EQUAL(MatrixA, MatrixB))`
+
+Succeeds if two matrices are binary equal.
+
+#### `EXPECT_TRUE(EIGEN_MATRIX_EQUAL_DOUBLE(MatrixA, MatrixB))`
+
+Succeeds if two matrices are equal to floating-point precision
+
+#### `EXPECT_TRUE(EIGEN_MATRIX_NEAR(MatrixA, MatrixB, Precision))`
+
+Succeeds if two matrices are equal to a user-specified precision.
+
+#### `EXPECT_TRUE(EIGEN_MATRIX_ZERO(MatrixA, Precision))`
+
+Succeeds if a matrix is equal to zero to a user-specified precision.
+
+
+### Entrypoint
+
+Instead of writing the following code in your unit-test runner file:
+
+```c++
+#include <gtest/gtest.h>
+int main(int argc, char** argv) {
+  testing::InitGoogleTest(&argc, argv);
+  google::ParseCommandLineFlags(&argc, &argv, true);
+  ::testing::FLAGS_gtest_death_test_style = "threadsafe";
+  //...
+  return RUN_ALL_TESTS();
+}
+```
+You can just use:
+
+```c++
+#include <eigen-checks/entrypoint.h>
+
+UNITTEST_ENTRYPOINT
+```
diff --git a/RWR/src/eigen_checks/cmake/eigen_checks-extras.cmake.in b/RWR/src/eigen_checks/cmake/eigen_checks-extras.cmake.in
new file mode 100644
index 0000000000000000000000000000000000000000..509e14c263df92cdf4bb4aaae9e97bb418111317
--- /dev/null
+++ b/RWR/src/eigen_checks/cmake/eigen_checks-extras.cmake.in
@@ -0,0 +1,5 @@
+if (CMAKE_COMPILER_IS_GNUCC)
+SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
+else()
+SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
+endif()
diff --git a/RWR/src/eigen_checks/include/eigen-checks/entrypoint.h b/RWR/src/eigen_checks/include/eigen-checks/entrypoint.h
new file mode 100644
index 0000000000000000000000000000000000000000..6f708ebaceb65fa251f325f8b2ce29ea86b5229a
--- /dev/null
+++ b/RWR/src/eigen_checks/include/eigen-checks/entrypoint.h
@@ -0,0 +1,62 @@
+// Copyright (c) 2015, Autonomous Systems Lab, ETH Zurich
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the <organization> nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#ifndef EIGEN_CHECKS_ENTRYPOINT_H_
+#define EIGEN_CHECKS_ENTRYPOINT_H_
+
+#include <gflags/gflags.h>
+#include <gtest/gtest.h>
+#include <glog/logging.h>
+
+// Let the Eclipse parser see the macro.
+#ifndef TEST
+#define TEST(a, b) int Test_##a##_##b()
+#endif
+
+#ifndef TEST_F
+#define TEST_F(a, b) int Test_##a##_##b()
+#endif
+
+#ifndef TEST_P
+#define TEST_P(a, b) int Test_##a##_##b()
+#endif
+
+#ifndef TYPED_TEST_CASE
+#define TYPED_TEST_CASE(a, b) int Test_##a##_##b()
+#endif
+
+#ifndef TYPED_TEST
+#define TYPED_TEST(a, b) int Test_##a##_##b()
+#endif
+
+#define UNITTEST_ENTRYPOINT \
+    int main(int argc, char** argv) { \
+  google::InitGoogleLogging(argv[0]); \
+  testing::InitGoogleTest(&argc, argv); \
+  google::ParseCommandLineFlags(&argc, &argv, true); \
+  ::testing::FLAGS_gtest_death_test_style = "threadsafe"; \
+  return RUN_ALL_TESTS();\
+}
+
+#endif  // EIGEN_CHECKS_ENTRYPOINT_H_
diff --git a/RWR/src/eigen_checks/include/eigen-checks/glog.h b/RWR/src/eigen_checks/include/eigen-checks/glog.h
new file mode 100644
index 0000000000000000000000000000000000000000..84853a4c1eadd1f550a0846b2cf62921fd067839
--- /dev/null
+++ b/RWR/src/eigen_checks/include/eigen-checks/glog.h
@@ -0,0 +1,72 @@
+// Copyright (c) 2015, Autonomous Systems Lab, ETH Zurich
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the <organization> nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#ifndef EIGEN_CHECKS_GLOG_H_
+#define EIGEN_CHECKS_GLOG_H_
+#include <limits>
+#include <type_traits>
+
+#include <eigen-checks/internal/gtest-equal.h>
+#include <glog/logging.h>
+
+// To allow the user to append custom messages we store the result of the
+// verification inside a unique local variable to be used in the check and for
+// outputting the message.
+#define INTERNAL_EIGEN_CHECKS_LOCAL_UNIQUE_VAR(var, line) var ## line
+#define INTERNAL_EIGEN_CHECKS_MAKE_VAR(var, line) \
+  INTERNAL_EIGEN_CHECKS_LOCAL_UNIQUE_VAR(var, line)
+
+#define INTERNAL_EIGEN_CHECKS_MAKE_GLOG_CHECK(X)                             \
+  ::testing::AssertionResult INTERNAL_EIGEN_CHECKS_MAKE_VAR(                 \
+      result_, __LINE__) = (X);                                              \
+  CHECK(static_cast<bool>(INTERNAL_EIGEN_CHECKS_MAKE_VAR(result_, __LINE__)))\
+      << (INTERNAL_EIGEN_CHECKS_MAKE_VAR(result_, __LINE__)).message()
+
+#define CHECK_EIGEN_MATRIX_EQUAL(MatrixA, MatrixB)                           \
+  INTERNAL_EIGEN_CHECKS_MAKE_GLOG_CHECK(                                     \
+    eigen_checks::internal::MatricesNear(                                    \
+    MatrixA, #MatrixA, MatrixB, #MatrixB,                                    \
+    static_cast<typename std::remove_reference<                              \
+    decltype(MatrixA)>::type::Scalar>(0.0), "0.0"))
+
+#define CHECK_EIGEN_MATRIX_EQUAL_DOUBLE(MatrixA, MatrixB)                    \
+  INTERNAL_EIGEN_CHECKS_MAKE_GLOG_CHECK(                                     \
+      eigen_checks::internal::MatricesNear(                                  \
+        MatrixA, #MatrixA, MatrixB, #MatrixB,                                \
+        static_cast<typename std::remove_reference<                          \
+        decltype(MatrixA)>::type::Scalar>(                                   \
+      eigen_checks::internal::kDefaultPrecision),                            \
+      "Floating point precision"))
+
+#define CHECK_EIGEN_MATRIX_NEAR(MatrixA, MatrixB, Precision)                 \
+  INTERNAL_EIGEN_CHECKS_MAKE_GLOG_CHECK(                                     \
+      eigen_checks::internal::MatricesNear(                                  \
+        MatrixA, #MatrixA, MatrixB, #MatrixB, Precision, #Precision))
+
+#define CHECK_EIGEN_MATRIX_ZERO(MatrixA, Precision)                          \
+  INTERNAL_EIGEN_CHECKS_MAKE_GLOG_CHECK(                                     \
+  eigen_checks::internal::MatrixZero(                                        \
+      MatrixA, #MatrixA, Precision, #Precision))
+
+#endif  // EIGEN_CHECKS_GLOG_H_
diff --git a/RWR/src/eigen_checks/include/eigen-checks/gtest.h b/RWR/src/eigen_checks/include/eigen-checks/gtest.h
new file mode 100644
index 0000000000000000000000000000000000000000..b9c29550f7b3c720e2609df60cc46d6b75f5924a
--- /dev/null
+++ b/RWR/src/eigen_checks/include/eigen-checks/gtest.h
@@ -0,0 +1,50 @@
+// Copyright (c) 2015, Autonomous Systems Lab, ETH Zurich
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the <organization> nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#ifndef EIGEN_CHECKS_GTEST_H_
+#define EIGEN_CHECKS_GTEST_H_
+#include <limits>
+#include <type_traits>
+
+#include <eigen-checks/internal/gtest-equal.h>
+
+#define EIGEN_MATRIX_EQUAL(MatrixA, MatrixB)                                 \
+  eigen_checks::internal::MatricesEqual(MatrixA, #MatrixA, MatrixB, #MatrixB, \
+      static_cast<typename std::remove_reference<                            \
+      decltype(MatrixA)>::type::Scalar>(0.0), "0.0")
+
+#define EIGEN_MATRIX_EQUAL_DOUBLE(MatrixA, MatrixB)                          \
+  eigen_checks::internal::MatricesEqual(MatrixA, #MatrixA, MatrixB, #MatrixB, \
+      static_cast<typename std::remove_reference<                            \
+      decltype(MatrixA)>::type::Scalar>(                                     \
+      eigen_checks::internal::kDefaultPrecision), "Floating point precision")
+
+#define EIGEN_MATRIX_NEAR(MatrixA, MatrixB, Precision)                       \
+  eigen_checks::internal::MatricesNear(MatrixA, #MatrixA, MatrixB, #MatrixB, \
+                                       Precision, #Precision)
+
+#define EIGEN_MATRIX_ZERO(MatrixA, Precision)                                \
+  eigen_checks::internal::MatrixZero(MatrixA, #MatrixA, Precision, #Precision)
+
+#endif  // EIGEN_CHECKS_GTEST_H_
diff --git a/RWR/src/eigen_checks/include/eigen-checks/internal/gtest-equal.h b/RWR/src/eigen_checks/include/eigen-checks/internal/gtest-equal.h
new file mode 100644
index 0000000000000000000000000000000000000000..8c1a20baff43c2b9fff972acf44096140b16b7f0
--- /dev/null
+++ b/RWR/src/eigen_checks/include/eigen-checks/internal/gtest-equal.h
@@ -0,0 +1,161 @@
+// Copyright (c) 2015, Autonomous Systems Lab, ETH Zurich
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the <organization> nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#ifndef EIGEN_CHECKS_INTERNAL_GTEST_H_
+#define EIGEN_CHECKS_INTERNAL_GTEST_H_
+
+#include <Eigen/Dense>
+#include <glog/logging.h>
+#include <gtest/gtest.h>
+
+namespace eigen_checks {
+namespace internal {
+constexpr double kDefaultPrecision = 1e-10;
+
+template<typename LeftMat, typename RightMat>
+::testing::AssertionResult MatricesEqual(const LeftMat& A,
+                                         const std::string& name_lhs,
+                                         const RightMat& B,
+                                         const std::string& name_rhs,
+                                         double threshold,
+                                         const std::string& name_threshold) {
+  if (A.rows() != B.rows() || A.cols() != B.cols()) {
+    return ::testing::AssertionFailure()
+      << "Matrix size mismatch: "
+      << A.rows() << "x" << A.cols() << " (" << name_lhs << ") != "
+      << B.rows() << "x" << B.cols() << " (" << name_rhs << ")";
+  }
+
+  bool success = true;
+  std::string message;
+  for (int i = 0; i < A.rows(); ++i) {
+    for (int j = 0; j < A.cols(); ++j) {
+      double Aij = A(i, j);
+      double Bij = B(i, j);
+      if (std::abs(Aij - Bij) > threshold) {
+        success = false;
+        message +=
+            "\n  Mismatch at [" + std::to_string(i) + "," + std::to_string(j) +
+            "] : " + std::to_string(Aij) + " != " + std::to_string(Bij) +
+            " (" + name_threshold + " = " + std::to_string(threshold) + ")";
+      }
+    }
+  }
+
+  return success ?
+      ::testing::AssertionSuccess() :
+      ::testing::AssertionFailure() << message << std::endl;
+}
+
+template<typename LHSMatrix, typename RHSMatrix>
+::testing::AssertionResult MatricesNear(
+    const Eigen::MatrixBase<LHSMatrix>& lhs,
+    const std::string& name_lhs,
+    const Eigen::MatrixBase<RHSMatrix>& rhs,
+    const std::string& name_rhs,
+    typename Eigen::MatrixBase<LHSMatrix>::Scalar tolerance,
+    const std::string& name_tolerance) {
+  if (lhs.rows() != rhs.rows()) {
+    ::testing::AssertionResult failure_reason(false);
+    failure_reason << "The matrices have a different number of rows: "
+        << name_lhs << " has " << lhs.rows() << " rows while " << name_rhs
+        << " has " << rhs.rows() << " rows.\n";
+    return failure_reason;
+  }
+  if (lhs.cols() != rhs.cols()) {
+    ::testing::AssertionResult failure_reason(false);
+    failure_reason << "The matrices have a different number of cols: "
+        << name_lhs << " has " << lhs.cols() << " cols while " << name_rhs
+        << " cols " << rhs.cols() << " cols.\n";
+    failure_reason << name_lhs << ":\n" << lhs << "\n";
+    failure_reason << name_rhs << ":\n" << rhs << "\n";
+    return failure_reason;
+  }
+
+  // Early exit for dynamic-sized matrices where one dimension is zero. No need to check values...
+  if(rhs.rows() == 0 || rhs.cols() == 0 || lhs.rows() == 0 || lhs.cols() == 0) {
+    return ::testing::AssertionSuccess();
+  }
+
+  typedef typename Eigen::MatrixBase<LHSMatrix>::Scalar Scalar;
+  const Scalar max_diff = (lhs - rhs).cwiseAbs().maxCoeff();
+
+  if (max_diff <= tolerance) {
+    return ::testing::AssertionSuccess();
+  } else {
+    ::testing::AssertionResult failure_reason(false);
+    failure_reason << "The matrices are different. The maximum difference "
+        << "between " << name_lhs << " and " << name_rhs << " is " << max_diff
+        << ", which exceeds " << tolerance << ", where\n";
+    for (int i = 0; i < lhs.rows(); ++i) {
+      for (int j = 0; j < lhs.cols(); ++j) {
+        const Scalar& lij = lhs(i, j);
+        const Scalar& rij = rhs(i, j);
+        const Scalar& diff = std::abs(lij - rij);
+        if (!std::isfinite(lij) ||
+            !std::isfinite(rij) ||
+             diff > tolerance) {
+          if (lhs.rows() == 1) {
+            failure_reason <<
+                "\nposition " << j << " evaluates to " << lij << " and " << rij;
+          } else if (lhs.cols() == 1) {
+            failure_reason <<
+                "\nposition " << i << " evaluates to " << lij << " and " << rij;
+          } else {
+            failure_reason << "\nposition " << i << "," << j << " evaluates to "
+                << lij << " and " << rij;
+          }
+          failure_reason << " with a tolerance of " << name_tolerance << ".\n";
+        }
+      }
+    }
+    failure_reason << name_lhs << ":\n" << lhs << "\n";
+    failure_reason << name_rhs << ":\n" << rhs << "\n";
+    failure_reason << "Difference:\n" << (lhs - rhs) << "\n";
+    return failure_reason;
+  }
+}
+
+template<typename LHSMatrix>
+::testing::AssertionResult MatrixZero(
+    const Eigen::MatrixBase<LHSMatrix>& lhs,
+    const std::string& name_lhs,
+    typename Eigen::MatrixBase<LHSMatrix>::Scalar tolerance,
+    const std::string& name_tolerance) {
+  if (lhs.isZero(tolerance)) {
+    return ::testing::AssertionSuccess();
+  } else {
+    // Make a copy to get the same size matrix even in the dynamic size case.
+    Eigen::Matrix<typename Eigen::MatrixBase<LHSMatrix>::Scalar,
+        Eigen::Dynamic, Eigen::Dynamic> zero;
+    zero.setZero(lhs.rows(), lhs.cols());
+    ::testing::AssertionResult failure_reason =
+        MatricesNear(lhs, name_lhs, zero, "Zero", tolerance, name_tolerance);
+    CHECK_EQ(false, static_cast<bool>(failure_reason));
+    return failure_reason;
+  }
+}
+}  // namespace internal
+}  // namespace eigen_checks
+#endif  // EIGEN_CHECKS_INTERNAL_GTEST_H_
diff --git a/RWR/src/eigen_checks/package.xml b/RWR/src/eigen_checks/package.xml
new file mode 100644
index 0000000000000000000000000000000000000000..fe4449b2b3bff3799fdff12722a1eb7b5a2ceaa4
--- /dev/null
+++ b/RWR/src/eigen_checks/package.xml
@@ -0,0 +1,20 @@
+<package>
+  <name>eigen_checks</name>
+  <version>0.0.1</version>
+  <description>Glog and Gtest check, assert, expect macros for eigen types.</description>
+  <maintainer email="simon.lynen@mavt.ethz.ch">simonlynen</maintainer>
+
+  <license>New BSD</license>
+
+  <url type="website">https://github.com/ethz-asl/eigen_checks</url>
+  <url type="bugtracker">https://github.com/ethz-asl/eigen_checks/issues</url>
+
+  <author>Simon Lynen</author>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <buildtool_depend>catkin_simple</buildtool_depend>
+
+  <build_depend>eigen_catkin</build_depend>
+  <build_depend>gflags_catkin</build_depend>
+  <build_depend>glog_catkin</build_depend>
+</package>
diff --git a/RWR/src/eigen_checks/src/eigen-checks.cc b/RWR/src/eigen_checks/src/eigen-checks.cc
new file mode 100644
index 0000000000000000000000000000000000000000..eb6f8c7cf664c54a9fa819744d7de82dee91bfd8
--- /dev/null
+++ b/RWR/src/eigen_checks/src/eigen-checks.cc
@@ -0,0 +1,24 @@
+// Copyright (c) 2015, Autonomous Systems Lab, ETH Zurich
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the <organization> nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
diff --git a/RWR/src/eigen_checks/test/test_glog-equal-double.cc b/RWR/src/eigen_checks/test/test_glog-equal-double.cc
new file mode 100644
index 0000000000000000000000000000000000000000..76485e7f7b04ca5d497f8599b6fc9ac2854c37c3
--- /dev/null
+++ b/RWR/src/eigen_checks/test/test_glog-equal-double.cc
@@ -0,0 +1,78 @@
+// Copyright (c) 2015, Autonomous Systems Lab, ETH Zurich
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the <organization> nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#include <eigen-checks/entrypoint.h>
+#include <eigen-checks/glog.h>
+
+#include "test_helper.h"
+
+TYPED_TEST(EigenChecks, EigenMatrixEqualDoubleGLog_Matrix_Equal) {
+  GLOG_TEST_EXPECT_NO_DEATH(
+      CHECK_EIGEN_MATRIX_EQUAL_DOUBLE(this->ground_truth_matrix_54,
+                                      this->test_matrix_54_equal));
+}
+
+TYPED_TEST(EigenChecks, EigenMatrixEqualDoubleGLog_Matrix_NonEqualRandom) {
+  GLOG_TEST_EXPECT_DEATH_DIFFERENT_DATA(
+      CHECK_EIGEN_MATRIX_EQUAL_DOUBLE(this->ground_truth_matrix_54,
+                                      this->test_matrix_54_random));
+}
+
+TYPED_TEST(EigenChecks, EigenMatrixEqualDoubleGLog_Matrix_OtherSize) {
+  GLOG_TEST_EXPECT_DEATH_DIFFERENT_SIZE(
+      CHECK_EIGEN_MATRIX_EQUAL_DOUBLE(this->ground_truth_matrix_54,
+                                      this->test_matrix_D4));
+}
+
+TYPED_TEST(EigenChecks, EigenMatrixEqualDoubleGLog_Matrix_Near5) {
+  GLOG_TEST_EXPECT_DEATH_DIFFERENT_DATA(
+      CHECK_EIGEN_MATRIX_EQUAL_DOUBLE(this->ground_truth_matrix_54,
+                                      this->test_matrix_54_near_e_minus_5));
+}
+
+TYPED_TEST(EigenChecks, EigenMatrixEqualDoubleGLog_Vector_Equal) {
+  GLOG_TEST_EXPECT_NO_DEATH(
+      CHECK_EIGEN_MATRIX_EQUAL_DOUBLE(this->ground_truth_vector_5,
+                                      this->test_vector_5_equal));
+}
+
+TYPED_TEST(EigenChecks, EigenMatrixEqualDoubleGLog_Vector_NonEqualRandom) {
+  GLOG_TEST_EXPECT_DEATH_DIFFERENT_DATA(
+      CHECK_EIGEN_MATRIX_EQUAL_DOUBLE(this->ground_truth_vector_5,
+                                      this->test_vector_5_random));
+}
+
+TYPED_TEST(EigenChecks, EigenMatrixEqualDoubleGLog_Vector_OtherSize) {
+  GLOG_TEST_EXPECT_DEATH_DIFFERENT_DATA(
+      CHECK_EIGEN_MATRIX_EQUAL_DOUBLE(this->ground_truth_vector_5,
+                                      this->test_vector_D));
+}
+
+TYPED_TEST(EigenChecks, EigenMatrixEqualDoubleGLog_Vector_Near5) {
+  GLOG_TEST_EXPECT_DEATH_DIFFERENT_DATA(
+      CHECK_EIGEN_MATRIX_EQUAL_DOUBLE(this->ground_truth_vector_5,
+                                      this->test_vector_5_near_e_minus_5));
+}
+
+UNITTEST_ENTRYPOINT
diff --git a/RWR/src/eigen_checks/test/test_glog-equal.cc b/RWR/src/eigen_checks/test/test_glog-equal.cc
new file mode 100644
index 0000000000000000000000000000000000000000..47bfced9cae681fd49a0835f83304076aac4cddd
--- /dev/null
+++ b/RWR/src/eigen_checks/test/test_glog-equal.cc
@@ -0,0 +1,78 @@
+// Copyright (c) 2015, Autonomous Systems Lab, ETH Zurich
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the <organization> nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#include <eigen-checks/entrypoint.h>
+#include <eigen-checks/glog.h>
+
+#include "test_helper.h"
+
+TYPED_TEST(EigenChecks, EigenMatrixEqualGLog_Matrix_Equal) {
+  GLOG_TEST_EXPECT_NO_DEATH(
+      CHECK_EIGEN_MATRIX_EQUAL(this->ground_truth_matrix_54,
+                               this->test_matrix_54_equal));
+}
+
+TYPED_TEST(EigenChecks, EigenMatrixEqualGLog_Matrix_NonEqualRandom) {
+  GLOG_TEST_EXPECT_DEATH_DIFFERENT_DATA(
+      CHECK_EIGEN_MATRIX_EQUAL(this->ground_truth_matrix_54,
+                               this->test_matrix_54_random));
+}
+
+TYPED_TEST(EigenChecks, EigenMatrixEqualGLog_Matrix_OtherSize) {
+  GLOG_TEST_EXPECT_DEATH_DIFFERENT_SIZE(
+      CHECK_EIGEN_MATRIX_EQUAL(this->ground_truth_matrix_54,
+                               this->test_matrix_D4));
+}
+
+TYPED_TEST(EigenChecks, EigenMatrixEqualGLog_Matrix_Near5) {
+  GLOG_TEST_EXPECT_DEATH_DIFFERENT_DATA(
+      CHECK_EIGEN_MATRIX_EQUAL(this->ground_truth_matrix_54,
+                               this->test_matrix_54_near_e_minus_5));
+}
+
+TYPED_TEST(EigenChecks, EigenMatrixEqualGLog_Vector_Equal) {
+  GLOG_TEST_EXPECT_NO_DEATH(
+      CHECK_EIGEN_MATRIX_EQUAL(this->ground_truth_vector_5,
+                               this->test_vector_5_equal));
+}
+
+TYPED_TEST(EigenChecks, EigenMatrixEqualGLog_Vector_NonEqualRandom) {
+  GLOG_TEST_EXPECT_DEATH_DIFFERENT_DATA(
+      CHECK_EIGEN_MATRIX_EQUAL(this->ground_truth_vector_5,
+                               this->test_vector_5_random));
+}
+
+TYPED_TEST(EigenChecks, EigenMatrixEqualGLog_Vector_OtherSize) {
+  GLOG_TEST_EXPECT_DEATH_DIFFERENT_DATA(
+      CHECK_EIGEN_MATRIX_EQUAL(this->ground_truth_vector_5,
+                               this->test_vector_D));
+}
+
+TYPED_TEST(EigenChecks, EigenMatrixEqualGLog_Vector_Near5) {
+  GLOG_TEST_EXPECT_DEATH_DIFFERENT_DATA(
+      CHECK_EIGEN_MATRIX_EQUAL(this->ground_truth_vector_5,
+                               this->test_vector_5_near_e_minus_5));
+}
+
+UNITTEST_ENTRYPOINT
diff --git a/RWR/src/eigen_checks/test/test_glog-near.cc b/RWR/src/eigen_checks/test/test_glog-near.cc
new file mode 100644
index 0000000000000000000000000000000000000000..6657fc43926167ed7da2fba8a9f6762bb672e7ad
--- /dev/null
+++ b/RWR/src/eigen_checks/test/test_glog-near.cc
@@ -0,0 +1,86 @@
+// Copyright (c) 2015, Autonomous Systems Lab, ETH Zurich
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the <organization> nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#include <eigen-checks/entrypoint.h>
+#include <eigen-checks/glog.h>
+
+#include "test_helper.h"
+
+TYPED_TEST(EigenChecks, EigenMatrixNearGLog_Matrix_Equal) {
+  GLOG_TEST_EXPECT_NO_DEATH(CHECK_EIGEN_MATRIX_NEAR(this->ground_truth_matrix_54,
+                                              this->test_matrix_54_equal,
+                                              1e-8));
+}
+
+TYPED_TEST(EigenChecks, EigenMatrixNearGLog_Matrix_EqualFloatingPoint) {
+  GLOG_TEST_EXPECT_NO_DEATH(
+      CHECK_EIGEN_MATRIX_NEAR(this->ground_truth_matrix_54,
+                        this->test_matrix_54_equal_floating_point, 1e-9));
+}
+
+TYPED_TEST(EigenChecks, EigenMatrixNearGLog_Matrix_NonEqualRandom) {
+  GLOG_TEST_EXPECT_DEATH_DIFFERENT_DATA(CHECK_EIGEN_MATRIX_NEAR(
+      this->ground_truth_matrix_54, this->test_matrix_54_random, 1e-8));
+}
+
+TYPED_TEST(EigenChecks, EigenMatrixNearGLog_Matrix_OtherSize) {
+  GLOG_TEST_EXPECT_DEATH_DIFFERENT_SIZE(
+      CHECK_EIGEN_MATRIX_NEAR(this->ground_truth_matrix_54,
+                              this->test_matrix_D4, 1e-8));
+}
+
+TYPED_TEST(EigenChecks, EigenMatrixNearGLog_Matrix_Near5) {
+  GLOG_TEST_EXPECT_NO_DEATH(CHECK_EIGEN_MATRIX_NEAR(
+      this->ground_truth_matrix_54, this->test_matrix_54_near_e_minus_5, 1e-4));
+}
+
+TYPED_TEST(EigenChecks, EigenMatrixNearGLog_Vector_Equal) {
+  GLOG_TEST_EXPECT_NO_DEATH(CHECK_EIGEN_MATRIX_NEAR(
+      this->ground_truth_vector_5, this->test_vector_5_equal, 1e-8));
+}
+
+TYPED_TEST(EigenChecks, EigenMatrixNearGLog_Vector_EqualFloatingPoint) {
+  GLOG_TEST_EXPECT_NO_DEATH(
+      CHECK_EIGEN_MATRIX_NEAR(this->ground_truth_vector_5,
+                              this->test_vector_5_equal_floating_point,
+                              1e-9));
+}
+
+TYPED_TEST(EigenChecks, EigenMatrixNearGLog_Vector_NonEqualRandom) {
+  GLOG_TEST_EXPECT_DEATH_DIFFERENT_DATA(CHECK_EIGEN_MATRIX_NEAR(
+      this->ground_truth_vector_5, this->test_vector_5_random, 1e-8));
+}
+
+TYPED_TEST(EigenChecks, EigenMatrixNearGLog_Vector_OtherSize) {
+  GLOG_TEST_EXPECT_DEATH_DIFFERENT_SIZE(CHECK_EIGEN_MATRIX_NEAR(
+      this->ground_truth_vector_5, this->test_vector_D, 1e-8));
+}
+
+TYPED_TEST(EigenChecks, EigenMatrixNearGLog_Vector_Near5) {
+  GLOG_TEST_EXPECT_NO_DEATH(
+      CHECK_EIGEN_MATRIX_NEAR(this->ground_truth_vector_5,
+                        this->test_vector_5_near_e_minus_5, 1e-4));
+}
+
+UNITTEST_ENTRYPOINT
diff --git a/RWR/src/eigen_checks/test/test_glog-zero-sized-types.cc b/RWR/src/eigen_checks/test/test_glog-zero-sized-types.cc
new file mode 100644
index 0000000000000000000000000000000000000000..8f1e4232c59ec9ff22cb9d77a7c2b82f9b84ff22
--- /dev/null
+++ b/RWR/src/eigen_checks/test/test_glog-zero-sized-types.cc
@@ -0,0 +1,46 @@
+// Copyright (c) 2015, Autonomous Systems Lab, ETH Zurich
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the <organization> nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#include <eigen-checks/entrypoint.h>
+#include <eigen-checks/glog.h>
+#include <gtest/gtest.h>
+
+#include "test_helper.h"
+
+TEST(EigenChecks, EigenMatrixEqualGlog_Zero_Sized_Dynamic_Matrix) {
+  Eigen::Matrix2Xd A;
+  Eigen::Matrix2Xd B(2, 3);
+  GLOG_TEST_EXPECT_DEATH_DIFFERENT_DATA(CHECK_EIGEN_MATRIX_NEAR(A, B, 0.01));
+  GLOG_TEST_EXPECT_NO_DEATH(CHECK_EIGEN_MATRIX_NEAR(A, A, 0.01));
+}
+
+TEST(EigenChecks, EigenVectorEqualGlog_Zero_Sized_Dynamic_Vector) {
+  Eigen::VectorXd A;
+  Eigen::VectorXd B(3);
+  GLOG_TEST_EXPECT_DEATH_DIFFERENT_DATA(CHECK_EIGEN_MATRIX_NEAR(A, B, 0.01));
+  GLOG_TEST_EXPECT_NO_DEATH(CHECK_EIGEN_MATRIX_NEAR(A, A, 0.01));
+}
+
+UNITTEST_ENTRYPOINT
+
diff --git a/RWR/src/eigen_checks/test/test_glog-zero.cc b/RWR/src/eigen_checks/test/test_glog-zero.cc
new file mode 100644
index 0000000000000000000000000000000000000000..ccb5786b481e7387c14d5628c82b35e5adeb3a66
--- /dev/null
+++ b/RWR/src/eigen_checks/test/test_glog-zero.cc
@@ -0,0 +1,70 @@
+// Copyright (c) 2015, Autonomous Systems Lab, ETH Zurich
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the <organization> nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#include <eigen-checks/entrypoint.h>
+#include <eigen-checks/glog.h>
+
+#include "test_helper.h"
+
+TYPED_TEST(EigenChecks, EigenMatrixZeroGLog_Matrix_Equal) {
+  GLOG_TEST_EXPECT_NO_DEATH(
+      CHECK_EIGEN_MATRIX_ZERO(this->ground_truth_matrix_54_zero, 1e-5));
+}
+
+TYPED_TEST(EigenChecks, EigenMatrixZeroGLog_Matrix_NonEqualRandom) {
+  GLOG_TEST_EXPECT_DEATH_DIFFERENT_DATA(
+      CHECK_EIGEN_MATRIX_ZERO(this->test_matrix_54_random, 1e-5));
+}
+
+TYPED_TEST(EigenChecks, EigenMatrixZeroGLog_Matrix_OtherSize) {
+  GLOG_TEST_EXPECT_DEATH_DIFFERENT_SIZE(
+      CHECK_EIGEN_MATRIX_ZERO(this->test_matrix_D4, 1e-5));
+}
+
+TYPED_TEST(EigenChecks, EigenMatrixZeroGLog_Matrix_Near5) {
+  GLOG_TEST_EXPECT_DEATH_DIFFERENT_DATA(
+      CHECK_EIGEN_MATRIX_ZERO(this->test_matrix_54_near_e_minus_5, 1e-5));
+}
+
+TYPED_TEST(EigenChecks, EigenMatrixZeroGLog_Vector_Equal) {
+  GLOG_TEST_EXPECT_NO_DEATH(
+      CHECK_EIGEN_MATRIX_ZERO(this->ground_truth_vector_5_zero, 1e-5));
+}
+
+TYPED_TEST(EigenChecks, EigenMatrixZeroGLog_Vector_NonEqualRandom) {
+  GLOG_TEST_EXPECT_DEATH_DIFFERENT_DATA(
+      CHECK_EIGEN_MATRIX_ZERO(this->ground_truth_vector_5, 1e-5));
+}
+
+TYPED_TEST(EigenChecks, EigenMatrixZeroGLog_Vector_OtherSize) {
+  GLOG_TEST_EXPECT_DEATH_DIFFERENT_DATA(
+      CHECK_EIGEN_MATRIX_ZERO(this->ground_truth_vector_5, 1e-5));
+}
+
+TYPED_TEST(EigenChecks, EigenMatrixZeroGLog_Vector_Near5) {
+  GLOG_TEST_EXPECT_DEATH_DIFFERENT_DATA(
+      CHECK_EIGEN_MATRIX_ZERO(this->ground_truth_vector_5, 1e-5));
+}
+
+UNITTEST_ENTRYPOINT
diff --git a/RWR/src/eigen_checks/test/test_gtest-equal-double.cc b/RWR/src/eigen_checks/test/test_gtest-equal-double.cc
new file mode 100644
index 0000000000000000000000000000000000000000..f47064534acb4471e28fdd3fb4b86419e0da75a4
--- /dev/null
+++ b/RWR/src/eigen_checks/test/test_gtest-equal-double.cc
@@ -0,0 +1,80 @@
+// Copyright (c) 2015, Autonomous Systems Lab, ETH Zurich
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the <organization> nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#include <eigen-checks/entrypoint.h>
+#include <eigen-checks/gtest.h>
+
+#include "test_helper.h"
+
+TYPED_TEST(EigenChecks, EigenMatrixEqualGTestDouble_Matrix_Equal) {
+  EXPECT_TRUE(EIGEN_MATRIX_EQUAL_DOUBLE(this->ground_truth_matrix_54,
+                                        this->test_matrix_54_equal));
+}
+
+TYPED_TEST(EigenChecks, EigenMatrixEqualGTestDouble_Matrix_EqualFloatingPoint) {
+  EXPECT_TRUE(EIGEN_MATRIX_EQUAL_DOUBLE(
+      this->ground_truth_matrix_54, this->test_matrix_54_equal_floating_point));
+}
+
+TYPED_TEST(EigenChecks, EigenMatrixEqualGTestDouble_Matrix_NonEqualRandom) {
+  EXPECT_FALSE(EIGEN_MATRIX_EQUAL_DOUBLE(this->ground_truth_matrix_54,
+                                         this->test_matrix_54_random));
+}
+
+TYPED_TEST(EigenChecks, EigenMatrixEqualGTestDouble_Matrix_OtherSize) {
+  EXPECT_FALSE(EIGEN_MATRIX_EQUAL_DOUBLE(this->ground_truth_matrix_54,
+                                         this->test_matrix_D4));
+}
+
+TYPED_TEST(EigenChecks, EigenMatrixEqualGTestDouble_Matrix_Near5) {
+  EXPECT_FALSE(EIGEN_MATRIX_EQUAL_DOUBLE(this->ground_truth_matrix_54,
+                                         this->test_matrix_54_near_e_minus_5));
+}
+
+TYPED_TEST(EigenChecks, EigenMatrixEqualGTestDouble_Vector_Equal) {
+  EXPECT_TRUE(EIGEN_MATRIX_EQUAL_DOUBLE(this->ground_truth_vector_5,
+                                        this->test_vector_5_equal));
+}
+
+TYPED_TEST(EigenChecks, EigenMatrixEqualGTestDouble_Vector_EqualFloatingPoint) {
+  EXPECT_TRUE(EIGEN_MATRIX_EQUAL_DOUBLE(
+      this->ground_truth_vector_5, this->test_vector_5_equal_floating_point));
+}
+
+TYPED_TEST(EigenChecks, EigenMatrixEqualGTestDouble_Vector_NonEqualRandom) {
+  EXPECT_FALSE(EIGEN_MATRIX_EQUAL_DOUBLE(this->ground_truth_vector_5,
+                                         this->test_vector_5_random));
+}
+
+TYPED_TEST(EigenChecks, EigenMatrixEqualGTestDouble_Vector_OtherSize) {
+  EXPECT_FALSE(EIGEN_MATRIX_EQUAL_DOUBLE(this->ground_truth_vector_5,
+                                         this->test_vector_D));
+}
+
+TYPED_TEST(EigenChecks, EigenMatrixEqualGTestDouble_Vector_Near5) {
+  EXPECT_FALSE(EIGEN_MATRIX_EQUAL_DOUBLE(this->ground_truth_vector_5,
+                                         this->test_vector_5_near_e_minus_5));
+}
+
+UNITTEST_ENTRYPOINT
diff --git a/RWR/src/eigen_checks/test/test_gtest-equal.cc b/RWR/src/eigen_checks/test/test_gtest-equal.cc
new file mode 100644
index 0000000000000000000000000000000000000000..50de12bcf303d5c8a7d412ce74ff208f43b7abed
--- /dev/null
+++ b/RWR/src/eigen_checks/test/test_gtest-equal.cc
@@ -0,0 +1,70 @@
+// Copyright (c) 2015, Autonomous Systems Lab, ETH Zurich
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the <organization> nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#include <eigen-checks/entrypoint.h>
+#include <eigen-checks/gtest.h>
+
+#include "test_helper.h"
+
+TYPED_TEST(EigenChecks, EigenMatrixEqualGTest_Matrix_Equal) {
+  EXPECT_TRUE(EIGEN_MATRIX_EQUAL(this->ground_truth_matrix_54,
+                                 this->test_matrix_54_equal));
+}
+
+TYPED_TEST(EigenChecks, EigenMatrixEqualGTest_Matrix_NonEqualRandom) {
+  EXPECT_FALSE(EIGEN_MATRIX_EQUAL(this->ground_truth_matrix_54,
+                                  this->test_matrix_54_random));
+}
+
+TYPED_TEST(EigenChecks, EigenMatrixEqualGTest_Matrix_OtherSize) {
+  EXPECT_FALSE(EIGEN_MATRIX_EQUAL(this->ground_truth_matrix_54,
+                                  this->test_matrix_D4));
+}
+
+TYPED_TEST(EigenChecks, EigenMatrixEqualGTest_Matrix_Near5) {
+  EXPECT_FALSE(EIGEN_MATRIX_EQUAL(this->ground_truth_matrix_54,
+                                  this->test_matrix_54_near_e_minus_5));
+}
+
+TYPED_TEST(EigenChecks, EigenMatrixEqualGTest_Vector_Equal) {
+  EXPECT_TRUE(EIGEN_MATRIX_EQUAL(this->ground_truth_vector_5,
+                                 this->test_vector_5_equal));
+}
+
+TYPED_TEST(EigenChecks, EigenMatrixEqualGTest_Vector_NonEqualRandom) {
+  EXPECT_FALSE(EIGEN_MATRIX_EQUAL(this->ground_truth_vector_5,
+                                  this->test_vector_5_random));
+}
+
+TYPED_TEST(EigenChecks, EigenMatrixEqualGTest_Vector_OtherSize) {
+  EXPECT_FALSE(EIGEN_MATRIX_EQUAL(this->ground_truth_vector_5,
+                                  this->test_vector_D));
+}
+
+TYPED_TEST(EigenChecks, EigenMatrixEqualGTest_Vector_Near5) {
+  EXPECT_FALSE(EIGEN_MATRIX_EQUAL(this->ground_truth_vector_5,
+                                  this->test_vector_5_near_e_minus_5));
+}
+
+UNITTEST_ENTRYPOINT
diff --git a/RWR/src/eigen_checks/test/test_gtest-near.cc b/RWR/src/eigen_checks/test/test_gtest-near.cc
new file mode 100644
index 0000000000000000000000000000000000000000..2fdd03d7d70ff4c56d7551dbd7ee76ecfd33b50b
--- /dev/null
+++ b/RWR/src/eigen_checks/test/test_gtest-near.cc
@@ -0,0 +1,90 @@
+// Copyright (c) 2015, Autonomous Systems Lab, ETH Zurich
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the <organization> nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#include <eigen-checks/entrypoint.h>
+#include <eigen-checks/gtest.h>
+
+#include "test_helper.h"
+
+TYPED_TEST(EigenChecks, EigenMatrixNearGTest_Matrix_Equal) {
+  EXPECT_TRUE(EIGEN_MATRIX_NEAR(this->ground_truth_matrix_54,
+                                this->test_matrix_54_equal,
+                                1e-8));
+}
+
+TYPED_TEST(EigenChecks, EigenMatrixNearGTest_Matrix_EqualFloatingPoint) {
+  EXPECT_TRUE(EIGEN_MATRIX_NEAR(
+      this->ground_truth_matrix_54, this->test_matrix_54_equal_floating_point,
+      1e-9));
+}
+
+TYPED_TEST(EigenChecks, EigenMatrixNearGTest_Matrix_NonEqualRandom) {
+  EXPECT_FALSE(EIGEN_MATRIX_NEAR(this->ground_truth_matrix_54,
+                                 this->test_matrix_54_random,
+                                 1e-8));
+}
+
+TYPED_TEST(EigenChecks, EigenMatrixNearGTest_Matrix_OtherSize) {
+  EXPECT_FALSE(EIGEN_MATRIX_NEAR(this->ground_truth_matrix_54,
+                                 this->test_matrix_D4,
+                                 1e-8));
+}
+
+TYPED_TEST(EigenChecks, EigenMatrixNearGTest_Matrix_Near5) {
+  EXPECT_TRUE(EIGEN_MATRIX_NEAR(this->ground_truth_matrix_54,
+                                this->test_matrix_54_near_e_minus_5,
+                                1e-4));
+}
+
+TYPED_TEST(EigenChecks, EigenMatrixNearGTest_Vector_Equal) {
+  EXPECT_TRUE(EIGEN_MATRIX_NEAR(this->ground_truth_vector_5,
+                                this->test_vector_5_equal,
+                                1e-8));
+}
+
+TYPED_TEST(EigenChecks, EigenMatrixNearGTest_Vector_EqualFloatingPoint) {
+  EXPECT_TRUE(EIGEN_MATRIX_NEAR(
+      this->ground_truth_vector_5, this->test_vector_5_equal_floating_point,
+      1e-9));
+}
+
+TYPED_TEST(EigenChecks, EigenMatrixNearGTest_Vector_NonEqualRandom) {
+  EXPECT_FALSE(EIGEN_MATRIX_NEAR(this->ground_truth_vector_5,
+                                 this->test_vector_5_random,
+                                 1e-8));
+}
+
+TYPED_TEST(EigenChecks, EigenMatrixNearGTest_Vector_OtherSize) {
+  EXPECT_FALSE(EIGEN_MATRIX_NEAR(this->ground_truth_vector_5,
+                                 this->test_vector_D,
+                                 1e-8));
+}
+
+TYPED_TEST(EigenChecks, EigenMatrixNearGTest_Vector_Near5) {
+  EXPECT_TRUE(EIGEN_MATRIX_NEAR(this->ground_truth_vector_5,
+                                this->test_vector_5_near_e_minus_5,
+                                1e-4));
+}
+
+UNITTEST_ENTRYPOINT
diff --git a/RWR/src/eigen_checks/test/test_gtest-zero-sized-types.cc b/RWR/src/eigen_checks/test/test_gtest-zero-sized-types.cc
new file mode 100644
index 0000000000000000000000000000000000000000..1c544f3f5cae43c256c3dba94f2033672477032b
--- /dev/null
+++ b/RWR/src/eigen_checks/test/test_gtest-zero-sized-types.cc
@@ -0,0 +1,44 @@
+// Copyright (c) 2015, Autonomous Systems Lab, ETH Zurich
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the <organization> nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#include <eigen-checks/entrypoint.h>
+#include <eigen-checks/gtest.h>
+
+#include "test_helper.h"
+
+TEST(EigenChecks, EigenMatrixEqualGtest_Zero_Sized_Dynamic_Matrix) {
+  Eigen::Matrix2Xd A;
+  Eigen::Matrix2Xd B(2, 3);
+  EXPECT_FALSE(EIGEN_MATRIX_NEAR(A, B, 0.01));
+  EXPECT_TRUE(EIGEN_MATRIX_NEAR(A, A, 0.01));
+}
+
+TEST(EigenChecks, EigenVectorEqualGtest_Zero_Sized_Dynamic_Vector) {
+  Eigen::VectorXd A;
+  Eigen::VectorXd B(3);
+  EXPECT_FALSE(EIGEN_MATRIX_NEAR(A, B, 0.01));
+  EXPECT_TRUE(EIGEN_MATRIX_NEAR(A, A, 0.01));
+}
+
+UNITTEST_ENTRYPOINT
diff --git a/RWR/src/eigen_checks/test/test_gtest-zero.cc b/RWR/src/eigen_checks/test/test_gtest-zero.cc
new file mode 100644
index 0000000000000000000000000000000000000000..62f154c7fccfe2e8512d34046ba8cf32d4f89583
--- /dev/null
+++ b/RWR/src/eigen_checks/test/test_gtest-zero.cc
@@ -0,0 +1,62 @@
+// Copyright (c) 2015, Autonomous Systems Lab, ETH Zurich
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the <organization> nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#include <eigen-checks/entrypoint.h>
+#include <eigen-checks/gtest.h>
+
+#include "test_helper.h"
+
+TYPED_TEST(EigenChecks, EigenMatrixZeroGTest_Matrix_Equal) {
+  EXPECT_TRUE(EIGEN_MATRIX_ZERO(this->ground_truth_matrix_54_zero, 1e-8));
+}
+
+TYPED_TEST(EigenChecks, EigenMatrixZeroGTest_Matrix_NonEqualRandom) {
+  EXPECT_FALSE(EIGEN_MATRIX_ZERO(this->test_matrix_54_random, 1e-8));
+}
+
+TYPED_TEST(EigenChecks, EigenMatrixZeroGTest_Matrix_OtherSize) {
+  EXPECT_FALSE(EIGEN_MATRIX_ZERO(this->test_matrix_D4, 1e-8));
+}
+
+TYPED_TEST(EigenChecks, EigenMatrixZeroGTest_Matrix_Near5) {
+  EXPECT_FALSE(EIGEN_MATRIX_ZERO(this->test_matrix_54_near_e_minus_5, 1e-8));
+}
+
+TYPED_TEST(EigenChecks, EigenMatrixZeroGTest_Vector_Equal) {
+  EXPECT_TRUE(EIGEN_MATRIX_ZERO(this->ground_truth_vector_5_zero, 1e-8));
+}
+
+TYPED_TEST(EigenChecks, EigenMatrixZeroGTest_Vector_NonEqualRandom) {
+  EXPECT_FALSE(EIGEN_MATRIX_ZERO(this->test_vector_5_random, 1e-8));
+}
+
+TYPED_TEST(EigenChecks, EigenMatrixZeroGTest_Vector_OtherSize) {
+  EXPECT_FALSE(EIGEN_MATRIX_ZERO(this->test_vector_D, 1e-8));
+}
+
+TYPED_TEST(EigenChecks, EigenMatrixZeroGTest_Vector_Near5) {
+  EXPECT_FALSE(EIGEN_MATRIX_ZERO(this->test_vector_5_near_e_minus_5, 1e-8));
+}
+
+UNITTEST_ENTRYPOINT
diff --git a/RWR/src/eigen_checks/test/test_helper.h b/RWR/src/eigen_checks/test/test_helper.h
new file mode 100644
index 0000000000000000000000000000000000000000..9b15459ed954528e794c100bce5b763ecfa8730f
--- /dev/null
+++ b/RWR/src/eigen_checks/test/test_helper.h
@@ -0,0 +1,138 @@
+// Copyright (c) 2015, Autonomous Systems Lab, ETH Zurich
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the <organization> nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#ifndef EIGEN_CHECKS_TEST_HELPER_H_
+#define EIGEN_CHECKS_TEST_HELPER_H_
+#include <limits>
+#include <random>
+
+#include <Eigen/Dense>
+#include <gtest/gtest.h>
+#include <glog/logging.h>
+
+#include <eigen-checks/internal/gtest-equal.h>
+
+template <typename Scalar>
+class EigenChecks : public testing::Test {
+ protected:
+  virtual void SetUp() {
+    int random_seed_ = 42;
+    std::mt19937 gen(random_seed_);
+    std::normal_distribution<> dis(10.0, 5.0);
+    Eigen::Matrix<Scalar, 5, 1> perturbance_v;
+    perturbance_v << dis(gen), dis(gen), dis(gen), dis(gen), dis(gen);
+    perturbance_v /= perturbance_v.maxCoeff();
+
+    ground_truth_vector_5 << dis(gen), dis(gen), dis(gen), dis(gen), dis(gen);
+
+    ground_truth_vector_5_zero.setZero();
+
+    test_vector_D.resize(4, Eigen::NoChange);
+    test_vector_D.template block<4, 1>(0, 0) =
+        ground_truth_vector_5.template block<4, 1>(0, 0);
+
+    test_vector_5_random << dis(gen), dis(gen), dis(gen), dis(gen), dis(gen);
+
+    test_vector_5_near_e_minus_5 = ground_truth_vector_5;
+    test_vector_5_near_e_minus_5 +=
+        perturbance_v * static_cast<Scalar>(1e-5);
+
+    test_vector_5_equal = ground_truth_vector_5;
+
+    test_vector_5_equal_floating_point = ground_truth_vector_5;
+    test_vector_5_equal_floating_point +=
+        perturbance_v * static_cast<Scalar>(0.9 *
+            eigen_checks::internal::kDefaultPrecision);
+
+    // Test matrix.
+    Eigen::Matrix<Scalar, 5, 4> perturbance_m;
+    perturbance_m <<
+        dis(gen), dis(gen), dis(gen), dis(gen),
+        dis(gen), dis(gen), dis(gen), dis(gen),
+        dis(gen), dis(gen), dis(gen), dis(gen),
+        dis(gen), dis(gen), dis(gen), dis(gen),
+        dis(gen), dis(gen), dis(gen), dis(gen);
+    perturbance_m /= perturbance_m.maxCoeff();
+
+    ground_truth_matrix_54 <<
+        dis(gen), dis(gen), dis(gen), dis(gen),
+        dis(gen), dis(gen), dis(gen), dis(gen),
+        dis(gen), dis(gen), dis(gen), dis(gen),
+        dis(gen), dis(gen), dis(gen), dis(gen),
+        dis(gen), dis(gen), dis(gen), dis(gen);
+
+    ground_truth_matrix_54_zero.setZero();
+
+    test_matrix_D4.resize(4, Eigen::NoChange);
+    test_matrix_D4.template block<4, 4>(0, 0) =
+        ground_truth_matrix_54.template block<4, 4>(0, 0);
+
+    test_matrix_54_random <<
+        dis(gen), dis(gen), dis(gen), dis(gen),
+        dis(gen), dis(gen), dis(gen), dis(gen),
+        dis(gen), dis(gen), dis(gen), dis(gen),
+        dis(gen), dis(gen), dis(gen), dis(gen),
+        dis(gen), dis(gen), dis(gen), dis(gen);
+
+    test_matrix_54_near_e_minus_5 = ground_truth_matrix_54;
+    test_matrix_54_near_e_minus_5 +=
+        perturbance_m * static_cast<Scalar>(1e-5);
+
+    test_matrix_54_equal = ground_truth_matrix_54;
+
+    test_matrix_54_equal_floating_point = ground_truth_matrix_54;
+    test_matrix_54_equal_floating_point +=
+        perturbance_m * static_cast<Scalar>(0.9 *
+            eigen_checks::internal::kDefaultPrecision);
+  }
+
+ protected:
+  Eigen::Matrix<Scalar, 5, 1> ground_truth_vector_5;
+  Eigen::Matrix<Scalar, 5, 1> ground_truth_vector_5_zero;
+  Eigen::Matrix<Scalar, Eigen::Dynamic, 1> test_vector_D;
+  Eigen::Matrix<Scalar, 5, 1> test_vector_5_random;
+  Eigen::Matrix<Scalar, 5, 1> test_vector_5_near_e_minus_5;
+  Eigen::Matrix<Scalar, 5, 1> test_vector_5_equal;
+  Eigen::Matrix<Scalar, 5, 1> test_vector_5_equal_floating_point;
+  Eigen::Matrix<Scalar, 5, 4> ground_truth_matrix_54;
+  Eigen::Matrix<Scalar, 5, 4> ground_truth_matrix_54_zero;
+  Eigen::Matrix<Scalar, Eigen::Dynamic, 4> test_matrix_D4;
+  Eigen::Matrix<Scalar, 5, 4> test_matrix_54_random;
+  Eigen::Matrix<Scalar, 5, 4> test_matrix_54_near_e_minus_5;
+  Eigen::Matrix<Scalar, 5, 4> test_matrix_54_equal;
+  Eigen::Matrix<Scalar, 5, 4> test_matrix_54_equal_floating_point;
+};
+
+typedef ::testing::Types<double, float> ScalarTypes;
+TYPED_TEST_CASE(EigenChecks, ScalarTypes);
+
+#define GLOG_TEST_EXPECT_NO_DEATH(X) X
+
+#define GLOG_TEST_EXPECT_DEATH_DIFFERENT_SIZE(X) \
+  EXPECT_DEATH(X, "^")  // ^The matrices have a different$
+
+#define GLOG_TEST_EXPECT_DEATH_DIFFERENT_DATA(X) \
+  EXPECT_DEATH(X, "^")  // ^The matrices are different$
+
+#endif  // EIGEN_CHECKS_TEST_HELPER_H_
diff --git a/RWR/src/gflags_catkin/.gitignore b/RWR/src/gflags_catkin/.gitignore
new file mode 100644
index 0000000000000000000000000000000000000000..2d3e7ceefc89e854086047a313af73284241399e
--- /dev/null
+++ b/RWR/src/gflags_catkin/.gitignore
@@ -0,0 +1,25 @@
+# Compiled Object files
+*.slo
+*.lo
+*.o
+*.obj
+
+# Compiled Dynamic libraries
+*.so
+*.dylib
+*.dll
+
+# Compiled Static libraries
+*.lai
+*.la
+*.a
+*.lib
+
+# Executables
+*.exe
+*.out
+*.app
+
+# Bloom.
+debian
+obj*
diff --git a/RWR/src/gflags_catkin/CMakeLists.txt b/RWR/src/gflags_catkin/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..eba614303feca7b847a31fa6f4d60dddd24c8cbc
--- /dev/null
+++ b/RWR/src/gflags_catkin/CMakeLists.txt
@@ -0,0 +1,37 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(gflags_catkin)
+
+find_package(catkin_simple REQUIRED)
+catkin_simple()
+
+include(ExternalProject)
+
+file(MAKE_DIRECTORY ${CATKIN_DEVEL_PREFIX}/include)
+
+ExternalProject_Add(
+  gflags_src
+  URL https://github.com/gflags/gflags/archive/v2.2.1.zip
+  URL_MD5 2d988ef0b50939fb50ada965dafce96b
+  UPDATE_COMMAND ""
+  CONFIGURE_COMMAND cd ../gflags_src &&
+     cmake .
+       -DCMAKE_INSTALL_PREFIX:PATH=${CATKIN_DEVEL_PREFIX}
+       -DBUILD_SHARED_LIBS:BOOL=true
+       -DGFLAGS_NAMESPACE:STRING=google
+       -DCMAKE_BUILD_TYPE:STRING=Release
+       -DCMAKE_TOOLCHAIN_FILE:STRING=${CMAKE_TOOLCHAIN_FILE}
+  BUILD_COMMAND cd ../gflags_src && make -j 8
+  INSTALL_COMMAND cd ../gflags_src && make install -j 8
+)
+
+install(DIRECTORY ${CATKIN_DEVEL_PREFIX}/include/gflags
+        DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}
+        FILES_MATCHING PATTERN "*.h")
+install(DIRECTORY ${CATKIN_DEVEL_PREFIX}/lib/
+        DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+        FILES_MATCHING PATTERN "libgflags*")
+install(FILES ${CATKIN_DEVEL_PREFIX}/bin/gflags_completions.sh
+        PERMISSIONS OWNER_READ OWNER_WRITE OWNER_EXECUTE GROUP_READ GROUP_EXECUTE WORLD_READ WORLD_EXECUTE
+        DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION})
+cs_export(INCLUDE_DIRS ${CATKIN_DEVEL_PREFIX}/include
+          LIBRARIES gflags)
diff --git a/RWR/src/gflags_catkin/README.md b/RWR/src/gflags_catkin/README.md
new file mode 100644
index 0000000000000000000000000000000000000000..36ca4a8874de75866c512cfbf69630a14fddf026
--- /dev/null
+++ b/RWR/src/gflags_catkin/README.md
@@ -0,0 +1,4 @@
+gflags_catkin
+=============
+
+A catkin wrapper for Google gflags
diff --git a/RWR/src/gflags_catkin/package.xml b/RWR/src/gflags_catkin/package.xml
new file mode 100644
index 0000000000000000000000000000000000000000..c2b3549d71bbe5bf11a8cce300e6603d549b1e26
--- /dev/null
+++ b/RWR/src/gflags_catkin/package.xml
@@ -0,0 +1,11 @@
+<package>
+  <name>gflags_catkin</name>
+  <version>2.2.1</version>
+  <description>gflags_catkin</description>
+  <maintainer email="slynen@ethz.ch">Simon Lynen</maintainer>
+
+  <license>See package</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <buildtool_depend>catkin_simple</buildtool_depend>
+</package>
diff --git a/sim_ws/src/glog_catkin b/RWR/src/glog_catkin
similarity index 100%
rename from sim_ws/src/glog_catkin
rename to RWR/src/glog_catkin
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/lcd_monitor/CMakeLists.txt b/RWR/src/lcd_monitor/CMakeLists.txt
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/lcd_monitor/CMakeLists.txt
rename to RWR/src/lcd_monitor/CMakeLists.txt
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/lcd_monitor/launch/lcd_monitor.launch b/RWR/src/lcd_monitor/launch/lcd_monitor.launch
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/lcd_monitor/launch/lcd_monitor.launch
rename to RWR/src/lcd_monitor/launch/lcd_monitor.launch
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/lcd_monitor/package.xml b/RWR/src/lcd_monitor/package.xml
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/lcd_monitor/package.xml
rename to RWR/src/lcd_monitor/package.xml
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/lcd_monitor/scripts/sm_lcd_node.py b/RWR/src/lcd_monitor/scripts/sm_lcd_node.py
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/lcd_monitor/scripts/sm_lcd_node.py
rename to RWR/src/lcd_monitor/scripts/sm_lcd_node.py
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/lcd_monitor/setup.py b/RWR/src/lcd_monitor/setup.py
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/lcd_monitor/setup.py
rename to RWR/src/lcd_monitor/setup.py
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/lcd_monitor/src/lcd_monitor/I2C_LCD_driver.py b/RWR/src/lcd_monitor/src/lcd_monitor/I2C_LCD_driver.py
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/lcd_monitor/src/lcd_monitor/I2C_LCD_driver.py
rename to RWR/src/lcd_monitor/src/lcd_monitor/I2C_LCD_driver.py
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/esim_visualization/src/py/__init__.py b/RWR/src/lcd_monitor/src/lcd_monitor/__init__.py
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/esim_visualization/src/py/__init__.py
rename to RWR/src/lcd_monitor/src/lcd_monitor/__init__.py
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/lcd_monitor/src/lcd_monitor/sm_lcd_driver.py b/RWR/src/lcd_monitor/src/lcd_monitor/sm_lcd_driver.py
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/lcd_monitor/src/lcd_monitor/sm_lcd_driver.py
rename to RWR/src/lcd_monitor/src/lcd_monitor/sm_lcd_driver.py
diff --git a/sim_ws/src/minkindr b/RWR/src/minkindr
similarity index 100%
rename from sim_ws/src/minkindr
rename to RWR/src/minkindr
diff --git a/sim_ws/src/minkindr_ros b/RWR/src/minkindr_ros
similarity index 100%
rename from sim_ws/src/minkindr_ros
rename to RWR/src/minkindr_ros
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/.gitlab-ci.yml b/RWR/src/ros-i2cpwmboard/.gitlab-ci.yml
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/.gitlab-ci.yml
rename to RWR/src/ros-i2cpwmboard/.gitlab-ci.yml
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/CMakeLists.txt b/RWR/src/ros-i2cpwmboard/CMakeLists.txt
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/CMakeLists.txt
rename to RWR/src/ros-i2cpwmboard/CMakeLists.txt
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/README.md b/RWR/src/ros-i2cpwmboard/README.md
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/README.md
rename to RWR/src/ros-i2cpwmboard/README.md
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/Doxyfile b/RWR/src/ros-i2cpwmboard/doc/Doxyfile
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/Doxyfile
rename to RWR/src/ros-i2cpwmboard/doc/Doxyfile
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/doxygen.css b/RWR/src/ros-i2cpwmboard/doc/doxygen.css
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/doxygen.css
rename to RWR/src/ros-i2cpwmboard/doc/doxygen.css
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/footer.html b/RWR/src/ros-i2cpwmboard/doc/footer.html
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/footer.html
rename to RWR/src/ros-i2cpwmboard/doc/footer.html
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/header.html b/RWR/src/ros-i2cpwmboard/doc/header.html
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/header.html
rename to RWR/src/ros-i2cpwmboard/doc/header.html
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/arrowdown.png b/RWR/src/ros-i2cpwmboard/doc/html/arrowdown.png
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/arrowdown.png
rename to RWR/src/ros-i2cpwmboard/doc/html/arrowdown.png
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/arrowright.png b/RWR/src/ros-i2cpwmboard/doc/html/arrowright.png
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/arrowright.png
rename to RWR/src/ros-i2cpwmboard/doc/html/arrowright.png
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/bc_s.png b/RWR/src/ros-i2cpwmboard/doc/html/bc_s.png
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/bc_s.png
rename to RWR/src/ros-i2cpwmboard/doc/html/bc_s.png
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/bdwn.png b/RWR/src/ros-i2cpwmboard/doc/html/bdwn.png
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/bdwn.png
rename to RWR/src/ros-i2cpwmboard/doc/html/bdwn.png
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/closed.png b/RWR/src/ros-i2cpwmboard/doc/html/closed.png
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/closed.png
rename to RWR/src/ros-i2cpwmboard/doc/html/closed.png
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/dir_68267d1309a1af8e8297ef4c3efbcdba.html b/RWR/src/ros-i2cpwmboard/doc/html/dir_68267d1309a1af8e8297ef4c3efbcdba.html
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/dir_68267d1309a1af8e8297ef4c3efbcdba.html
rename to RWR/src/ros-i2cpwmboard/doc/html/dir_68267d1309a1af8e8297ef4c3efbcdba.html
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/doc.png b/RWR/src/ros-i2cpwmboard/doc/html/doc.png
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/doc.png
rename to RWR/src/ros-i2cpwmboard/doc/html/doc.png
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/doxygen.css b/RWR/src/ros-i2cpwmboard/doc/html/doxygen.css
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/doxygen.css
rename to RWR/src/ros-i2cpwmboard/doc/html/doxygen.css
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/doxygen.png b/RWR/src/ros-i2cpwmboard/doc/html/doxygen.png
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/doxygen.png
rename to RWR/src/ros-i2cpwmboard/doc/html/doxygen.png
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/files.html b/RWR/src/ros-i2cpwmboard/doc/html/files.html
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/files.html
rename to RWR/src/ros-i2cpwmboard/doc/html/files.html
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/folderclosed.png b/RWR/src/ros-i2cpwmboard/doc/html/folderclosed.png
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/folderclosed.png
rename to RWR/src/ros-i2cpwmboard/doc/html/folderclosed.png
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/folderopen.png b/RWR/src/ros-i2cpwmboard/doc/html/folderopen.png
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/folderopen.png
rename to RWR/src/ros-i2cpwmboard/doc/html/folderopen.png
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/globals.html b/RWR/src/ros-i2cpwmboard/doc/html/globals.html
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/globals.html
rename to RWR/src/ros-i2cpwmboard/doc/html/globals.html
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/globals_func.html b/RWR/src/ros-i2cpwmboard/doc/html/globals_func.html
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/globals_func.html
rename to RWR/src/ros-i2cpwmboard/doc/html/globals_func.html
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/group___services.html b/RWR/src/ros-i2cpwmboard/doc/html/group___services.html
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/group___services.html
rename to RWR/src/ros-i2cpwmboard/doc/html/group___services.html
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/group___topics.html b/RWR/src/ros-i2cpwmboard/doc/html/group___topics.html
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/group___topics.html
rename to RWR/src/ros-i2cpwmboard/doc/html/group___topics.html
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/i2cpwm__controller_8cpp.html b/RWR/src/ros-i2cpwmboard/doc/html/i2cpwm__controller_8cpp.html
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/i2cpwm__controller_8cpp.html
rename to RWR/src/ros-i2cpwmboard/doc/html/i2cpwm__controller_8cpp.html
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/i2cpwm__controller_8cpp_source.html b/RWR/src/ros-i2cpwmboard/doc/html/i2cpwm__controller_8cpp_source.html
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/i2cpwm__controller_8cpp_source.html
rename to RWR/src/ros-i2cpwmboard/doc/html/i2cpwm__controller_8cpp_source.html
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/index.html b/RWR/src/ros-i2cpwmboard/doc/html/index.html
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/index.html
rename to RWR/src/ros-i2cpwmboard/doc/html/index.html
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/jquery.js b/RWR/src/ros-i2cpwmboard/doc/html/jquery.js
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/jquery.js
rename to RWR/src/ros-i2cpwmboard/doc/html/jquery.js
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/modules.html b/RWR/src/ros-i2cpwmboard/doc/html/modules.html
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/modules.html
rename to RWR/src/ros-i2cpwmboard/doc/html/modules.html
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/nav_f.png b/RWR/src/ros-i2cpwmboard/doc/html/nav_f.png
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/nav_f.png
rename to RWR/src/ros-i2cpwmboard/doc/html/nav_f.png
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/nav_g.png b/RWR/src/ros-i2cpwmboard/doc/html/nav_g.png
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/nav_g.png
rename to RWR/src/ros-i2cpwmboard/doc/html/nav_g.png
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/nav_h.png b/RWR/src/ros-i2cpwmboard/doc/html/nav_h.png
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/nav_h.png
rename to RWR/src/ros-i2cpwmboard/doc/html/nav_h.png
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/open.png b/RWR/src/ros-i2cpwmboard/doc/html/open.png
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/open.png
rename to RWR/src/ros-i2cpwmboard/doc/html/open.png
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/splitbar.png b/RWR/src/ros-i2cpwmboard/doc/html/splitbar.png
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/splitbar.png
rename to RWR/src/ros-i2cpwmboard/doc/html/splitbar.png
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/styles.css b/RWR/src/ros-i2cpwmboard/doc/html/styles.css
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/styles.css
rename to RWR/src/ros-i2cpwmboard/doc/html/styles.css
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/sync_off.png b/RWR/src/ros-i2cpwmboard/doc/html/sync_off.png
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/sync_off.png
rename to RWR/src/ros-i2cpwmboard/doc/html/sync_off.png
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/sync_on.png b/RWR/src/ros-i2cpwmboard/doc/html/sync_on.png
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/sync_on.png
rename to RWR/src/ros-i2cpwmboard/doc/html/sync_on.png
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/tab_a.png b/RWR/src/ros-i2cpwmboard/doc/html/tab_a.png
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/tab_a.png
rename to RWR/src/ros-i2cpwmboard/doc/html/tab_a.png
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/tab_b.png b/RWR/src/ros-i2cpwmboard/doc/html/tab_b.png
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/tab_b.png
rename to RWR/src/ros-i2cpwmboard/doc/html/tab_b.png
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/tab_h.png b/RWR/src/ros-i2cpwmboard/doc/html/tab_h.png
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/tab_h.png
rename to RWR/src/ros-i2cpwmboard/doc/html/tab_h.png
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/tab_s.png b/RWR/src/ros-i2cpwmboard/doc/html/tab_s.png
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/tab_s.png
rename to RWR/src/ros-i2cpwmboard/doc/html/tab_s.png
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/tabs.css b/RWR/src/ros-i2cpwmboard/doc/html/tabs.css
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/html/tabs.css
rename to RWR/src/ros-i2cpwmboard/doc/html/tabs.css
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/styles.css b/RWR/src/ros-i2cpwmboard/doc/styles.css
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/doc/styles.css
rename to RWR/src/ros-i2cpwmboard/doc/styles.css
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/launch/i2cpwm_node.launch b/RWR/src/ros-i2cpwmboard/launch/i2cpwm_node.launch
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/launch/i2cpwm_node.launch
rename to RWR/src/ros-i2cpwmboard/launch/i2cpwm_node.launch
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/msg/Position.msg b/RWR/src/ros-i2cpwmboard/msg/Position.msg
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/msg/Position.msg
rename to RWR/src/ros-i2cpwmboard/msg/Position.msg
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/msg/PositionArray.msg b/RWR/src/ros-i2cpwmboard/msg/PositionArray.msg
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/msg/PositionArray.msg
rename to RWR/src/ros-i2cpwmboard/msg/PositionArray.msg
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/msg/Servo.msg b/RWR/src/ros-i2cpwmboard/msg/Servo.msg
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/msg/Servo.msg
rename to RWR/src/ros-i2cpwmboard/msg/Servo.msg
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/msg/ServoArray.msg b/RWR/src/ros-i2cpwmboard/msg/ServoArray.msg
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/msg/ServoArray.msg
rename to RWR/src/ros-i2cpwmboard/msg/ServoArray.msg
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/msg/ServoConfig.msg b/RWR/src/ros-i2cpwmboard/msg/ServoConfig.msg
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/msg/ServoConfig.msg
rename to RWR/src/ros-i2cpwmboard/msg/ServoConfig.msg
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/msg/ServoConfigArray.msg b/RWR/src/ros-i2cpwmboard/msg/ServoConfigArray.msg
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/msg/ServoConfigArray.msg
rename to RWR/src/ros-i2cpwmboard/msg/ServoConfigArray.msg
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/package.xml b/RWR/src/ros-i2cpwmboard/package.xml
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/package.xml
rename to RWR/src/ros-i2cpwmboard/package.xml
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/src/i2cpwm_controller.cpp b/RWR/src/ros-i2cpwmboard/src/i2cpwm_controller.cpp
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/src/i2cpwm_controller.cpp
rename to RWR/src/ros-i2cpwmboard/src/i2cpwm_controller.cpp
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/srv/DriveMode.srv b/RWR/src/ros-i2cpwmboard/srv/DriveMode.srv
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/srv/DriveMode.srv
rename to RWR/src/ros-i2cpwmboard/srv/DriveMode.srv
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/srv/IntValue.srv b/RWR/src/ros-i2cpwmboard/srv/IntValue.srv
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/srv/IntValue.srv
rename to RWR/src/ros-i2cpwmboard/srv/IntValue.srv
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/srv/ServosConfig.srv b/RWR/src/ros-i2cpwmboard/srv/ServosConfig.srv
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/srv/ServosConfig.srv
rename to RWR/src/ros-i2cpwmboard/srv/ServosConfig.srv
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/srv/StopServos.srv b/RWR/src/ros-i2cpwmboard/srv/StopServos.srv
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/ros-i2cpwmboard/srv/StopServos.srv
rename to RWR/src/ros-i2cpwmboard/srv/StopServos.srv
diff --git a/sim_ws/src/rpg_dvs_ros b/RWR/src/rpg_dvs_ros
similarity index 100%
rename from sim_ws/src/rpg_dvs_ros
rename to RWR/src/rpg_dvs_ros
diff --git a/sim_ws/src/rpg_esim/dependencies.yaml b/RWR/src/rpg_esim/dependencies.yaml
similarity index 100%
rename from sim_ws/src/rpg_esim/dependencies.yaml
rename to RWR/src/rpg_esim/dependencies.yaml
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/.gitignore b/RWR/src/rpg_esim/event_camera_simulator/.gitignore
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/.gitignore
rename to RWR/src/rpg_esim/event_camera_simulator/.gitignore
diff --git a/RWR/src/rpg_esim/event_camera_simulator/.vscode/c_cpp_properties.json b/RWR/src/rpg_esim/event_camera_simulator/.vscode/c_cpp_properties.json
new file mode 100644
index 0000000000000000000000000000000000000000..c57bf0cafed91f36e59350bb4136a54c9a62a611
--- /dev/null
+++ b/RWR/src/rpg_esim/event_camera_simulator/.vscode/c_cpp_properties.json
@@ -0,0 +1,44 @@
+{
+  "configurations": [
+    {
+      "browse": {
+        "databaseFilename": "",
+        "limitSymbolsToIncludedHeaders": true
+      },
+      "includePath": [
+        "/home/chris/3rdYear/RWR/Robot-With-Reflexes/sim_ws/devel/include/**",
+        "/opt/ros/noetic/include/**",
+        "/home/chris/3rdYear/RWR/Robot-With-Reflexes/sim_ws/src/rpg_dvs_ros/dvs_renderer/include/**",
+        "/home/chris/3rdYear/RWR/Robot-With-Reflexes/sim_ws/src/eigen_checks/include/**",
+        "/home/chris/3rdYear/RWR/Robot-With-Reflexes/sim_ws/src/rpg_esim/event_camera_simulator/esim/include/**",
+        "/home/chris/3rdYear/RWR/Robot-With-Reflexes/sim_ws/src/rpg_esim/event_camera_simulator/esim_common/include/**",
+        "/home/chris/3rdYear/RWR/Robot-With-Reflexes/sim_ws/src/rpg_esim/event_camera_simulator/esim_data_provider/include/**",
+        "/home/chris/3rdYear/RWR/Robot-With-Reflexes/sim_ws/src/rpg_esim/event_camera_simulator/esim_rendering/include/**",
+        "/home/chris/3rdYear/RWR/Robot-With-Reflexes/sim_ws/src/rpg_esim/event_camera_simulator/esim_trajectory/include/**",
+        "/home/chris/3rdYear/RWR/Robot-With-Reflexes/sim_ws/src/rpg_esim/event_camera_simulator/esim_unrealcv_bridge/include/**",
+        "/home/chris/3rdYear/RWR/Robot-With-Reflexes/sim_ws/src/rpg_esim/event_camera_simulator/esim_visualization/include/**",
+        "/home/chris/3rdYear/RWR/Robot-With-Reflexes/sim_ws/src/ze_oss/imp_bridge_opencv/include/**",
+        "/home/chris/3rdYear/RWR/Robot-With-Reflexes/sim_ws/src/ze_oss/imp_bridge_ros/include/**",
+        "/home/chris/3rdYear/RWR/Robot-With-Reflexes/sim_ws/src/ze_oss/imp_core/include/**",
+        "/home/chris/3rdYear/RWR/Robot-With-Reflexes/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_multi_objects_2d/include/**",
+        "/home/chris/3rdYear/RWR/Robot-With-Reflexes/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/include/**",
+        "/home/chris/3rdYear/RWR/Robot-With-Reflexes/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_panorama_renderer/include/**",
+        "/home/chris/3rdYear/RWR/Robot-With-Reflexes/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/include/**",
+        "/home/chris/3rdYear/RWR/Robot-With-Reflexes/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_unrealcv_renderer/include/**",
+        "/home/chris/3rdYear/RWR/Robot-With-Reflexes/sim_ws/src/minkindr/minkindr/include/**",
+        "/home/chris/3rdYear/RWR/Robot-With-Reflexes/sim_ws/src/minkindr_ros/minkindr_conversions/include/**",
+        "/home/chris/3rdYear/RWR/Robot-With-Reflexes/sim_ws/src/ze_oss/ze_cameras/include/**",
+        "/home/chris/3rdYear/RWR/Robot-With-Reflexes/sim_ws/src/ze_oss/ze_common/include/**",
+        "/home/chris/3rdYear/RWR/Robot-With-Reflexes/sim_ws/src/ze_oss/ze_matplotlib/include/**",
+        "/home/chris/3rdYear/RWR/Robot-With-Reflexes/sim_ws/src/ze_oss/ze_pangolin/include/**",
+        "/home/chris/3rdYear/RWR/Robot-With-Reflexes/sim_ws/src/ze_oss/ze_ros/include/**",
+        "/home/chris/3rdYear/RWR/Robot-With-Reflexes/sim_ws/src/ze_oss/ze_splines/include/**",
+        "/home/chris/3rdYear/RWR/Robot-With-Reflexes/sim_ws/src/ze_oss/ze_vi_simulation/include/**",
+        "/home/chris/3rdYear/RWR/Robot-With-Reflexes/sim_ws/src/ze_oss/ze_visualization/include/**",
+        "/usr/include/**"
+      ],
+      "name": "ROS"
+    }
+  ],
+  "version": 4
+}
\ No newline at end of file
diff --git a/RWR/src/rpg_esim/event_camera_simulator/.vscode/settings.json b/RWR/src/rpg_esim/event_camera_simulator/.vscode/settings.json
new file mode 100644
index 0000000000000000000000000000000000000000..1eb9395dfbf856d2057f3781ae82eeaa7eae06f8
--- /dev/null
+++ b/RWR/src/rpg_esim/event_camera_simulator/.vscode/settings.json
@@ -0,0 +1,13 @@
+{
+    "python.autoComplete.extraPaths": [
+        "/home/chris/3rdYear/RWR/Robot-With-Reflexes/sim_ws/devel/lib/python3/dist-packages",
+        "/opt/ros/noetic/lib/python3/dist-packages"
+    ],
+    "python.analysis.extraPaths": [
+        "/home/chris/3rdYear/RWR/Robot-With-Reflexes/sim_ws/devel/lib/python3/dist-packages",
+        "/opt/ros/noetic/lib/python3/dist-packages"
+    ],
+    "files.associations": {
+        "condition_variable": "cpp"
+    }
+}
\ No newline at end of file
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/esim/CMakeLists.txt b/RWR/src/rpg_esim/event_camera_simulator/esim/CMakeLists.txt
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/esim/CMakeLists.txt
rename to RWR/src/rpg_esim/event_camera_simulator/esim/CMakeLists.txt
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/esim/include/esim/esim/camera_simulator.hpp b/RWR/src/rpg_esim/event_camera_simulator/esim/include/esim/esim/camera_simulator.hpp
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/esim/include/esim/esim/camera_simulator.hpp
rename to RWR/src/rpg_esim/event_camera_simulator/esim/include/esim/esim/camera_simulator.hpp
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/esim/include/esim/esim/event_simulator.hpp b/RWR/src/rpg_esim/event_camera_simulator/esim/include/esim/esim/event_simulator.hpp
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/esim/include/esim/esim/event_simulator.hpp
rename to RWR/src/rpg_esim/event_camera_simulator/esim/include/esim/esim/event_simulator.hpp
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/esim/include/esim/esim/simulator.hpp b/RWR/src/rpg_esim/event_camera_simulator/esim/include/esim/esim/simulator.hpp
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/esim/include/esim/esim/simulator.hpp
rename to RWR/src/rpg_esim/event_camera_simulator/esim/include/esim/esim/simulator.hpp
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/esim/package.xml b/RWR/src/rpg_esim/event_camera_simulator/esim/package.xml
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/esim/package.xml
rename to RWR/src/rpg_esim/event_camera_simulator/esim/package.xml
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/esim/src/camera_simulator.cpp b/RWR/src/rpg_esim/event_camera_simulator/esim/src/camera_simulator.cpp
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/esim/src/camera_simulator.cpp
rename to RWR/src/rpg_esim/event_camera_simulator/esim/src/camera_simulator.cpp
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/esim/src/event_simulator.cpp b/RWR/src/rpg_esim/event_camera_simulator/esim/src/event_simulator.cpp
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/esim/src/event_simulator.cpp
rename to RWR/src/rpg_esim/event_camera_simulator/esim/src/event_simulator.cpp
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/esim/src/simulator.cpp b/RWR/src/rpg_esim/event_camera_simulator/esim/src/simulator.cpp
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/esim/src/simulator.cpp
rename to RWR/src/rpg_esim/event_camera_simulator/esim/src/simulator.cpp
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/esim/test/test_event_simulator.cpp b/RWR/src/rpg_esim/event_camera_simulator/esim/test/test_event_simulator.cpp
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/esim/test/test_event_simulator.cpp
rename to RWR/src/rpg_esim/event_camera_simulator/esim/test/test_event_simulator.cpp
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/esim_common/CMakeLists.txt b/RWR/src/rpg_esim/event_camera_simulator/esim_common/CMakeLists.txt
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/esim_common/CMakeLists.txt
rename to RWR/src/rpg_esim/event_camera_simulator/esim_common/CMakeLists.txt
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/esim_common/include/esim/common/types.hpp b/RWR/src/rpg_esim/event_camera_simulator/esim_common/include/esim/common/types.hpp
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/esim_common/include/esim/common/types.hpp
rename to RWR/src/rpg_esim/event_camera_simulator/esim_common/include/esim/common/types.hpp
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/esim_common/include/esim/common/utils.hpp b/RWR/src/rpg_esim/event_camera_simulator/esim_common/include/esim/common/utils.hpp
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/esim_common/include/esim/common/utils.hpp
rename to RWR/src/rpg_esim/event_camera_simulator/esim_common/include/esim/common/utils.hpp
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/esim_common/package.xml b/RWR/src/rpg_esim/event_camera_simulator/esim_common/package.xml
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/esim_common/package.xml
rename to RWR/src/rpg_esim/event_camera_simulator/esim_common/package.xml
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/esim_common/src/utils.cpp b/RWR/src/rpg_esim/event_camera_simulator/esim_common/src/utils.cpp
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/esim_common/src/utils.cpp
rename to RWR/src/rpg_esim/event_camera_simulator/esim_common/src/utils.cpp
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/esim_common/test/test_utils.cpp b/RWR/src/rpg_esim/event_camera_simulator/esim_common/test/test_utils.cpp
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/esim_common/test/test_utils.cpp
rename to RWR/src/rpg_esim/event_camera_simulator/esim_common/test/test_utils.cpp
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/esim_data_provider/CMakeLists.txt b/RWR/src/rpg_esim/event_camera_simulator/esim_data_provider/CMakeLists.txt
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/esim_data_provider/CMakeLists.txt
rename to RWR/src/rpg_esim/event_camera_simulator/esim_data_provider/CMakeLists.txt
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/esim_data_provider/include/esim/data_provider/data_provider_base.hpp b/RWR/src/rpg_esim/event_camera_simulator/esim_data_provider/include/esim/data_provider/data_provider_base.hpp
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/esim_data_provider/include/esim/data_provider/data_provider_base.hpp
rename to RWR/src/rpg_esim/event_camera_simulator/esim_data_provider/include/esim/data_provider/data_provider_base.hpp
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/esim_data_provider/include/esim/data_provider/data_provider_factory.hpp b/RWR/src/rpg_esim/event_camera_simulator/esim_data_provider/include/esim/data_provider/data_provider_factory.hpp
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/esim_data_provider/include/esim/data_provider/data_provider_factory.hpp
rename to RWR/src/rpg_esim/event_camera_simulator/esim_data_provider/include/esim/data_provider/data_provider_factory.hpp
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/esim_data_provider/include/esim/data_provider/data_provider_from_folder.hpp b/RWR/src/rpg_esim/event_camera_simulator/esim_data_provider/include/esim/data_provider/data_provider_from_folder.hpp
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/esim_data_provider/include/esim/data_provider/data_provider_from_folder.hpp
rename to RWR/src/rpg_esim/event_camera_simulator/esim_data_provider/include/esim/data_provider/data_provider_from_folder.hpp
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/esim_data_provider/include/esim/data_provider/data_provider_online_render.hpp b/RWR/src/rpg_esim/event_camera_simulator/esim_data_provider/include/esim/data_provider/data_provider_online_render.hpp
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/esim_data_provider/include/esim/data_provider/data_provider_online_render.hpp
rename to RWR/src/rpg_esim/event_camera_simulator/esim_data_provider/include/esim/data_provider/data_provider_online_render.hpp
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/esim_data_provider/include/esim/data_provider/data_provider_online_simple.hpp b/RWR/src/rpg_esim/event_camera_simulator/esim_data_provider/include/esim/data_provider/data_provider_online_simple.hpp
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/esim_data_provider/include/esim/data_provider/data_provider_online_simple.hpp
rename to RWR/src/rpg_esim/event_camera_simulator/esim_data_provider/include/esim/data_provider/data_provider_online_simple.hpp
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/esim_data_provider/include/esim/data_provider/data_provider_rosbag.hpp b/RWR/src/rpg_esim/event_camera_simulator/esim_data_provider/include/esim/data_provider/data_provider_rosbag.hpp
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/esim_data_provider/include/esim/data_provider/data_provider_rosbag.hpp
rename to RWR/src/rpg_esim/event_camera_simulator/esim_data_provider/include/esim/data_provider/data_provider_rosbag.hpp
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/esim_data_provider/include/esim/data_provider/renderer_factory.hpp b/RWR/src/rpg_esim/event_camera_simulator/esim_data_provider/include/esim/data_provider/renderer_factory.hpp
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/esim_data_provider/include/esim/data_provider/renderer_factory.hpp
rename to RWR/src/rpg_esim/event_camera_simulator/esim_data_provider/include/esim/data_provider/renderer_factory.hpp
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/esim_data_provider/package.xml b/RWR/src/rpg_esim/event_camera_simulator/esim_data_provider/package.xml
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/esim_data_provider/package.xml
rename to RWR/src/rpg_esim/event_camera_simulator/esim_data_provider/package.xml
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/esim_data_provider/src/data_provider_base.cpp b/RWR/src/rpg_esim/event_camera_simulator/esim_data_provider/src/data_provider_base.cpp
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/esim_data_provider/src/data_provider_base.cpp
rename to RWR/src/rpg_esim/event_camera_simulator/esim_data_provider/src/data_provider_base.cpp
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/esim_data_provider/src/data_provider_factory.cpp b/RWR/src/rpg_esim/event_camera_simulator/esim_data_provider/src/data_provider_factory.cpp
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/esim_data_provider/src/data_provider_factory.cpp
rename to RWR/src/rpg_esim/event_camera_simulator/esim_data_provider/src/data_provider_factory.cpp
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/esim_data_provider/src/data_provider_from_folder.cpp b/RWR/src/rpg_esim/event_camera_simulator/esim_data_provider/src/data_provider_from_folder.cpp
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/esim_data_provider/src/data_provider_from_folder.cpp
rename to RWR/src/rpg_esim/event_camera_simulator/esim_data_provider/src/data_provider_from_folder.cpp
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/esim_data_provider/src/data_provider_online_render.cpp b/RWR/src/rpg_esim/event_camera_simulator/esim_data_provider/src/data_provider_online_render.cpp
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/esim_data_provider/src/data_provider_online_render.cpp
rename to RWR/src/rpg_esim/event_camera_simulator/esim_data_provider/src/data_provider_online_render.cpp
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/esim_data_provider/src/data_provider_online_simple.cpp b/RWR/src/rpg_esim/event_camera_simulator/esim_data_provider/src/data_provider_online_simple.cpp
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/esim_data_provider/src/data_provider_online_simple.cpp
rename to RWR/src/rpg_esim/event_camera_simulator/esim_data_provider/src/data_provider_online_simple.cpp
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/esim_data_provider/src/data_provider_rosbag.cpp b/RWR/src/rpg_esim/event_camera_simulator/esim_data_provider/src/data_provider_rosbag.cpp
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/esim_data_provider/src/data_provider_rosbag.cpp
rename to RWR/src/rpg_esim/event_camera_simulator/esim_data_provider/src/data_provider_rosbag.cpp
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/esim_data_provider/src/renderer_factory.cpp b/RWR/src/rpg_esim/event_camera_simulator/esim_data_provider/src/renderer_factory.cpp
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/esim_data_provider/src/renderer_factory.cpp
rename to RWR/src/rpg_esim/event_camera_simulator/esim_data_provider/src/renderer_factory.cpp
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/esim_msgs/CMakeLists.txt b/RWR/src/rpg_esim/event_camera_simulator/esim_msgs/CMakeLists.txt
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/esim_msgs/CMakeLists.txt
rename to RWR/src/rpg_esim/event_camera_simulator/esim_msgs/CMakeLists.txt
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/esim_msgs/msg/OpticFlow.msg b/RWR/src/rpg_esim/event_camera_simulator/esim_msgs/msg/OpticFlow.msg
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/esim_msgs/msg/OpticFlow.msg
rename to RWR/src/rpg_esim/event_camera_simulator/esim_msgs/msg/OpticFlow.msg
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/esim_msgs/package.xml b/RWR/src/rpg_esim/event_camera_simulator/esim_msgs/package.xml
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/esim_msgs/package.xml
rename to RWR/src/rpg_esim/event_camera_simulator/esim_msgs/package.xml
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/esim_rendering/CMakeLists.txt b/RWR/src/rpg_esim/event_camera_simulator/esim_rendering/CMakeLists.txt
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/esim_rendering/CMakeLists.txt
rename to RWR/src/rpg_esim/event_camera_simulator/esim_rendering/CMakeLists.txt
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/esim_rendering/include/esim/rendering/renderer_base.hpp b/RWR/src/rpg_esim/event_camera_simulator/esim_rendering/include/esim/rendering/renderer_base.hpp
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/esim_rendering/include/esim/rendering/renderer_base.hpp
rename to RWR/src/rpg_esim/event_camera_simulator/esim_rendering/include/esim/rendering/renderer_base.hpp
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/esim_rendering/include/esim/rendering/simple_renderer_base.hpp b/RWR/src/rpg_esim/event_camera_simulator/esim_rendering/include/esim/rendering/simple_renderer_base.hpp
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/esim_rendering/include/esim/rendering/simple_renderer_base.hpp
rename to RWR/src/rpg_esim/event_camera_simulator/esim_rendering/include/esim/rendering/simple_renderer_base.hpp
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/esim_rendering/package.xml b/RWR/src/rpg_esim/event_camera_simulator/esim_rendering/package.xml
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/esim_rendering/package.xml
rename to RWR/src/rpg_esim/event_camera_simulator/esim_rendering/package.xml
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/esim_rendering/src/renderer_base.cpp b/RWR/src/rpg_esim/event_camera_simulator/esim_rendering/src/renderer_base.cpp
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/esim_rendering/src/renderer_base.cpp
rename to RWR/src/rpg_esim/event_camera_simulator/esim_rendering/src/renderer_base.cpp
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/esim_rendering/src/simple_renderer_base.cpp b/RWR/src/rpg_esim/event_camera_simulator/esim_rendering/src/simple_renderer_base.cpp
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/esim_rendering/src/simple_renderer_base.cpp
rename to RWR/src/rpg_esim/event_camera_simulator/esim_rendering/src/simple_renderer_base.cpp
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/esim_ros/CMakeLists.txt b/RWR/src/rpg_esim/event_camera_simulator/esim_ros/CMakeLists.txt
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/esim_ros/CMakeLists.txt
rename to RWR/src/rpg_esim/event_camera_simulator/esim_ros/CMakeLists.txt
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/esim_ros/cfg/calib/example.conf b/RWR/src/rpg_esim/event_camera_simulator/esim_ros/cfg/calib/example.conf
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/esim_ros/cfg/calib/example.conf
rename to RWR/src/rpg_esim/event_camera_simulator/esim_ros/cfg/calib/example.conf
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/esim_ros/cfg/calib/ijrr.yaml b/RWR/src/rpg_esim/event_camera_simulator/esim_ros/cfg/calib/ijrr.yaml
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/esim_ros/cfg/calib/ijrr.yaml
rename to RWR/src/rpg_esim/event_camera_simulator/esim_ros/cfg/calib/ijrr.yaml
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/esim_ros/cfg/calib/pinhole_mono.yaml b/RWR/src/rpg_esim/event_camera_simulator/esim_ros/cfg/calib/pinhole_mono.yaml
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/esim_ros/cfg/calib/pinhole_mono.yaml
rename to RWR/src/rpg_esim/event_camera_simulator/esim_ros/cfg/calib/pinhole_mono.yaml
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/esim_ros/cfg/calib/pinhole_mono_nodistort.yaml b/RWR/src/rpg_esim/event_camera_simulator/esim_ros/cfg/calib/pinhole_mono_nodistort.yaml
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/esim_ros/cfg/calib/pinhole_mono_nodistort.yaml
rename to RWR/src/rpg_esim/event_camera_simulator/esim_ros/cfg/calib/pinhole_mono_nodistort.yaml
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/esim_ros/cfg/calib/pinhole_mono_nodistort_forward.yaml b/RWR/src/rpg_esim/event_camera_simulator/esim_ros/cfg/calib/pinhole_mono_nodistort_forward.yaml
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/esim_ros/cfg/calib/pinhole_mono_nodistort_forward.yaml
rename to RWR/src/rpg_esim/event_camera_simulator/esim_ros/cfg/calib/pinhole_mono_nodistort_forward.yaml
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/esim_ros/cfg/calib/pinhole_wide_fov.yaml b/RWR/src/rpg_esim/event_camera_simulator/esim_ros/cfg/calib/pinhole_wide_fov.yaml
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/esim_ros/cfg/calib/pinhole_wide_fov.yaml
rename to RWR/src/rpg_esim/event_camera_simulator/esim_ros/cfg/calib/pinhole_wide_fov.yaml
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/esim_ros/cfg/calib/quadrotor.yaml b/RWR/src/rpg_esim/event_camera_simulator/esim_ros/cfg/calib/quadrotor.yaml
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/esim_ros/cfg/calib/quadrotor.yaml
rename to RWR/src/rpg_esim/event_camera_simulator/esim_ros/cfg/calib/quadrotor.yaml
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/esim_ros/cfg/example.conf b/RWR/src/rpg_esim/event_camera_simulator/esim_ros/cfg/example.conf
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/esim_ros/cfg/example.conf
rename to RWR/src/rpg_esim/event_camera_simulator/esim_ros/cfg/example.conf
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/esim_ros/cfg/multi_objects.conf b/RWR/src/rpg_esim/event_camera_simulator/esim_ros/cfg/multi_objects.conf
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/esim_ros/cfg/multi_objects.conf
rename to RWR/src/rpg_esim/event_camera_simulator/esim_ros/cfg/multi_objects.conf
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/esim_ros/cfg/obstacle.conf b/RWR/src/rpg_esim/event_camera_simulator/esim_ros/cfg/obstacle.conf
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/esim_ros/cfg/obstacle.conf
rename to RWR/src/rpg_esim/event_camera_simulator/esim_ros/cfg/obstacle.conf
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/esim_ros/cfg/opengl.conf b/RWR/src/rpg_esim/event_camera_simulator/esim_ros/cfg/opengl.conf
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/esim_ros/cfg/opengl.conf
rename to RWR/src/rpg_esim/event_camera_simulator/esim_ros/cfg/opengl.conf
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/esim_ros/cfg/panorama.conf b/RWR/src/rpg_esim/event_camera_simulator/esim_ros/cfg/panorama.conf
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/esim_ros/cfg/panorama.conf
rename to RWR/src/rpg_esim/event_camera_simulator/esim_ros/cfg/panorama.conf
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/esim_ros/cfg/sponza.conf b/RWR/src/rpg_esim/event_camera_simulator/esim_ros/cfg/sponza.conf
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/esim_ros/cfg/sponza.conf
rename to RWR/src/rpg_esim/event_camera_simulator/esim_ros/cfg/sponza.conf
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/esim_ros/cfg/traj/poster_6dof.csv b/RWR/src/rpg_esim/event_camera_simulator/esim_ros/cfg/traj/poster_6dof.csv
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/esim_ros/cfg/traj/poster_6dof.csv
rename to RWR/src/rpg_esim/event_camera_simulator/esim_ros/cfg/traj/poster_6dof.csv
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/esim_ros/cfg/traj/quadrotor_circles.csv b/RWR/src/rpg_esim/event_camera_simulator/esim_ros/cfg/traj/quadrotor_circles.csv
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/esim_ros/cfg/traj/quadrotor_circles.csv
rename to RWR/src/rpg_esim/event_camera_simulator/esim_ros/cfg/traj/quadrotor_circles.csv
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/esim_ros/cfg/unrealcv.conf b/RWR/src/rpg_esim/event_camera_simulator/esim_ros/cfg/unrealcv.conf
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/esim_ros/cfg/unrealcv.conf
rename to RWR/src/rpg_esim/event_camera_simulator/esim_ros/cfg/unrealcv.conf
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/esim_ros/launch/esim.launch b/RWR/src/rpg_esim/event_camera_simulator/esim_ros/launch/esim.launch
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/esim_ros/launch/esim.launch
rename to RWR/src/rpg_esim/event_camera_simulator/esim_ros/launch/esim.launch
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/esim_ros/launch/visualization.launch b/RWR/src/rpg_esim/event_camera_simulator/esim_ros/launch/visualization.launch
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/esim_ros/launch/visualization.launch
rename to RWR/src/rpg_esim/event_camera_simulator/esim_ros/launch/visualization.launch
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/esim_ros/package.xml b/RWR/src/rpg_esim/event_camera_simulator/esim_ros/package.xml
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/esim_ros/package.xml
rename to RWR/src/rpg_esim/event_camera_simulator/esim_ros/package.xml
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/esim_ros/scripts/generate_stamps_file.py b/RWR/src/rpg_esim/event_camera_simulator/esim_ros/scripts/generate_stamps_file.py
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/esim_ros/scripts/generate_stamps_file.py
rename to RWR/src/rpg_esim/event_camera_simulator/esim_ros/scripts/generate_stamps_file.py
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/esim_ros/src/esim_node.cpp b/RWR/src/rpg_esim/event_camera_simulator/esim_ros/src/esim_node.cpp
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/esim_ros/src/esim_node.cpp
rename to RWR/src/rpg_esim/event_camera_simulator/esim_ros/src/esim_node.cpp
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/esim_test_data/data/camera_calibs/pinhole_mono.yaml b/RWR/src/rpg_esim/event_camera_simulator/esim_test_data/data/camera_calibs/pinhole_mono.yaml
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/esim_test_data/data/camera_calibs/pinhole_mono.yaml
rename to RWR/src/rpg_esim/event_camera_simulator/esim_test_data/data/camera_calibs/pinhole_mono.yaml
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/esim_trajectory/CMakeLists.txt b/RWR/src/rpg_esim/event_camera_simulator/esim_trajectory/CMakeLists.txt
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/esim_trajectory/CMakeLists.txt
rename to RWR/src/rpg_esim/event_camera_simulator/esim_trajectory/CMakeLists.txt
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/esim_trajectory/include/esim/trajectory/imu_factory.hpp b/RWR/src/rpg_esim/event_camera_simulator/esim_trajectory/include/esim/trajectory/imu_factory.hpp
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/esim_trajectory/include/esim/trajectory/imu_factory.hpp
rename to RWR/src/rpg_esim/event_camera_simulator/esim_trajectory/include/esim/trajectory/imu_factory.hpp
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/esim_trajectory/include/esim/trajectory/trajectory_factory.hpp b/RWR/src/rpg_esim/event_camera_simulator/esim_trajectory/include/esim/trajectory/trajectory_factory.hpp
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/esim_trajectory/include/esim/trajectory/trajectory_factory.hpp
rename to RWR/src/rpg_esim/event_camera_simulator/esim_trajectory/include/esim/trajectory/trajectory_factory.hpp
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/esim_trajectory/package.xml b/RWR/src/rpg_esim/event_camera_simulator/esim_trajectory/package.xml
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/esim_trajectory/package.xml
rename to RWR/src/rpg_esim/event_camera_simulator/esim_trajectory/package.xml
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/esim_trajectory/src/imu_factory.cpp b/RWR/src/rpg_esim/event_camera_simulator/esim_trajectory/src/imu_factory.cpp
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/esim_trajectory/src/imu_factory.cpp
rename to RWR/src/rpg_esim/event_camera_simulator/esim_trajectory/src/imu_factory.cpp
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/esim_trajectory/src/trajectory_factory.cpp b/RWR/src/rpg_esim/event_camera_simulator/esim_trajectory/src/trajectory_factory.cpp
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/esim_trajectory/src/trajectory_factory.cpp
rename to RWR/src/rpg_esim/event_camera_simulator/esim_trajectory/src/trajectory_factory.cpp
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/esim_unrealcv_bridge/CMakeLists.txt b/RWR/src/rpg_esim/event_camera_simulator/esim_unrealcv_bridge/CMakeLists.txt
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/esim_unrealcv_bridge/CMakeLists.txt
rename to RWR/src/rpg_esim/event_camera_simulator/esim_unrealcv_bridge/CMakeLists.txt
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/esim_unrealcv_bridge/include/esim/unrealcv_bridge/unrealcv_bridge.hpp b/RWR/src/rpg_esim/event_camera_simulator/esim_unrealcv_bridge/include/esim/unrealcv_bridge/unrealcv_bridge.hpp
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/esim_unrealcv_bridge/include/esim/unrealcv_bridge/unrealcv_bridge.hpp
rename to RWR/src/rpg_esim/event_camera_simulator/esim_unrealcv_bridge/include/esim/unrealcv_bridge/unrealcv_bridge.hpp
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/esim_unrealcv_bridge/package.xml b/RWR/src/rpg_esim/event_camera_simulator/esim_unrealcv_bridge/package.xml
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/esim_unrealcv_bridge/package.xml
rename to RWR/src/rpg_esim/event_camera_simulator/esim_unrealcv_bridge/package.xml
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/esim_unrealcv_bridge/src/unrealcv_bridge.cpp b/RWR/src/rpg_esim/event_camera_simulator/esim_unrealcv_bridge/src/unrealcv_bridge.cpp
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/esim_unrealcv_bridge/src/unrealcv_bridge.cpp
rename to RWR/src/rpg_esim/event_camera_simulator/esim_unrealcv_bridge/src/unrealcv_bridge.cpp
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/esim_unrealcv_bridge/test/test_unrealcv_bridge.cpp b/RWR/src/rpg_esim/event_camera_simulator/esim_unrealcv_bridge/test/test_unrealcv_bridge.cpp
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/esim_unrealcv_bridge/test/test_unrealcv_bridge.cpp
rename to RWR/src/rpg_esim/event_camera_simulator/esim_unrealcv_bridge/test/test_unrealcv_bridge.cpp
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/esim_visualization/CMakeLists.txt b/RWR/src/rpg_esim/event_camera_simulator/esim_visualization/CMakeLists.txt
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/esim_visualization/CMakeLists.txt
rename to RWR/src/rpg_esim/event_camera_simulator/esim_visualization/CMakeLists.txt
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/esim_visualization/cfg/esim.perspective b/RWR/src/rpg_esim/event_camera_simulator/esim_visualization/cfg/esim.perspective
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/esim_visualization/cfg/esim.perspective
rename to RWR/src/rpg_esim/event_camera_simulator/esim_visualization/cfg/esim.perspective
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/esim_visualization/cfg/esim.rviz b/RWR/src/rpg_esim/event_camera_simulator/esim_visualization/cfg/esim.rviz
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/esim_visualization/cfg/esim.rviz
rename to RWR/src/rpg_esim/event_camera_simulator/esim_visualization/cfg/esim.rviz
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/esim_visualization/include/esim/visualization/adaptive_sampling_benchmark_publisher.hpp b/RWR/src/rpg_esim/event_camera_simulator/esim_visualization/include/esim/visualization/adaptive_sampling_benchmark_publisher.hpp
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/esim_visualization/include/esim/visualization/adaptive_sampling_benchmark_publisher.hpp
rename to RWR/src/rpg_esim/event_camera_simulator/esim_visualization/include/esim/visualization/adaptive_sampling_benchmark_publisher.hpp
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/esim_visualization/include/esim/visualization/publisher_interface.hpp b/RWR/src/rpg_esim/event_camera_simulator/esim_visualization/include/esim/visualization/publisher_interface.hpp
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/esim_visualization/include/esim/visualization/publisher_interface.hpp
rename to RWR/src/rpg_esim/event_camera_simulator/esim_visualization/include/esim/visualization/publisher_interface.hpp
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/esim_visualization/include/esim/visualization/ros_publisher.hpp b/RWR/src/rpg_esim/event_camera_simulator/esim_visualization/include/esim/visualization/ros_publisher.hpp
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/esim_visualization/include/esim/visualization/ros_publisher.hpp
rename to RWR/src/rpg_esim/event_camera_simulator/esim_visualization/include/esim/visualization/ros_publisher.hpp
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/esim_visualization/include/esim/visualization/ros_utils.hpp b/RWR/src/rpg_esim/event_camera_simulator/esim_visualization/include/esim/visualization/ros_utils.hpp
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/esim_visualization/include/esim/visualization/ros_utils.hpp
rename to RWR/src/rpg_esim/event_camera_simulator/esim_visualization/include/esim/visualization/ros_utils.hpp
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/esim_visualization/include/esim/visualization/rosbag_writer.hpp b/RWR/src/rpg_esim/event_camera_simulator/esim_visualization/include/esim/visualization/rosbag_writer.hpp
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/esim_visualization/include/esim/visualization/rosbag_writer.hpp
rename to RWR/src/rpg_esim/event_camera_simulator/esim_visualization/include/esim/visualization/rosbag_writer.hpp
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/esim_visualization/include/esim/visualization/synthetic_optic_flow_publisher.hpp b/RWR/src/rpg_esim/event_camera_simulator/esim_visualization/include/esim/visualization/synthetic_optic_flow_publisher.hpp
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/esim_visualization/include/esim/visualization/synthetic_optic_flow_publisher.hpp
rename to RWR/src/rpg_esim/event_camera_simulator/esim_visualization/include/esim/visualization/synthetic_optic_flow_publisher.hpp
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/esim_visualization/package.xml b/RWR/src/rpg_esim/event_camera_simulator/esim_visualization/package.xml
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/esim_visualization/package.xml
rename to RWR/src/rpg_esim/event_camera_simulator/esim_visualization/package.xml
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/esim_visualization/setup.py b/RWR/src/rpg_esim/event_camera_simulator/esim_visualization/setup.py
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/esim_visualization/setup.py
rename to RWR/src/rpg_esim/event_camera_simulator/esim_visualization/setup.py
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/esim_visualization/src/adaptive_sampling_benchmark_publisher.cpp b/RWR/src/rpg_esim/event_camera_simulator/esim_visualization/src/adaptive_sampling_benchmark_publisher.cpp
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/esim_visualization/src/adaptive_sampling_benchmark_publisher.cpp
rename to RWR/src/rpg_esim/event_camera_simulator/esim_visualization/src/adaptive_sampling_benchmark_publisher.cpp
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/lcd_monitor/src/lcd_monitor/__init__.py b/RWR/src/rpg_esim/event_camera_simulator/esim_visualization/src/py/__init__.py
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/lcd_monitor/src/lcd_monitor/__init__.py
rename to RWR/src/rpg_esim/event_camera_simulator/esim_visualization/src/py/__init__.py
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/esim_visualization/src/py/optic_flow_converter.py b/RWR/src/rpg_esim/event_camera_simulator/esim_visualization/src/py/optic_flow_converter.py
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/esim_visualization/src/py/optic_flow_converter.py
rename to RWR/src/rpg_esim/event_camera_simulator/esim_visualization/src/py/optic_flow_converter.py
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/esim_visualization/src/ros_publisher.cpp b/RWR/src/rpg_esim/event_camera_simulator/esim_visualization/src/ros_publisher.cpp
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/esim_visualization/src/ros_publisher.cpp
rename to RWR/src/rpg_esim/event_camera_simulator/esim_visualization/src/ros_publisher.cpp
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/esim_visualization/src/ros_utils.cpp b/RWR/src/rpg_esim/event_camera_simulator/esim_visualization/src/ros_utils.cpp
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/esim_visualization/src/ros_utils.cpp
rename to RWR/src/rpg_esim/event_camera_simulator/esim_visualization/src/ros_utils.cpp
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/esim_visualization/src/rosbag_writer.cpp b/RWR/src/rpg_esim/event_camera_simulator/esim_visualization/src/rosbag_writer.cpp
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/esim_visualization/src/rosbag_writer.cpp
rename to RWR/src/rpg_esim/event_camera_simulator/esim_visualization/src/rosbag_writer.cpp
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/esim_visualization/src/synthetic_optic_flow_publisher.cpp b/RWR/src/rpg_esim/event_camera_simulator/esim_visualization/src/synthetic_optic_flow_publisher.cpp
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/esim_visualization/src/synthetic_optic_flow_publisher.cpp
rename to RWR/src/rpg_esim/event_camera_simulator/esim_visualization/src/synthetic_optic_flow_publisher.cpp
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/img/example_video_to_events.gif b/RWR/src/rpg_esim/event_camera_simulator/img/example_video_to_events.gif
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/img/example_video_to_events.gif
rename to RWR/src/rpg_esim/event_camera_simulator/img/example_video_to_events.gif
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/img/overview_video_to_events.png b/RWR/src/rpg_esim/event_camera_simulator/img/overview_video_to_events.png
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/img/overview_video_to_events.png
rename to RWR/src/rpg_esim/event_camera_simulator/img/overview_video_to_events.png
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_multi_objects_2d/CMakeLists.txt b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_multi_objects_2d/CMakeLists.txt
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_multi_objects_2d/CMakeLists.txt
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_multi_objects_2d/CMakeLists.txt
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_multi_objects_2d/include/esim/imp_multi_objects_2d/imp_multi_objects_2d_renderer.hpp b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_multi_objects_2d/include/esim/imp_multi_objects_2d/imp_multi_objects_2d_renderer.hpp
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_multi_objects_2d/include/esim/imp_multi_objects_2d/imp_multi_objects_2d_renderer.hpp
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_multi_objects_2d/include/esim/imp_multi_objects_2d/imp_multi_objects_2d_renderer.hpp
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_multi_objects_2d/include/esim/imp_multi_objects_2d/object.hpp b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_multi_objects_2d/include/esim/imp_multi_objects_2d/object.hpp
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_multi_objects_2d/include/esim/imp_multi_objects_2d/object.hpp
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_multi_objects_2d/include/esim/imp_multi_objects_2d/object.hpp
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_multi_objects_2d/package.xml b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_multi_objects_2d/package.xml
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_multi_objects_2d/package.xml
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_multi_objects_2d/package.xml
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_multi_objects_2d/scenes/example.scene b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_multi_objects_2d/scenes/example.scene
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_multi_objects_2d/scenes/example.scene
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_multi_objects_2d/scenes/example.scene
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_multi_objects_2d/src/imp_multi_objects_2d_renderer.cpp b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_multi_objects_2d/src/imp_multi_objects_2d_renderer.cpp
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_multi_objects_2d/src/imp_multi_objects_2d_renderer.cpp
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_multi_objects_2d/src/imp_multi_objects_2d_renderer.cpp
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_multi_objects_2d/src/object.cpp b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_multi_objects_2d/src/object.cpp
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_multi_objects_2d/src/object.cpp
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_multi_objects_2d/src/object.cpp
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_multi_objects_2d/textures/chair.png b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_multi_objects_2d/textures/chair.png
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_multi_objects_2d/textures/chair.png
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_multi_objects_2d/textures/chair.png
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_multi_objects_2d/textures/chair2.png b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_multi_objects_2d/textures/chair2.png
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_multi_objects_2d/textures/chair2.png
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_multi_objects_2d/textures/chair2.png
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_multi_objects_2d/textures/chair3.png b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_multi_objects_2d/textures/chair3.png
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_multi_objects_2d/textures/chair3.png
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_multi_objects_2d/textures/chair3.png
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_multi_objects_2d/textures/rugby_ball.png b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_multi_objects_2d/textures/rugby_ball.png
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_multi_objects_2d/textures/rugby_ball.png
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_multi_objects_2d/textures/rugby_ball.png
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/CMakeLists.txt b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/CMakeLists.txt
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/CMakeLists.txt
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/CMakeLists.txt
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/configuration/root_directory.h.in b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/configuration/root_directory.h.in
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/configuration/root_directory.h.in
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/configuration/root_directory.h.in
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/include/esim/imp_opengl_renderer/opengl_renderer.hpp b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/include/esim/imp_opengl_renderer/opengl_renderer.hpp
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/include/esim/imp_opengl_renderer/opengl_renderer.hpp
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/include/esim/imp_opengl_renderer/opengl_renderer.hpp
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/include/learnopengl/camera.h b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/include/learnopengl/camera.h
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/include/learnopengl/camera.h
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/include/learnopengl/camera.h
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/include/learnopengl/filesystem.h b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/include/learnopengl/filesystem.h
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/include/learnopengl/filesystem.h
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/include/learnopengl/filesystem.h
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/include/learnopengl/mesh.h b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/include/learnopengl/mesh.h
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/include/learnopengl/mesh.h
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/include/learnopengl/mesh.h
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/include/learnopengl/model.h b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/include/learnopengl/model.h
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/include/learnopengl/model.h
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/include/learnopengl/model.h
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/include/learnopengl/shader.h b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/include/learnopengl/shader.h
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/include/learnopengl/shader.h
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/include/learnopengl/shader.h
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/include/learnopengl/shader_m.h b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/include/learnopengl/shader_m.h
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/include/learnopengl/shader_m.h
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/include/learnopengl/shader_m.h
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/include/learnopengl/shader_s.h b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/include/learnopengl/shader_s.h
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/include/learnopengl/shader_s.h
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/include/learnopengl/shader_s.h
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/libraries/glad/include/KHR/khrplatform.h b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/libraries/glad/include/KHR/khrplatform.h
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/libraries/glad/include/KHR/khrplatform.h
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/libraries/glad/include/KHR/khrplatform.h
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/libraries/glad/include/glad/glad.h b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/libraries/glad/include/glad/glad.h
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/libraries/glad/include/glad/glad.h
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/libraries/glad/include/glad/glad.h
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/libraries/glad/src/glad.c b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/libraries/glad/src/glad.c
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/libraries/glad/src/glad.c
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/libraries/glad/src/glad.c
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/package.xml b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/package.xml
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/package.xml
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/package.xml
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/dynamic_objects/BumpMap.png b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/dynamic_objects/BumpMap.png
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/dynamic_objects/BumpMap.png
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/dynamic_objects/BumpMap.png
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/dynamic_objects/ColorMap.png b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/dynamic_objects/ColorMap.png
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/dynamic_objects/ColorMap.png
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/dynamic_objects/ColorMap.png
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/dynamic_objects/LEATHBUMP.jpg b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/dynamic_objects/LEATHBUMP.jpg
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/dynamic_objects/LEATHBUMP.jpg
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/dynamic_objects/LEATHBUMP.jpg
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/dynamic_objects/NormalMap.png b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/dynamic_objects/NormalMap.png
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/dynamic_objects/NormalMap.png
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/dynamic_objects/NormalMap.png
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/dynamic_objects/americanfootball.csv b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/dynamic_objects/americanfootball.csv
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/dynamic_objects/americanfootball.csv
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/dynamic_objects/americanfootball.csv
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/dynamic_objects/americanfootball.mtl b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/dynamic_objects/americanfootball.mtl
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/dynamic_objects/americanfootball.mtl
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/dynamic_objects/americanfootball.mtl
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/dynamic_objects/americanfootball.obj b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/dynamic_objects/americanfootball.obj
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/dynamic_objects/americanfootball.obj
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/dynamic_objects/americanfootball.obj
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/dynamic_objects/soccerball.mtl b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/dynamic_objects/soccerball.mtl
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/dynamic_objects/soccerball.mtl
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/dynamic_objects/soccerball.mtl
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/dynamic_objects/soccerball.obj b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/dynamic_objects/soccerball.obj
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/dynamic_objects/soccerball.obj
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/dynamic_objects/soccerball.obj
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/flying_room.blend b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/flying_room.blend
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/flying_room.blend
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/flying_room.blend
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/flying_room/camera_trajectory.csv b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/flying_room/camera_trajectory.csv
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/flying_room/camera_trajectory.csv
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/flying_room/camera_trajectory.csv
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/flying_room/carpet.jpg b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/flying_room/carpet.jpg
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/flying_room/carpet.jpg
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/flying_room/carpet.jpg
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/flying_room/concrete3.jpg b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/flying_room/concrete3.jpg
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/flying_room/concrete3.jpg
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/flying_room/concrete3.jpg
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/flying_room/flying_room.mtl b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/flying_room/flying_room.mtl
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/flying_room/flying_room.mtl
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/flying_room/flying_room.mtl
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/flying_room/flying_room.obj b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/flying_room/flying_room.obj
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/flying_room/flying_room.obj
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/flying_room/flying_room.obj
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/flying_room/grass.jpg b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/flying_room/grass.jpg
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/flying_room/grass.jpg
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/flying_room/grass.jpg
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/flying_room/leaves.jpg b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/flying_room/leaves.jpg
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/flying_room/leaves.jpg
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/flying_room/leaves.jpg
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/flying_room/rocks.jpg b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/flying_room/rocks.jpg
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/flying_room/rocks.jpg
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/flying_room/rocks.jpg
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/sponza/00_SKAP.JPG b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/sponza/00_SKAP.JPG
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/sponza/00_SKAP.JPG
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/sponza/00_SKAP.JPG
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/sponza/01_STUB-bump.jpg b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/sponza/01_STUB-bump.jpg
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/sponza/01_STUB-bump.jpg
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/sponza/01_STUB-bump.jpg
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/sponza/01_STUB.JPG b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/sponza/01_STUB.JPG
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/sponza/01_STUB.JPG
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/sponza/01_STUB.JPG
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/sponza/01_ST_KP.JPG b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/sponza/01_ST_KP.JPG
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/sponza/01_ST_KP.JPG
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/sponza/01_ST_KP.JPG
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/sponza/01_S_BA.JPG b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/sponza/01_S_BA.JPG
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/sponza/01_S_BA.JPG
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/sponza/01_S_BA.JPG
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/sponza/01_St_kp-bump.jpg b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/sponza/01_St_kp-bump.jpg
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/sponza/01_St_kp-bump.jpg
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/sponza/01_St_kp-bump.jpg
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/sponza/KAMEN-bump.jpg b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/sponza/KAMEN-bump.jpg
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/sponza/KAMEN-bump.jpg
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/sponza/KAMEN-bump.jpg
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/sponza/KAMEN-stup.JPG b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/sponza/KAMEN-stup.JPG
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/sponza/KAMEN-stup.JPG
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/sponza/KAMEN-stup.JPG
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/sponza/KAMEN.JPG b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/sponza/KAMEN.JPG
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/sponza/KAMEN.JPG
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/sponza/KAMEN.JPG
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/sponza/PROZOR1.JPG b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/sponza/PROZOR1.JPG
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/sponza/PROZOR1.JPG
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/sponza/PROZOR1.JPG
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/sponza/RELJEF.JPG b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/sponza/RELJEF.JPG
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/sponza/RELJEF.JPG
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/sponza/RELJEF.JPG
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/sponza/SP_LUK.JPG b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/sponza/SP_LUK.JPG
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/sponza/SP_LUK.JPG
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/sponza/SP_LUK.JPG
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/sponza/VRATA_KO.JPG b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/sponza/VRATA_KO.JPG
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/sponza/VRATA_KO.JPG
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/sponza/VRATA_KO.JPG
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/sponza/VRATA_KR.JPG b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/sponza/VRATA_KR.JPG
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/sponza/VRATA_KR.JPG
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/sponza/VRATA_KR.JPG
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/sponza/X01_ST.JPG b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/sponza/X01_ST.JPG
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/sponza/X01_ST.JPG
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/sponza/X01_ST.JPG
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/sponza/camera_intrinsics.csv b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/sponza/camera_intrinsics.csv
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/sponza/camera_intrinsics.csv
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/sponza/camera_intrinsics.csv
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/sponza/camera_trajectory.csv b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/sponza/camera_trajectory.csv
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/sponza/camera_trajectory.csv
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/sponza/camera_trajectory.csv
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/sponza/reljef-bump.jpg b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/sponza/reljef-bump.jpg
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/sponza/reljef-bump.jpg
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/sponza/reljef-bump.jpg
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/sponza/sp_luk-bump.JPG b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/sponza/sp_luk-bump.JPG
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/sponza/sp_luk-bump.JPG
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/sponza/sp_luk-bump.JPG
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/sponza/sponza.blend b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/sponza/sponza.blend
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/sponza/sponza.blend
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/sponza/sponza.blend
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/sponza/sponza.mtl b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/sponza/sponza.mtl
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/sponza/sponza.mtl
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/sponza/sponza.mtl
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/sponza/sponza.obj b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/sponza/sponza.obj
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/sponza/sponza.obj
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/resources/objects/sponza/sponza.obj
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/src/opengl_renderer.cpp b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/src/opengl_renderer.cpp
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/src/opengl_renderer.cpp
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/src/opengl_renderer.cpp
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/src/optic_flow_shader.frag b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/src/optic_flow_shader.frag
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/src/optic_flow_shader.frag
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/src/optic_flow_shader.frag
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/src/shader.frag b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/src/shader.frag
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/src/shader.frag
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/src/shader.frag
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/src/shader.vert b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/src/shader.vert
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/src/shader.vert
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_opengl_renderer/src/shader.vert
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_panorama_renderer/CMakeLists.txt b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_panorama_renderer/CMakeLists.txt
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_panorama_renderer/CMakeLists.txt
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_panorama_renderer/CMakeLists.txt
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_panorama_renderer/include/esim/imp_panorama_renderer/panorama_renderer.hpp b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_panorama_renderer/include/esim/imp_panorama_renderer/panorama_renderer.hpp
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_panorama_renderer/include/esim/imp_panorama_renderer/panorama_renderer.hpp
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_panorama_renderer/include/esim/imp_panorama_renderer/panorama_renderer.hpp
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_panorama_renderer/package.xml b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_panorama_renderer/package.xml
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_panorama_renderer/package.xml
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_panorama_renderer/package.xml
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_panorama_renderer/src/panorama_renderer.cpp b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_panorama_renderer/src/panorama_renderer.cpp
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_panorama_renderer/src/panorama_renderer.cpp
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_panorama_renderer/src/panorama_renderer.cpp
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_panorama_renderer/textures/bicycle_parking.jpg b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_panorama_renderer/textures/bicycle_parking.jpg
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_panorama_renderer/textures/bicycle_parking.jpg
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_panorama_renderer/textures/bicycle_parking.jpg
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/CMakeLists.txt b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/CMakeLists.txt
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/CMakeLists.txt
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/CMakeLists.txt
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/include/esim/imp_planar_renderer/panorama_renderer.hpp b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/include/esim/imp_planar_renderer/panorama_renderer.hpp
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/include/esim/imp_planar_renderer/panorama_renderer.hpp
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/include/esim/imp_planar_renderer/panorama_renderer.hpp
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/include/esim/imp_planar_renderer/planar_renderer.hpp b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/include/esim/imp_planar_renderer/planar_renderer.hpp
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/include/esim/imp_planar_renderer/planar_renderer.hpp
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/include/esim/imp_planar_renderer/planar_renderer.hpp
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/include/esim/imp_planar_renderer/renderer_base.hpp b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/include/esim/imp_planar_renderer/renderer_base.hpp
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/include/esim/imp_planar_renderer/renderer_base.hpp
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/include/esim/imp_planar_renderer/renderer_base.hpp
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/include/esim/imp_planar_renderer/renderer_factory.hpp b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/include/esim/imp_planar_renderer/renderer_factory.hpp
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/include/esim/imp_planar_renderer/renderer_factory.hpp
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/include/esim/imp_planar_renderer/renderer_factory.hpp
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/package.xml b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/package.xml
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/package.xml
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/package.xml
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/src/panorama_renderer.cpp b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/src/panorama_renderer.cpp
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/src/panorama_renderer.cpp
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/src/panorama_renderer.cpp
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/src/planar_renderer.cpp b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/src/planar_renderer.cpp
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/src/planar_renderer.cpp
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/src/planar_renderer.cpp
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/src/renderer_base.cpp b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/src/renderer_base.cpp
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/src/renderer_base.cpp
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/src/renderer_base.cpp
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/src/renderer_factory.cpp b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/src/renderer_factory.cpp
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/src/renderer_factory.cpp
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/src/renderer_factory.cpp
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/textures/april_tag.png b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/textures/april_tag.png
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/textures/april_tag.png
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/textures/april_tag.png
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/textures/brick1.jpg b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/textures/brick1.jpg
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/textures/brick1.jpg
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/textures/brick1.jpg
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/textures/brick2.jpg b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/textures/brick2.jpg
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/textures/brick2.jpg
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/textures/brick2.jpg
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/textures/brick3.jpg b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/textures/brick3.jpg
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/textures/brick3.jpg
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/textures/brick3.jpg
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/textures/carpet.jpg b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/textures/carpet.jpg
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/textures/carpet.jpg
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/textures/carpet.jpg
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/textures/checkerboard.png b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/textures/checkerboard.png
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/textures/checkerboard.png
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/textures/checkerboard.png
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/textures/city.jpg b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/textures/city.jpg
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/textures/city.jpg
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/textures/city.jpg
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/textures/composite.jpg b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/textures/composite.jpg
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/textures/composite.jpg
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/textures/composite.jpg
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/textures/concrete1.jpg b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/textures/concrete1.jpg
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/textures/concrete1.jpg
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/textures/concrete1.jpg
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/textures/concrete2.jpg b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/textures/concrete2.jpg
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/textures/concrete2.jpg
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/textures/concrete2.jpg
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/textures/concrete3.jpg b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/textures/concrete3.jpg
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/textures/concrete3.jpg
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/textures/concrete3.jpg
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/textures/dirt.jpg b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/textures/dirt.jpg
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/textures/dirt.jpg
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/textures/dirt.jpg
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/textures/esim.png b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/textures/esim.png
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/textures/esim.png
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/textures/esim.png
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/textures/forest.jpg b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/textures/forest.jpg
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/textures/forest.jpg
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/textures/forest.jpg
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/textures/grass.jpg b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/textures/grass.jpg
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/textures/grass.jpg
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/textures/grass.jpg
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/textures/grass1.jpg b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/textures/grass1.jpg
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/textures/grass1.jpg
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/textures/grass1.jpg
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/textures/landscape.jpg b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/textures/landscape.jpg
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/textures/landscape.jpg
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/textures/landscape.jpg
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/textures/leaves.jpg b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/textures/leaves.jpg
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/textures/leaves.jpg
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/textures/leaves.jpg
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/textures/office.jpg b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/textures/office.jpg
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/textures/office.jpg
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/textures/office.jpg
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/textures/rocks.jpg b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/textures/rocks.jpg
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/textures/rocks.jpg
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/textures/rocks.jpg
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/textures/shapes_poster.png b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/textures/shapes_poster.png
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/textures/shapes_poster.png
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/textures/shapes_poster.png
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/textures/train_tracks.jpg b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/textures/train_tracks.jpg
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/textures/train_tracks.jpg
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/textures/train_tracks.jpg
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/textures/water1.jpg b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/textures/water1.jpg
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/textures/water1.jpg
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_planar_renderer/textures/water1.jpg
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_unrealcv_renderer/.gitignore b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_unrealcv_renderer/.gitignore
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_unrealcv_renderer/.gitignore
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_unrealcv_renderer/.gitignore
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_unrealcv_renderer/CMakeLists.txt b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_unrealcv_renderer/CMakeLists.txt
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_unrealcv_renderer/CMakeLists.txt
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_unrealcv_renderer/CMakeLists.txt
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_unrealcv_renderer/include/esim/imp_unrealcv_renderer/unrealcv_renderer.hpp b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_unrealcv_renderer/include/esim/imp_unrealcv_renderer/unrealcv_renderer.hpp
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_unrealcv_renderer/include/esim/imp_unrealcv_renderer/unrealcv_renderer.hpp
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_unrealcv_renderer/include/esim/imp_unrealcv_renderer/unrealcv_renderer.hpp
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_unrealcv_renderer/include/esim/imp_unrealcv_renderer/utils.hpp b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_unrealcv_renderer/include/esim/imp_unrealcv_renderer/utils.hpp
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_unrealcv_renderer/include/esim/imp_unrealcv_renderer/utils.hpp
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_unrealcv_renderer/include/esim/imp_unrealcv_renderer/utils.hpp
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_unrealcv_renderer/package.xml b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_unrealcv_renderer/package.xml
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_unrealcv_renderer/package.xml
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_unrealcv_renderer/package.xml
diff --git a/sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_unrealcv_renderer/src/unrealcv_renderer.cpp b/RWR/src/rpg_esim/event_camera_simulator/imp/imp_unrealcv_renderer/src/unrealcv_renderer.cpp
similarity index 100%
rename from sim_ws/src/rpg_esim/event_camera_simulator/imp/imp_unrealcv_renderer/src/unrealcv_renderer.cpp
rename to RWR/src/rpg_esim/event_camera_simulator/imp/imp_unrealcv_renderer/src/unrealcv_renderer.cpp
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/servo_move_keyboard/CMakeLists.txt b/RWR/src/servo_move_keyboard/CMakeLists.txt
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/servo_move_keyboard/CMakeLists.txt
rename to RWR/src/servo_move_keyboard/CMakeLists.txt
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/servo_move_keyboard/launch/keyboard_move.launch b/RWR/src/servo_move_keyboard/launch/keyboard_move.launch
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/servo_move_keyboard/launch/keyboard_move.launch
rename to RWR/src/servo_move_keyboard/launch/keyboard_move.launch
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/servo_move_keyboard/package.xml b/RWR/src/servo_move_keyboard/package.xml
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/servo_move_keyboard/package.xml
rename to RWR/src/servo_move_keyboard/package.xml
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/servo_move_keyboard/scripts/servoConfigTest.py b/RWR/src/servo_move_keyboard/scripts/servoConfigTest.py
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/servo_move_keyboard/scripts/servoConfigTest.py
rename to RWR/src/servo_move_keyboard/scripts/servoConfigTest.py
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/servo_move_keyboard/scripts/servoMoveKeyboard.py b/RWR/src/servo_move_keyboard/scripts/servoMoveKeyboard.py
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/servo_move_keyboard/scripts/servoMoveKeyboard.py
rename to RWR/src/servo_move_keyboard/scripts/servoMoveKeyboard.py
diff --git a/sim_ws/setupeventsim.sh b/RWR/src/setupeventsim.sh
similarity index 100%
rename from sim_ws/setupeventsim.sh
rename to RWR/src/setupeventsim.sh
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_joy/CMakeLists.txt b/RWR/src/spot_micro_joy/CMakeLists.txt
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/spot_micro_joy/CMakeLists.txt
rename to RWR/src/spot_micro_joy/CMakeLists.txt
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_joy/launch/everything.launch b/RWR/src/spot_micro_joy/launch/everything.launch
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/spot_micro_joy/launch/everything.launch
rename to RWR/src/spot_micro_joy/launch/everything.launch
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_joy/package.xml b/RWR/src/spot_micro_joy/package.xml
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/spot_micro_joy/package.xml
rename to RWR/src/spot_micro_joy/package.xml
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_joy/scripts/spotMicroJoystickMove.py b/RWR/src/spot_micro_joy/scripts/spotMicroJoystickMove.py
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/spot_micro_joy/scripts/spotMicroJoystickMove.py
rename to RWR/src/spot_micro_joy/scripts/spotMicroJoystickMove.py
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_keyboard_command/CMakeLists.txt b/RWR/src/spot_micro_keyboard_command/CMakeLists.txt
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/spot_micro_keyboard_command/CMakeLists.txt
rename to RWR/src/spot_micro_keyboard_command/CMakeLists.txt
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_keyboard_command/launch/keyboard_command.launch b/RWR/src/spot_micro_keyboard_command/launch/keyboard_command.launch
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/spot_micro_keyboard_command/launch/keyboard_command.launch
rename to RWR/src/spot_micro_keyboard_command/launch/keyboard_command.launch
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_keyboard_command/objectDet.cpp b/RWR/src/spot_micro_keyboard_command/objectDet.cpp
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/spot_micro_keyboard_command/objectDet.cpp
rename to RWR/src/spot_micro_keyboard_command/objectDet.cpp
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_keyboard_command/package.xml b/RWR/src/spot_micro_keyboard_command/package.xml
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/spot_micro_keyboard_command/package.xml
rename to RWR/src/spot_micro_keyboard_command/package.xml
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_keyboard_command/scripts/postObjDetect.py b/RWR/src/spot_micro_keyboard_command/scripts/postObjDetect.py
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/spot_micro_keyboard_command/scripts/postObjDetect.py
rename to RWR/src/spot_micro_keyboard_command/scripts/postObjDetect.py
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_keyboard_command/scripts/spotMicroKeyboardMove.py b/RWR/src/spot_micro_keyboard_command/scripts/spotMicroKeyboardMove.py
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/spot_micro_keyboard_command/scripts/spotMicroKeyboardMove.py
rename to RWR/src/spot_micro_keyboard_command/scripts/spotMicroKeyboardMove.py
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_launch/CMakeLists.txt b/RWR/src/spot_micro_launch/CMakeLists.txt
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/spot_micro_launch/CMakeLists.txt
rename to RWR/src/spot_micro_launch/CMakeLists.txt
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_launch/launch/keyboard_control_and_rviz.launch b/RWR/src/spot_micro_launch/launch/keyboard_control_and_rviz.launch
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/spot_micro_launch/launch/keyboard_control_and_rviz.launch
rename to RWR/src/spot_micro_launch/launch/keyboard_control_and_rviz.launch
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_launch/launch/motion_control_and_hector_slam.launch b/RWR/src/spot_micro_launch/launch/motion_control_and_hector_slam.launch
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/spot_micro_launch/launch/motion_control_and_hector_slam.launch
rename to RWR/src/spot_micro_launch/launch/motion_control_and_hector_slam.launch
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_launch/package.xml b/RWR/src/spot_micro_launch/package.xml
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/spot_micro_launch/package.xml
rename to RWR/src/spot_micro_launch/package.xml
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/CMakeLists.txt b/RWR/src/spot_micro_motion_cmd/CMakeLists.txt
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/CMakeLists.txt
rename to RWR/src/spot_micro_motion_cmd/CMakeLists.txt
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/config/spot_micro_motion_cmd.yaml b/RWR/src/spot_micro_motion_cmd/config/spot_micro_motion_cmd.yaml
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/config/spot_micro_motion_cmd.yaml
rename to RWR/src/spot_micro_motion_cmd/config/spot_micro_motion_cmd.yaml
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/data/usage.txt b/RWR/src/spot_micro_motion_cmd/data/usage.txt
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/data/usage.txt
rename to RWR/src/spot_micro_motion_cmd/data/usage.txt
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/include/spot_micro_motion_cmd/spot_micro_motion_cmd.h b/RWR/src/spot_micro_motion_cmd/include/spot_micro_motion_cmd/spot_micro_motion_cmd.h
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/include/spot_micro_motion_cmd/spot_micro_motion_cmd.h
rename to RWR/src/spot_micro_motion_cmd/include/spot_micro_motion_cmd/spot_micro_motion_cmd.h
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/include/spot_micro_motion_cmd/utils.h b/RWR/src/spot_micro_motion_cmd/include/spot_micro_motion_cmd/utils.h
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/include/spot_micro_motion_cmd/utils.h
rename to RWR/src/spot_micro_motion_cmd/include/spot_micro_motion_cmd/utils.h
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/launch/motion_cmd.launch b/RWR/src/spot_micro_motion_cmd/launch/motion_cmd.launch
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/launch/motion_cmd.launch
rename to RWR/src/spot_micro_motion_cmd/launch/motion_cmd.launch
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/.gitignore b/RWR/src/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/.gitignore
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/.gitignore
rename to RWR/src/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/.gitignore
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/.ycm_extra_conf.py b/RWR/src/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/.ycm_extra_conf.py
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/.ycm_extra_conf.py
rename to RWR/src/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/.ycm_extra_conf.py
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/3rd_party/google-test/CMakeLists.txt b/RWR/src/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/3rd_party/google-test/CMakeLists.txt
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/3rd_party/google-test/CMakeLists.txt
rename to RWR/src/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/3rd_party/google-test/CMakeLists.txt
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/3rd_party/google-test/CMakeLists.txt.in b/RWR/src/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/3rd_party/google-test/CMakeLists.txt.in
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/3rd_party/google-test/CMakeLists.txt.in
rename to RWR/src/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/3rd_party/google-test/CMakeLists.txt.in
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/CMakeLists.txt b/RWR/src/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/CMakeLists.txt
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/CMakeLists.txt
rename to RWR/src/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/CMakeLists.txt
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/LICENSE b/RWR/src/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/LICENSE
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/LICENSE
rename to RWR/src/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/LICENSE
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/README.md b/RWR/src/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/README.md
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/README.md
rename to RWR/src/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/README.md
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/include/spot_micro_kinematics/spot_micro_kinematics.h b/RWR/src/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/include/spot_micro_kinematics/spot_micro_kinematics.h
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/include/spot_micro_kinematics/spot_micro_kinematics.h
rename to RWR/src/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/include/spot_micro_kinematics/spot_micro_kinematics.h
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/include/spot_micro_kinematics/spot_micro_leg.h b/RWR/src/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/include/spot_micro_kinematics/spot_micro_leg.h
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/include/spot_micro_kinematics/spot_micro_leg.h
rename to RWR/src/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/include/spot_micro_kinematics/spot_micro_leg.h
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/include/spot_micro_kinematics/utils.h b/RWR/src/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/include/spot_micro_kinematics/utils.h
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/include/spot_micro_kinematics/utils.h
rename to RWR/src/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/include/spot_micro_kinematics/utils.h
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/src/spot_micro_kinematics.cpp b/RWR/src/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/src/spot_micro_kinematics.cpp
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/src/spot_micro_kinematics.cpp
rename to RWR/src/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/src/spot_micro_kinematics.cpp
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/src/spot_micro_leg.cpp b/RWR/src/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/src/spot_micro_leg.cpp
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/src/spot_micro_leg.cpp
rename to RWR/src/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/src/spot_micro_leg.cpp
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/src/utils.cpp b/RWR/src/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/src/utils.cpp
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/src/utils.cpp
rename to RWR/src/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/src/utils.cpp
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/tests/test_spot_micro_kinematics.cpp b/RWR/src/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/tests/test_spot_micro_kinematics.cpp
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/tests/test_spot_micro_kinematics.cpp
rename to RWR/src/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/tests/test_spot_micro_kinematics.cpp
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/tests/test_spot_micro_leg.cpp b/RWR/src/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/tests/test_spot_micro_leg.cpp
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/tests/test_spot_micro_leg.cpp
rename to RWR/src/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/tests/test_spot_micro_leg.cpp
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/tests/test_utils.cpp b/RWR/src/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/tests/test_utils.cpp
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/tests/test_utils.cpp
rename to RWR/src/spot_micro_motion_cmd/libs/spot_micro_kinematics_cpp/tests/test_utils.cpp
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/package.xml b/RWR/src/spot_micro_motion_cmd/package.xml
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/package.xml
rename to RWR/src/spot_micro_motion_cmd/package.xml
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/src/rate_limited_first_order_filter/rate_limited_first_order_filter.h b/RWR/src/spot_micro_motion_cmd/src/rate_limited_first_order_filter/rate_limited_first_order_filter.h
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/src/rate_limited_first_order_filter/rate_limited_first_order_filter.h
rename to RWR/src/spot_micro_motion_cmd/src/rate_limited_first_order_filter/rate_limited_first_order_filter.h
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/src/smfsm/command.h b/RWR/src/spot_micro_motion_cmd/src/smfsm/command.h
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/src/smfsm/command.h
rename to RWR/src/spot_micro_motion_cmd/src/smfsm/command.h
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/src/smfsm/spot_micro_idle.cpp b/RWR/src/spot_micro_motion_cmd/src/smfsm/spot_micro_idle.cpp
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/src/smfsm/spot_micro_idle.cpp
rename to RWR/src/spot_micro_motion_cmd/src/smfsm/spot_micro_idle.cpp
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/src/smfsm/spot_micro_idle.h b/RWR/src/spot_micro_motion_cmd/src/smfsm/spot_micro_idle.h
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/src/smfsm/spot_micro_idle.h
rename to RWR/src/spot_micro_motion_cmd/src/smfsm/spot_micro_idle.h
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/src/smfsm/spot_micro_object.cpp b/RWR/src/spot_micro_motion_cmd/src/smfsm/spot_micro_object.cpp
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/src/smfsm/spot_micro_object.cpp
rename to RWR/src/spot_micro_motion_cmd/src/smfsm/spot_micro_object.cpp
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/src/smfsm/spot_micro_object.h b/RWR/src/spot_micro_motion_cmd/src/smfsm/spot_micro_object.h
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/src/smfsm/spot_micro_object.h
rename to RWR/src/spot_micro_motion_cmd/src/smfsm/spot_micro_object.h
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/src/smfsm/spot_micro_stand.cpp b/RWR/src/spot_micro_motion_cmd/src/smfsm/spot_micro_stand.cpp
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/src/smfsm/spot_micro_stand.cpp
rename to RWR/src/spot_micro_motion_cmd/src/smfsm/spot_micro_stand.cpp
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/src/smfsm/spot_micro_stand.h b/RWR/src/spot_micro_motion_cmd/src/smfsm/spot_micro_stand.h
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/src/smfsm/spot_micro_stand.h
rename to RWR/src/spot_micro_motion_cmd/src/smfsm/spot_micro_stand.h
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/src/smfsm/spot_micro_state.cpp b/RWR/src/spot_micro_motion_cmd/src/smfsm/spot_micro_state.cpp
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/src/smfsm/spot_micro_state.cpp
rename to RWR/src/spot_micro_motion_cmd/src/smfsm/spot_micro_state.cpp
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/src/smfsm/spot_micro_state.h b/RWR/src/spot_micro_motion_cmd/src/smfsm/spot_micro_state.h
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/src/smfsm/spot_micro_state.h
rename to RWR/src/spot_micro_motion_cmd/src/smfsm/spot_micro_state.h
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/src/smfsm/spot_micro_transition_idle.cpp b/RWR/src/spot_micro_motion_cmd/src/smfsm/spot_micro_transition_idle.cpp
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/src/smfsm/spot_micro_transition_idle.cpp
rename to RWR/src/spot_micro_motion_cmd/src/smfsm/spot_micro_transition_idle.cpp
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/src/smfsm/spot_micro_transition_idle.h b/RWR/src/spot_micro_motion_cmd/src/smfsm/spot_micro_transition_idle.h
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/src/smfsm/spot_micro_transition_idle.h
rename to RWR/src/spot_micro_motion_cmd/src/smfsm/spot_micro_transition_idle.h
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/src/smfsm/spot_micro_transition_stand.cpp b/RWR/src/spot_micro_motion_cmd/src/smfsm/spot_micro_transition_stand.cpp
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/src/smfsm/spot_micro_transition_stand.cpp
rename to RWR/src/spot_micro_motion_cmd/src/smfsm/spot_micro_transition_stand.cpp
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/src/smfsm/spot_micro_transition_stand.h b/RWR/src/spot_micro_motion_cmd/src/smfsm/spot_micro_transition_stand.h
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/src/smfsm/spot_micro_transition_stand.h
rename to RWR/src/spot_micro_motion_cmd/src/smfsm/spot_micro_transition_stand.h
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/src/smfsm/spot_micro_walk.cpp b/RWR/src/spot_micro_motion_cmd/src/smfsm/spot_micro_walk.cpp
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/src/smfsm/spot_micro_walk.cpp
rename to RWR/src/spot_micro_motion_cmd/src/smfsm/spot_micro_walk.cpp
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/src/smfsm/spot_micro_walk.h b/RWR/src/spot_micro_motion_cmd/src/smfsm/spot_micro_walk.h
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/src/smfsm/spot_micro_walk.h
rename to RWR/src/spot_micro_motion_cmd/src/smfsm/spot_micro_walk.h
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/src/spot_micro_motion_cmd.cpp b/RWR/src/spot_micro_motion_cmd/src/spot_micro_motion_cmd.cpp
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/src/spot_micro_motion_cmd.cpp
rename to RWR/src/spot_micro_motion_cmd/src/spot_micro_motion_cmd.cpp
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/src/spot_micro_motion_cmd_node.cpp b/RWR/src/spot_micro_motion_cmd/src/spot_micro_motion_cmd_node.cpp
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/src/spot_micro_motion_cmd_node.cpp
rename to RWR/src/spot_micro_motion_cmd/src/spot_micro_motion_cmd_node.cpp
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/src/utils.cpp b/RWR/src/spot_micro_motion_cmd/src/utils.cpp
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/spot_micro_motion_cmd/src/utils.cpp
rename to RWR/src/spot_micro_motion_cmd/src/utils.cpp
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/CMakeLists.txt b/RWR/src/spot_micro_plot/CMakeLists.txt
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/CMakeLists.txt
rename to RWR/src/spot_micro_plot/CMakeLists.txt
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/launch/start_plotting.launch b/RWR/src/spot_micro_plot/launch/start_plotting.launch
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/launch/start_plotting.launch
rename to RWR/src/spot_micro_plot/launch/start_plotting.launch
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/package.xml b/RWR/src/spot_micro_plot/package.xml
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/package.xml
rename to RWR/src/spot_micro_plot/package.xml
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spotMicroPlot.py b/RWR/src/spot_micro_plot/scripts/spotMicroPlot.py
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spotMicroPlot.py
rename to RWR/src/spot_micro_plot/scripts/spotMicroPlot.py
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spot_micro_kinematics_python/.gitignore b/RWR/src/spot_micro_plot/scripts/spot_micro_kinematics_python/.gitignore
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spot_micro_kinematics_python/.gitignore
rename to RWR/src/spot_micro_plot/scripts/spot_micro_kinematics_python/.gitignore
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spot_micro_kinematics_python/LICENSE b/RWR/src/spot_micro_plot/scripts/spot_micro_kinematics_python/LICENSE
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spot_micro_kinematics_python/LICENSE
rename to RWR/src/spot_micro_plot/scripts/spot_micro_kinematics_python/LICENSE
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spot_micro_kinematics_python/README.md b/RWR/src/spot_micro_plot/scripts/spot_micro_kinematics_python/README.md
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spot_micro_kinematics_python/README.md
rename to RWR/src/spot_micro_plot/scripts/spot_micro_kinematics_python/README.md
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spot_micro_kinematics_python/__init__.py b/RWR/src/spot_micro_plot/scripts/spot_micro_kinematics_python/__init__.py
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spot_micro_kinematics_python/__init__.py
rename to RWR/src/spot_micro_plot/scripts/spot_micro_kinematics_python/__init__.py
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spot_micro_kinematics_python/assets/animation.gif b/RWR/src/spot_micro_plot/scripts/spot_micro_kinematics_python/assets/animation.gif
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spot_micro_kinematics_python/assets/animation.gif
rename to RWR/src/spot_micro_plot/scripts/spot_micro_kinematics_python/assets/animation.gif
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spot_micro_kinematics_python/assets/general_leg_geometry.png b/RWR/src/spot_micro_plot/scripts/spot_micro_kinematics_python/assets/general_leg_geometry.png
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spot_micro_kinematics_python/assets/general_leg_geometry.png
rename to RWR/src/spot_micro_plot/scripts/spot_micro_kinematics_python/assets/general_leg_geometry.png
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spot_micro_kinematics_python/assets/robot_geometry.png b/RWR/src/spot_micro_plot/scripts/spot_micro_kinematics_python/assets/robot_geometry.png
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spot_micro_kinematics_python/assets/robot_geometry.png
rename to RWR/src/spot_micro_plot/scripts/spot_micro_kinematics_python/assets/robot_geometry.png
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spot_micro_kinematics_python/requirements.txt b/RWR/src/spot_micro_plot/scripts/spot_micro_kinematics_python/requirements.txt
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spot_micro_kinematics_python/requirements.txt
rename to RWR/src/spot_micro_plot/scripts/spot_micro_kinematics_python/requirements.txt
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spot_micro_kinematics_python/setup.py b/RWR/src/spot_micro_plot/scripts/spot_micro_kinematics_python/setup.py
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spot_micro_kinematics_python/setup.py
rename to RWR/src/spot_micro_plot/scripts/spot_micro_kinematics_python/setup.py
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spot_micro_kinematics_python/spot_micro_stick_figure.py b/RWR/src/spot_micro_plot/scripts/spot_micro_kinematics_python/spot_micro_stick_figure.py
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spot_micro_kinematics_python/spot_micro_stick_figure.py
rename to RWR/src/spot_micro_plot/scripts/spot_micro_kinematics_python/spot_micro_stick_figure.py
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spot_micro_kinematics_python/tests/__init__.py b/RWR/src/spot_micro_plot/scripts/spot_micro_kinematics_python/tests/__init__.py
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spot_micro_kinematics_python/tests/__init__.py
rename to RWR/src/spot_micro_plot/scripts/spot_micro_kinematics_python/tests/__init__.py
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spot_micro_kinematics_python/tests/plotting/__init__.py b/RWR/src/spot_micro_plot/scripts/spot_micro_kinematics_python/tests/plotting/__init__.py
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spot_micro_kinematics_python/tests/plotting/__init__.py
rename to RWR/src/spot_micro_plot/scripts/spot_micro_kinematics_python/tests/plotting/__init__.py
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spot_micro_kinematics_python/tests/plotting/plot_animation.py b/RWR/src/spot_micro_plot/scripts/spot_micro_kinematics_python/tests/plotting/plot_animation.py
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spot_micro_kinematics_python/tests/plotting/plot_animation.py
rename to RWR/src/spot_micro_plot/scripts/spot_micro_kinematics_python/tests/plotting/plot_animation.py
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spot_micro_kinematics_python/tests/plotting/plot_body.py b/RWR/src/spot_micro_plot/scripts/spot_micro_kinematics_python/tests/plotting/plot_body.py
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spot_micro_kinematics_python/tests/plotting/plot_body.py
rename to RWR/src/spot_micro_plot/scripts/spot_micro_kinematics_python/tests/plotting/plot_body.py
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spot_micro_kinematics_python/tests/plotting/plot_single_leg.py b/RWR/src/spot_micro_plot/scripts/spot_micro_kinematics_python/tests/plotting/plot_single_leg.py
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spot_micro_kinematics_python/tests/plotting/plot_single_leg.py
rename to RWR/src/spot_micro_plot/scripts/spot_micro_kinematics_python/tests/plotting/plot_single_leg.py
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spot_micro_kinematics_python/tests/test_spot_micro_stick_figure.py b/RWR/src/spot_micro_plot/scripts/spot_micro_kinematics_python/tests/test_spot_micro_stick_figure.py
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spot_micro_kinematics_python/tests/test_spot_micro_stick_figure.py
rename to RWR/src/spot_micro_plot/scripts/spot_micro_kinematics_python/tests/test_spot_micro_stick_figure.py
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spot_micro_kinematics_python/utilities/__init__.py b/RWR/src/spot_micro_plot/scripts/spot_micro_kinematics_python/utilities/__init__.py
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spot_micro_kinematics_python/utilities/__init__.py
rename to RWR/src/spot_micro_plot/scripts/spot_micro_kinematics_python/utilities/__init__.py
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spot_micro_kinematics_python/utilities/spot_micro_kinematics.py b/RWR/src/spot_micro_plot/scripts/spot_micro_kinematics_python/utilities/spot_micro_kinematics.py
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spot_micro_kinematics_python/utilities/spot_micro_kinematics.py
rename to RWR/src/spot_micro_plot/scripts/spot_micro_kinematics_python/utilities/spot_micro_kinematics.py
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spot_micro_kinematics_python/utilities/tests/__init__.py b/RWR/src/spot_micro_plot/scripts/spot_micro_kinematics_python/utilities/tests/__init__.py
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spot_micro_kinematics_python/utilities/tests/__init__.py
rename to RWR/src/spot_micro_plot/scripts/spot_micro_kinematics_python/utilities/tests/__init__.py
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spot_micro_kinematics_python/utilities/tests/test_spot_micro_kinematics.py b/RWR/src/spot_micro_plot/scripts/spot_micro_kinematics_python/utilities/tests/test_spot_micro_kinematics.py
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spot_micro_kinematics_python/utilities/tests/test_spot_micro_kinematics.py
rename to RWR/src/spot_micro_plot/scripts/spot_micro_kinematics_python/utilities/tests/test_spot_micro_kinematics.py
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spot_micro_kinematics_python/utilities/tests/test_transformations.py b/RWR/src/spot_micro_plot/scripts/spot_micro_kinematics_python/utilities/tests/test_transformations.py
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spot_micro_kinematics_python/utilities/tests/test_transformations.py
rename to RWR/src/spot_micro_plot/scripts/spot_micro_kinematics_python/utilities/tests/test_transformations.py
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spot_micro_kinematics_python/utilities/transformations.py b/RWR/src/spot_micro_plot/scripts/spot_micro_kinematics_python/utilities/transformations.py
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/spot_micro_plot/scripts/spot_micro_kinematics_python/utilities/transformations.py
rename to RWR/src/spot_micro_plot/scripts/spot_micro_kinematics_python/utilities/transformations.py
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/CMakeLists.txt b/RWR/src/spot_micro_rviz/CMakeLists.txt
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/CMakeLists.txt
rename to RWR/src/spot_micro_rviz/CMakeLists.txt
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/launch/show_and_move_model_via_gui.launch b/RWR/src/spot_micro_rviz/launch/show_and_move_model_via_gui.launch
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/launch/show_and_move_model_via_gui.launch
rename to RWR/src/spot_micro_rviz/launch/show_and_move_model_via_gui.launch
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/launch/show_model.launch b/RWR/src/spot_micro_rviz/launch/show_model.launch
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/launch/show_model.launch
rename to RWR/src/spot_micro_rviz/launch/show_model.launch
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/launch/slam.launch b/RWR/src/spot_micro_rviz/launch/slam.launch
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/launch/slam.launch
rename to RWR/src/spot_micro_rviz/launch/slam.launch
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/package.xml b/RWR/src/spot_micro_rviz/package.xml
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/package.xml
rename to RWR/src/spot_micro_rviz/package.xml
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/rviz/mapping_demo.rviz b/RWR/src/spot_micro_rviz/rviz/mapping_demo.rviz
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/rviz/mapping_demo.rviz
rename to RWR/src/spot_micro_rviz/rviz/mapping_demo.rviz
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/rviz/spot_micro.rviz b/RWR/src/spot_micro_rviz/rviz/spot_micro.rviz
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/rviz/spot_micro.rviz
rename to RWR/src/spot_micro_rviz/rviz/spot_micro.rviz
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/rviz/spot_micro_slam.rviz b/RWR/src/spot_micro_rviz/rviz/spot_micro_slam.rviz
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/rviz/spot_micro_slam.rviz
rename to RWR/src/spot_micro_rviz/rviz/spot_micro_slam.rviz
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/urdf/gen_urdf.sh b/RWR/src/spot_micro_rviz/urdf/gen_urdf.sh
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/urdf/gen_urdf.sh
rename to RWR/src/spot_micro_rviz/urdf/gen_urdf.sh
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/urdf/spot_micro.urdf b/RWR/src/spot_micro_rviz/urdf/spot_micro.urdf
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/urdf/spot_micro.urdf
rename to RWR/src/spot_micro_rviz/urdf/spot_micro.urdf
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/urdf/spot_micro.urdf.xacro b/RWR/src/spot_micro_rviz/urdf/spot_micro.urdf.xacro
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/urdf/spot_micro.urdf.xacro
rename to RWR/src/spot_micro_rviz/urdf/spot_micro.urdf.xacro
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/urdf/stl/backpart.stl b/RWR/src/spot_micro_rviz/urdf/stl/backpart.stl
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/urdf/stl/backpart.stl
rename to RWR/src/spot_micro_rviz/urdf/stl/backpart.stl
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/urdf/stl/foot.stl b/RWR/src/spot_micro_rviz/urdf/stl/foot.stl
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/urdf/stl/foot.stl
rename to RWR/src/spot_micro_rviz/urdf/stl/foot.stl
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/urdf/stl/frontpart.stl b/RWR/src/spot_micro_rviz/urdf/stl/frontpart.stl
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/urdf/stl/frontpart.stl
rename to RWR/src/spot_micro_rviz/urdf/stl/frontpart.stl
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/urdf/stl/larm.stl b/RWR/src/spot_micro_rviz/urdf/stl/larm.stl
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/urdf/stl/larm.stl
rename to RWR/src/spot_micro_rviz/urdf/stl/larm.stl
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/urdf/stl/larm_cover.stl b/RWR/src/spot_micro_rviz/urdf/stl/larm_cover.stl
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/urdf/stl/larm_cover.stl
rename to RWR/src/spot_micro_rviz/urdf/stl/larm_cover.stl
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/urdf/stl/lfoot.stl b/RWR/src/spot_micro_rviz/urdf/stl/lfoot.stl
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/urdf/stl/lfoot.stl
rename to RWR/src/spot_micro_rviz/urdf/stl/lfoot.stl
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/urdf/stl/lshoulder.stl b/RWR/src/spot_micro_rviz/urdf/stl/lshoulder.stl
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/urdf/stl/lshoulder.stl
rename to RWR/src/spot_micro_rviz/urdf/stl/lshoulder.stl
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/urdf/stl/mainbody.stl b/RWR/src/spot_micro_rviz/urdf/stl/mainbody.stl
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/urdf/stl/mainbody.stl
rename to RWR/src/spot_micro_rviz/urdf/stl/mainbody.stl
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/urdf/stl/rarm.stl b/RWR/src/spot_micro_rviz/urdf/stl/rarm.stl
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/urdf/stl/rarm.stl
rename to RWR/src/spot_micro_rviz/urdf/stl/rarm.stl
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/urdf/stl/rarm_cover.stl b/RWR/src/spot_micro_rviz/urdf/stl/rarm_cover.stl
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/urdf/stl/rarm_cover.stl
rename to RWR/src/spot_micro_rviz/urdf/stl/rarm_cover.stl
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/urdf/stl/rfoot.stl b/RWR/src/spot_micro_rviz/urdf/stl/rfoot.stl
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/urdf/stl/rfoot.stl
rename to RWR/src/spot_micro_rviz/urdf/stl/rfoot.stl
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/urdf/stl/rplidar_main.STL b/RWR/src/spot_micro_rviz/urdf/stl/rplidar_main.STL
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/urdf/stl/rplidar_main.STL
rename to RWR/src/spot_micro_rviz/urdf/stl/rplidar_main.STL
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/urdf/stl/rshoulder.stl b/RWR/src/spot_micro_rviz/urdf/stl/rshoulder.stl
similarity index 100%
rename from spotMicroCode/catkin_ws/src/spotMicro/spot_micro_rviz/urdf/stl/rshoulder.stl
rename to RWR/src/spot_micro_rviz/urdf/stl/rshoulder.stl
diff --git a/sim_ws/src/yaml_cpp_catkin b/RWR/src/yaml_cpp_catkin
similarity index 100%
rename from sim_ws/src/yaml_cpp_catkin
rename to RWR/src/yaml_cpp_catkin
diff --git a/RWR/src/ze_oss/.gitignore b/RWR/src/ze_oss/.gitignore
new file mode 100644
index 0000000000000000000000000000000000000000..4581ef2eeefc7832704f4a78f3018455f7a06178
--- /dev/null
+++ b/RWR/src/ze_oss/.gitignore
@@ -0,0 +1,29 @@
+# Compiled Object files
+*.slo
+*.lo
+*.o
+*.obj
+
+# Precompiled Headers
+*.gch
+*.pch
+
+# Compiled Dynamic libraries
+*.so
+*.dylib
+*.dll
+
+# Fortran module files
+*.mod
+*.smod
+
+# Compiled Static libraries
+*.lai
+*.la
+*.a
+*.lib
+
+# Executables
+*.exe
+*.out
+*.app
diff --git a/RWR/src/ze_oss/imp_3rdparty_cuda_toolkit/7.0/include/imp/cuda_toolkit/helper_math.hpp b/RWR/src/ze_oss/imp_3rdparty_cuda_toolkit/7.0/include/imp/cuda_toolkit/helper_math.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..9f02edb95b555cd89516751d07f94e8404f846b4
--- /dev/null
+++ b/RWR/src/ze_oss/imp_3rdparty_cuda_toolkit/7.0/include/imp/cuda_toolkit/helper_math.hpp
@@ -0,0 +1,1467 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+/*
+ *  This file implements common mathematical operations on vector types
+ *  (float3, float4 etc.) since these are not provided as standard by CUDA.
+ *
+ *  The syntax is modeled on the Cg standard library.
+ *
+ *  This is part of the Helper library includes
+ *
+ *    Thanks to Linh Hah for additions and fixes.
+ */
+
+#ifndef HELPER_MATH_H
+#define HELPER_MATH_H
+
+#include "cuda_runtime.h"
+
+typedef unsigned int uint;
+typedef unsigned short ushort;
+
+#ifndef EXIT_WAIVED
+#define EXIT_WAIVED 2
+#endif
+
+#ifndef __CUDACC__
+#include <math.h>
+
+////////////////////////////////////////////////////////////////////////////////
+// host implementations of CUDA functions
+////////////////////////////////////////////////////////////////////////////////
+
+inline float fminf(float a, float b)
+{
+    return a < b ? a : b;
+}
+
+inline float fmaxf(float a, float b)
+{
+    return a > b ? a : b;
+}
+
+inline int max(int a, int b)
+{
+    return a > b ? a : b;
+}
+
+inline int min(int a, int b)
+{
+    return a < b ? a : b;
+}
+
+inline float rsqrtf(float x)
+{
+    return 1.0f / sqrtf(x);
+}
+#endif
+
+////////////////////////////////////////////////////////////////////////////////
+// constructors
+////////////////////////////////////////////////////////////////////////////////
+
+inline __host__ __device__ float2 make_float2(float s)
+{
+    return make_float2(s, s);
+}
+inline __host__ __device__ float2 make_float2(float3 a)
+{
+    return make_float2(a.x, a.y);
+}
+inline __host__ __device__ float2 make_float2(int2 a)
+{
+    return make_float2(float(a.x), float(a.y));
+}
+inline __host__ __device__ float2 make_float2(uint2 a)
+{
+    return make_float2(float(a.x), float(a.y));
+}
+
+inline __host__ __device__ int2 make_int2(int s)
+{
+    return make_int2(s, s);
+}
+inline __host__ __device__ int2 make_int2(int3 a)
+{
+    return make_int2(a.x, a.y);
+}
+inline __host__ __device__ int2 make_int2(uint2 a)
+{
+    return make_int2(int(a.x), int(a.y));
+}
+inline __host__ __device__ int2 make_int2(float2 a)
+{
+    return make_int2(int(a.x), int(a.y));
+}
+
+inline __host__ __device__ uint2 make_uint2(uint s)
+{
+    return make_uint2(s, s);
+}
+inline __host__ __device__ uint2 make_uint2(uint3 a)
+{
+    return make_uint2(a.x, a.y);
+}
+inline __host__ __device__ uint2 make_uint2(int2 a)
+{
+    return make_uint2(uint(a.x), uint(a.y));
+}
+
+inline __host__ __device__ float3 make_float3(float s)
+{
+    return make_float3(s, s, s);
+}
+inline __host__ __device__ float3 make_float3(float2 a)
+{
+    return make_float3(a.x, a.y, 0.0f);
+}
+inline __host__ __device__ float3 make_float3(float2 a, float s)
+{
+    return make_float3(a.x, a.y, s);
+}
+inline __host__ __device__ float3 make_float3(float4 a)
+{
+    return make_float3(a.x, a.y, a.z);
+}
+inline __host__ __device__ float3 make_float3(int3 a)
+{
+    return make_float3(float(a.x), float(a.y), float(a.z));
+}
+inline __host__ __device__ float3 make_float3(uint3 a)
+{
+    return make_float3(float(a.x), float(a.y), float(a.z));
+}
+
+inline __host__ __device__ int3 make_int3(int s)
+{
+    return make_int3(s, s, s);
+}
+inline __host__ __device__ int3 make_int3(int2 a)
+{
+    return make_int3(a.x, a.y, 0);
+}
+inline __host__ __device__ int3 make_int3(int2 a, int s)
+{
+    return make_int3(a.x, a.y, s);
+}
+inline __host__ __device__ int3 make_int3(uint3 a)
+{
+    return make_int3(int(a.x), int(a.y), int(a.z));
+}
+inline __host__ __device__ int3 make_int3(float3 a)
+{
+    return make_int3(int(a.x), int(a.y), int(a.z));
+}
+
+inline __host__ __device__ uint3 make_uint3(uint s)
+{
+    return make_uint3(s, s, s);
+}
+inline __host__ __device__ uint3 make_uint3(uint2 a)
+{
+    return make_uint3(a.x, a.y, 0);
+}
+inline __host__ __device__ uint3 make_uint3(uint2 a, uint s)
+{
+    return make_uint3(a.x, a.y, s);
+}
+inline __host__ __device__ uint3 make_uint3(uint4 a)
+{
+    return make_uint3(a.x, a.y, a.z);
+}
+inline __host__ __device__ uint3 make_uint3(int3 a)
+{
+    return make_uint3(uint(a.x), uint(a.y), uint(a.z));
+}
+
+inline __host__ __device__ float4 make_float4(float s)
+{
+    return make_float4(s, s, s, s);
+}
+inline __host__ __device__ float4 make_float4(float3 a)
+{
+    return make_float4(a.x, a.y, a.z, 0.0f);
+}
+inline __host__ __device__ float4 make_float4(float3 a, float w)
+{
+    return make_float4(a.x, a.y, a.z, w);
+}
+inline __host__ __device__ float4 make_float4(int4 a)
+{
+    return make_float4(float(a.x), float(a.y), float(a.z), float(a.w));
+}
+inline __host__ __device__ float4 make_float4(uint4 a)
+{
+    return make_float4(float(a.x), float(a.y), float(a.z), float(a.w));
+}
+
+inline __host__ __device__ int4 make_int4(int s)
+{
+    return make_int4(s, s, s, s);
+}
+inline __host__ __device__ int4 make_int4(int3 a)
+{
+    return make_int4(a.x, a.y, a.z, 0);
+}
+inline __host__ __device__ int4 make_int4(int3 a, int w)
+{
+    return make_int4(a.x, a.y, a.z, w);
+}
+inline __host__ __device__ int4 make_int4(uint4 a)
+{
+    return make_int4(int(a.x), int(a.y), int(a.z), int(a.w));
+}
+inline __host__ __device__ int4 make_int4(float4 a)
+{
+    return make_int4(int(a.x), int(a.y), int(a.z), int(a.w));
+}
+
+
+inline __host__ __device__ uint4 make_uint4(uint s)
+{
+    return make_uint4(s, s, s, s);
+}
+inline __host__ __device__ uint4 make_uint4(uint3 a)
+{
+    return make_uint4(a.x, a.y, a.z, 0);
+}
+inline __host__ __device__ uint4 make_uint4(uint3 a, uint w)
+{
+    return make_uint4(a.x, a.y, a.z, w);
+}
+inline __host__ __device__ uint4 make_uint4(int4 a)
+{
+    return make_uint4(uint(a.x), uint(a.y), uint(a.z), uint(a.w));
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// negate
+////////////////////////////////////////////////////////////////////////////////
+
+inline __host__ __device__ float2 operator-(float2 &a)
+{
+    return make_float2(-a.x, -a.y);
+}
+inline __host__ __device__ int2 operator-(int2 &a)
+{
+    return make_int2(-a.x, -a.y);
+}
+inline __host__ __device__ float3 operator-(float3 &a)
+{
+    return make_float3(-a.x, -a.y, -a.z);
+}
+inline __host__ __device__ int3 operator-(int3 &a)
+{
+    return make_int3(-a.x, -a.y, -a.z);
+}
+inline __host__ __device__ float4 operator-(float4 &a)
+{
+    return make_float4(-a.x, -a.y, -a.z, -a.w);
+}
+inline __host__ __device__ int4 operator-(int4 &a)
+{
+    return make_int4(-a.x, -a.y, -a.z, -a.w);
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// addition
+////////////////////////////////////////////////////////////////////////////////
+
+inline __host__ __device__ float2 operator+(float2 a, float2 b)
+{
+    return make_float2(a.x + b.x, a.y + b.y);
+}
+inline __host__ __device__ void operator+=(float2 &a, float2 b)
+{
+    a.x += b.x;
+    a.y += b.y;
+}
+inline __host__ __device__ float2 operator+(float2 a, float b)
+{
+    return make_float2(a.x + b, a.y + b);
+}
+inline __host__ __device__ float2 operator+(float b, float2 a)
+{
+    return make_float2(a.x + b, a.y + b);
+}
+inline __host__ __device__ void operator+=(float2 &a, float b)
+{
+    a.x += b;
+    a.y += b;
+}
+
+inline __host__ __device__ int2 operator+(int2 a, int2 b)
+{
+    return make_int2(a.x + b.x, a.y + b.y);
+}
+inline __host__ __device__ void operator+=(int2 &a, int2 b)
+{
+    a.x += b.x;
+    a.y += b.y;
+}
+inline __host__ __device__ int2 operator+(int2 a, int b)
+{
+    return make_int2(a.x + b, a.y + b);
+}
+inline __host__ __device__ int2 operator+(int b, int2 a)
+{
+    return make_int2(a.x + b, a.y + b);
+}
+inline __host__ __device__ void operator+=(int2 &a, int b)
+{
+    a.x += b;
+    a.y += b;
+}
+
+inline __host__ __device__ uint2 operator+(uint2 a, uint2 b)
+{
+    return make_uint2(a.x + b.x, a.y + b.y);
+}
+inline __host__ __device__ void operator+=(uint2 &a, uint2 b)
+{
+    a.x += b.x;
+    a.y += b.y;
+}
+inline __host__ __device__ uint2 operator+(uint2 a, uint b)
+{
+    return make_uint2(a.x + b, a.y + b);
+}
+inline __host__ __device__ uint2 operator+(uint b, uint2 a)
+{
+    return make_uint2(a.x + b, a.y + b);
+}
+inline __host__ __device__ void operator+=(uint2 &a, uint b)
+{
+    a.x += b;
+    a.y += b;
+}
+
+
+inline __host__ __device__ float3 operator+(float3 a, float3 b)
+{
+    return make_float3(a.x + b.x, a.y + b.y, a.z + b.z);
+}
+inline __host__ __device__ void operator+=(float3 &a, float3 b)
+{
+    a.x += b.x;
+    a.y += b.y;
+    a.z += b.z;
+}
+inline __host__ __device__ float3 operator+(float3 a, float b)
+{
+    return make_float3(a.x + b, a.y + b, a.z + b);
+}
+inline __host__ __device__ void operator+=(float3 &a, float b)
+{
+    a.x += b;
+    a.y += b;
+    a.z += b;
+}
+
+inline __host__ __device__ int3 operator+(int3 a, int3 b)
+{
+    return make_int3(a.x + b.x, a.y + b.y, a.z + b.z);
+}
+inline __host__ __device__ void operator+=(int3 &a, int3 b)
+{
+    a.x += b.x;
+    a.y += b.y;
+    a.z += b.z;
+}
+inline __host__ __device__ int3 operator+(int3 a, int b)
+{
+    return make_int3(a.x + b, a.y + b, a.z + b);
+}
+inline __host__ __device__ void operator+=(int3 &a, int b)
+{
+    a.x += b;
+    a.y += b;
+    a.z += b;
+}
+
+inline __host__ __device__ uint3 operator+(uint3 a, uint3 b)
+{
+    return make_uint3(a.x + b.x, a.y + b.y, a.z + b.z);
+}
+inline __host__ __device__ void operator+=(uint3 &a, uint3 b)
+{
+    a.x += b.x;
+    a.y += b.y;
+    a.z += b.z;
+}
+inline __host__ __device__ uint3 operator+(uint3 a, uint b)
+{
+    return make_uint3(a.x + b, a.y + b, a.z + b);
+}
+inline __host__ __device__ void operator+=(uint3 &a, uint b)
+{
+    a.x += b;
+    a.y += b;
+    a.z += b;
+}
+
+inline __host__ __device__ int3 operator+(int b, int3 a)
+{
+    return make_int3(a.x + b, a.y + b, a.z + b);
+}
+inline __host__ __device__ uint3 operator+(uint b, uint3 a)
+{
+    return make_uint3(a.x + b, a.y + b, a.z + b);
+}
+inline __host__ __device__ float3 operator+(float b, float3 a)
+{
+    return make_float3(a.x + b, a.y + b, a.z + b);
+}
+
+inline __host__ __device__ float4 operator+(float4 a, float4 b)
+{
+    return make_float4(a.x + b.x, a.y + b.y, a.z + b.z,  a.w + b.w);
+}
+inline __host__ __device__ void operator+=(float4 &a, float4 b)
+{
+    a.x += b.x;
+    a.y += b.y;
+    a.z += b.z;
+    a.w += b.w;
+}
+inline __host__ __device__ float4 operator+(float4 a, float b)
+{
+    return make_float4(a.x + b, a.y + b, a.z + b, a.w + b);
+}
+inline __host__ __device__ float4 operator+(float b, float4 a)
+{
+    return make_float4(a.x + b, a.y + b, a.z + b, a.w + b);
+}
+inline __host__ __device__ void operator+=(float4 &a, float b)
+{
+    a.x += b;
+    a.y += b;
+    a.z += b;
+    a.w += b;
+}
+
+inline __host__ __device__ int4 operator+(int4 a, int4 b)
+{
+    return make_int4(a.x + b.x, a.y + b.y, a.z + b.z,  a.w + b.w);
+}
+inline __host__ __device__ void operator+=(int4 &a, int4 b)
+{
+    a.x += b.x;
+    a.y += b.y;
+    a.z += b.z;
+    a.w += b.w;
+}
+inline __host__ __device__ int4 operator+(int4 a, int b)
+{
+    return make_int4(a.x + b, a.y + b, a.z + b,  a.w + b);
+}
+inline __host__ __device__ int4 operator+(int b, int4 a)
+{
+    return make_int4(a.x + b, a.y + b, a.z + b,  a.w + b);
+}
+inline __host__ __device__ void operator+=(int4 &a, int b)
+{
+    a.x += b;
+    a.y += b;
+    a.z += b;
+    a.w += b;
+}
+
+inline __host__ __device__ uint4 operator+(uint4 a, uint4 b)
+{
+    return make_uint4(a.x + b.x, a.y + b.y, a.z + b.z,  a.w + b.w);
+}
+inline __host__ __device__ void operator+=(uint4 &a, uint4 b)
+{
+    a.x += b.x;
+    a.y += b.y;
+    a.z += b.z;
+    a.w += b.w;
+}
+inline __host__ __device__ uint4 operator+(uint4 a, uint b)
+{
+    return make_uint4(a.x + b, a.y + b, a.z + b,  a.w + b);
+}
+inline __host__ __device__ uint4 operator+(uint b, uint4 a)
+{
+    return make_uint4(a.x + b, a.y + b, a.z + b,  a.w + b);
+}
+inline __host__ __device__ void operator+=(uint4 &a, uint b)
+{
+    a.x += b;
+    a.y += b;
+    a.z += b;
+    a.w += b;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// subtract
+////////////////////////////////////////////////////////////////////////////////
+
+inline __host__ __device__ float2 operator-(float2 a, float2 b)
+{
+    return make_float2(a.x - b.x, a.y - b.y);
+}
+inline __host__ __device__ void operator-=(float2 &a, float2 b)
+{
+    a.x -= b.x;
+    a.y -= b.y;
+}
+inline __host__ __device__ float2 operator-(float2 a, float b)
+{
+    return make_float2(a.x - b, a.y - b);
+}
+inline __host__ __device__ float2 operator-(float b, float2 a)
+{
+    return make_float2(b - a.x, b - a.y);
+}
+inline __host__ __device__ void operator-=(float2 &a, float b)
+{
+    a.x -= b;
+    a.y -= b;
+}
+
+inline __host__ __device__ int2 operator-(int2 a, int2 b)
+{
+    return make_int2(a.x - b.x, a.y - b.y);
+}
+inline __host__ __device__ void operator-=(int2 &a, int2 b)
+{
+    a.x -= b.x;
+    a.y -= b.y;
+}
+inline __host__ __device__ int2 operator-(int2 a, int b)
+{
+    return make_int2(a.x - b, a.y - b);
+}
+inline __host__ __device__ int2 operator-(int b, int2 a)
+{
+    return make_int2(b - a.x, b - a.y);
+}
+inline __host__ __device__ void operator-=(int2 &a, int b)
+{
+    a.x -= b;
+    a.y -= b;
+}
+
+inline __host__ __device__ uint2 operator-(uint2 a, uint2 b)
+{
+    return make_uint2(a.x - b.x, a.y - b.y);
+}
+inline __host__ __device__ void operator-=(uint2 &a, uint2 b)
+{
+    a.x -= b.x;
+    a.y -= b.y;
+}
+inline __host__ __device__ uint2 operator-(uint2 a, uint b)
+{
+    return make_uint2(a.x - b, a.y - b);
+}
+inline __host__ __device__ uint2 operator-(uint b, uint2 a)
+{
+    return make_uint2(b - a.x, b - a.y);
+}
+inline __host__ __device__ void operator-=(uint2 &a, uint b)
+{
+    a.x -= b;
+    a.y -= b;
+}
+
+inline __host__ __device__ float3 operator-(float3 a, float3 b)
+{
+    return make_float3(a.x - b.x, a.y - b.y, a.z - b.z);
+}
+inline __host__ __device__ void operator-=(float3 &a, float3 b)
+{
+    a.x -= b.x;
+    a.y -= b.y;
+    a.z -= b.z;
+}
+inline __host__ __device__ float3 operator-(float3 a, float b)
+{
+    return make_float3(a.x - b, a.y - b, a.z - b);
+}
+inline __host__ __device__ float3 operator-(float b, float3 a)
+{
+    return make_float3(b - a.x, b - a.y, b - a.z);
+}
+inline __host__ __device__ void operator-=(float3 &a, float b)
+{
+    a.x -= b;
+    a.y -= b;
+    a.z -= b;
+}
+
+inline __host__ __device__ int3 operator-(int3 a, int3 b)
+{
+    return make_int3(a.x - b.x, a.y - b.y, a.z - b.z);
+}
+inline __host__ __device__ void operator-=(int3 &a, int3 b)
+{
+    a.x -= b.x;
+    a.y -= b.y;
+    a.z -= b.z;
+}
+inline __host__ __device__ int3 operator-(int3 a, int b)
+{
+    return make_int3(a.x - b, a.y - b, a.z - b);
+}
+inline __host__ __device__ int3 operator-(int b, int3 a)
+{
+    return make_int3(b - a.x, b - a.y, b - a.z);
+}
+inline __host__ __device__ void operator-=(int3 &a, int b)
+{
+    a.x -= b;
+    a.y -= b;
+    a.z -= b;
+}
+
+inline __host__ __device__ uint3 operator-(uint3 a, uint3 b)
+{
+    return make_uint3(a.x - b.x, a.y - b.y, a.z - b.z);
+}
+inline __host__ __device__ void operator-=(uint3 &a, uint3 b)
+{
+    a.x -= b.x;
+    a.y -= b.y;
+    a.z -= b.z;
+}
+inline __host__ __device__ uint3 operator-(uint3 a, uint b)
+{
+    return make_uint3(a.x - b, a.y - b, a.z - b);
+}
+inline __host__ __device__ uint3 operator-(uint b, uint3 a)
+{
+    return make_uint3(b - a.x, b - a.y, b - a.z);
+}
+inline __host__ __device__ void operator-=(uint3 &a, uint b)
+{
+    a.x -= b;
+    a.y -= b;
+    a.z -= b;
+}
+
+inline __host__ __device__ float4 operator-(float4 a, float4 b)
+{
+    return make_float4(a.x - b.x, a.y - b.y, a.z - b.z,  a.w - b.w);
+}
+inline __host__ __device__ void operator-=(float4 &a, float4 b)
+{
+    a.x -= b.x;
+    a.y -= b.y;
+    a.z -= b.z;
+    a.w -= b.w;
+}
+inline __host__ __device__ float4 operator-(float4 a, float b)
+{
+    return make_float4(a.x - b, a.y - b, a.z - b,  a.w - b);
+}
+inline __host__ __device__ void operator-=(float4 &a, float b)
+{
+    a.x -= b;
+    a.y -= b;
+    a.z -= b;
+    a.w -= b;
+}
+
+inline __host__ __device__ int4 operator-(int4 a, int4 b)
+{
+    return make_int4(a.x - b.x, a.y - b.y, a.z - b.z,  a.w - b.w);
+}
+inline __host__ __device__ void operator-=(int4 &a, int4 b)
+{
+    a.x -= b.x;
+    a.y -= b.y;
+    a.z -= b.z;
+    a.w -= b.w;
+}
+inline __host__ __device__ int4 operator-(int4 a, int b)
+{
+    return make_int4(a.x - b, a.y - b, a.z - b,  a.w - b);
+}
+inline __host__ __device__ int4 operator-(int b, int4 a)
+{
+    return make_int4(b - a.x, b - a.y, b - a.z, b - a.w);
+}
+inline __host__ __device__ void operator-=(int4 &a, int b)
+{
+    a.x -= b;
+    a.y -= b;
+    a.z -= b;
+    a.w -= b;
+}
+
+inline __host__ __device__ uint4 operator-(uint4 a, uint4 b)
+{
+    return make_uint4(a.x - b.x, a.y - b.y, a.z - b.z,  a.w - b.w);
+}
+inline __host__ __device__ void operator-=(uint4 &a, uint4 b)
+{
+    a.x -= b.x;
+    a.y -= b.y;
+    a.z -= b.z;
+    a.w -= b.w;
+}
+inline __host__ __device__ uint4 operator-(uint4 a, uint b)
+{
+    return make_uint4(a.x - b, a.y - b, a.z - b,  a.w - b);
+}
+inline __host__ __device__ uint4 operator-(uint b, uint4 a)
+{
+    return make_uint4(b - a.x, b - a.y, b - a.z, b - a.w);
+}
+inline __host__ __device__ void operator-=(uint4 &a, uint b)
+{
+    a.x -= b;
+    a.y -= b;
+    a.z -= b;
+    a.w -= b;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// multiply
+////////////////////////////////////////////////////////////////////////////////
+
+inline __host__ __device__ float2 operator*(float2 a, float2 b)
+{
+    return make_float2(a.x * b.x, a.y * b.y);
+}
+inline __host__ __device__ void operator*=(float2 &a, float2 b)
+{
+    a.x *= b.x;
+    a.y *= b.y;
+}
+inline __host__ __device__ float2 operator*(float2 a, float b)
+{
+    return make_float2(a.x * b, a.y * b);
+}
+inline __host__ __device__ float2 operator*(float b, float2 a)
+{
+    return make_float2(b * a.x, b * a.y);
+}
+inline __host__ __device__ void operator*=(float2 &a, float b)
+{
+    a.x *= b;
+    a.y *= b;
+}
+
+inline __host__ __device__ int2 operator*(int2 a, int2 b)
+{
+    return make_int2(a.x * b.x, a.y * b.y);
+}
+inline __host__ __device__ void operator*=(int2 &a, int2 b)
+{
+    a.x *= b.x;
+    a.y *= b.y;
+}
+inline __host__ __device__ int2 operator*(int2 a, int b)
+{
+    return make_int2(a.x * b, a.y * b);
+}
+inline __host__ __device__ int2 operator*(int b, int2 a)
+{
+    return make_int2(b * a.x, b * a.y);
+}
+inline __host__ __device__ void operator*=(int2 &a, int b)
+{
+    a.x *= b;
+    a.y *= b;
+}
+
+inline __host__ __device__ uint2 operator*(uint2 a, uint2 b)
+{
+    return make_uint2(a.x * b.x, a.y * b.y);
+}
+inline __host__ __device__ void operator*=(uint2 &a, uint2 b)
+{
+    a.x *= b.x;
+    a.y *= b.y;
+}
+inline __host__ __device__ uint2 operator*(uint2 a, uint b)
+{
+    return make_uint2(a.x * b, a.y * b);
+}
+inline __host__ __device__ uint2 operator*(uint b, uint2 a)
+{
+    return make_uint2(b * a.x, b * a.y);
+}
+inline __host__ __device__ void operator*=(uint2 &a, uint b)
+{
+    a.x *= b;
+    a.y *= b;
+}
+
+inline __host__ __device__ float3 operator*(float3 a, float3 b)
+{
+    return make_float3(a.x * b.x, a.y * b.y, a.z * b.z);
+}
+inline __host__ __device__ void operator*=(float3 &a, float3 b)
+{
+    a.x *= b.x;
+    a.y *= b.y;
+    a.z *= b.z;
+}
+inline __host__ __device__ float3 operator*(float3 a, float b)
+{
+    return make_float3(a.x * b, a.y * b, a.z * b);
+}
+inline __host__ __device__ float3 operator*(float b, float3 a)
+{
+    return make_float3(b * a.x, b * a.y, b * a.z);
+}
+inline __host__ __device__ void operator*=(float3 &a, float b)
+{
+    a.x *= b;
+    a.y *= b;
+    a.z *= b;
+}
+
+inline __host__ __device__ int3 operator*(int3 a, int3 b)
+{
+    return make_int3(a.x * b.x, a.y * b.y, a.z * b.z);
+}
+inline __host__ __device__ void operator*=(int3 &a, int3 b)
+{
+    a.x *= b.x;
+    a.y *= b.y;
+    a.z *= b.z;
+}
+inline __host__ __device__ int3 operator*(int3 a, int b)
+{
+    return make_int3(a.x * b, a.y * b, a.z * b);
+}
+inline __host__ __device__ int3 operator*(int b, int3 a)
+{
+    return make_int3(b * a.x, b * a.y, b * a.z);
+}
+inline __host__ __device__ void operator*=(int3 &a, int b)
+{
+    a.x *= b;
+    a.y *= b;
+    a.z *= b;
+}
+
+inline __host__ __device__ uint3 operator*(uint3 a, uint3 b)
+{
+    return make_uint3(a.x * b.x, a.y * b.y, a.z * b.z);
+}
+inline __host__ __device__ void operator*=(uint3 &a, uint3 b)
+{
+    a.x *= b.x;
+    a.y *= b.y;
+    a.z *= b.z;
+}
+inline __host__ __device__ uint3 operator*(uint3 a, uint b)
+{
+    return make_uint3(a.x * b, a.y * b, a.z * b);
+}
+inline __host__ __device__ uint3 operator*(uint b, uint3 a)
+{
+    return make_uint3(b * a.x, b * a.y, b * a.z);
+}
+inline __host__ __device__ void operator*=(uint3 &a, uint b)
+{
+    a.x *= b;
+    a.y *= b;
+    a.z *= b;
+}
+
+inline __host__ __device__ float4 operator*(float4 a, float4 b)
+{
+    return make_float4(a.x * b.x, a.y * b.y, a.z * b.z,  a.w * b.w);
+}
+inline __host__ __device__ void operator*=(float4 &a, float4 b)
+{
+    a.x *= b.x;
+    a.y *= b.y;
+    a.z *= b.z;
+    a.w *= b.w;
+}
+inline __host__ __device__ float4 operator*(float4 a, float b)
+{
+    return make_float4(a.x * b, a.y * b, a.z * b,  a.w * b);
+}
+inline __host__ __device__ float4 operator*(float b, float4 a)
+{
+    return make_float4(b * a.x, b * a.y, b * a.z, b * a.w);
+}
+inline __host__ __device__ void operator*=(float4 &a, float b)
+{
+    a.x *= b;
+    a.y *= b;
+    a.z *= b;
+    a.w *= b;
+}
+
+inline __host__ __device__ int4 operator*(int4 a, int4 b)
+{
+    return make_int4(a.x * b.x, a.y * b.y, a.z * b.z,  a.w * b.w);
+}
+inline __host__ __device__ void operator*=(int4 &a, int4 b)
+{
+    a.x *= b.x;
+    a.y *= b.y;
+    a.z *= b.z;
+    a.w *= b.w;
+}
+inline __host__ __device__ int4 operator*(int4 a, int b)
+{
+    return make_int4(a.x * b, a.y * b, a.z * b,  a.w * b);
+}
+inline __host__ __device__ int4 operator*(int b, int4 a)
+{
+    return make_int4(b * a.x, b * a.y, b * a.z, b * a.w);
+}
+inline __host__ __device__ void operator*=(int4 &a, int b)
+{
+    a.x *= b;
+    a.y *= b;
+    a.z *= b;
+    a.w *= b;
+}
+
+inline __host__ __device__ uint4 operator*(uint4 a, uint4 b)
+{
+    return make_uint4(a.x * b.x, a.y * b.y, a.z * b.z,  a.w * b.w);
+}
+inline __host__ __device__ void operator*=(uint4 &a, uint4 b)
+{
+    a.x *= b.x;
+    a.y *= b.y;
+    a.z *= b.z;
+    a.w *= b.w;
+}
+inline __host__ __device__ uint4 operator*(uint4 a, uint b)
+{
+    return make_uint4(a.x * b, a.y * b, a.z * b,  a.w * b);
+}
+inline __host__ __device__ uint4 operator*(uint b, uint4 a)
+{
+    return make_uint4(b * a.x, b * a.y, b * a.z, b * a.w);
+}
+inline __host__ __device__ void operator*=(uint4 &a, uint b)
+{
+    a.x *= b;
+    a.y *= b;
+    a.z *= b;
+    a.w *= b;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// divide
+////////////////////////////////////////////////////////////////////////////////
+
+inline __host__ __device__ float2 operator/(float2 a, float2 b)
+{
+    return make_float2(a.x / b.x, a.y / b.y);
+}
+inline __host__ __device__ void operator/=(float2 &a, float2 b)
+{
+    a.x /= b.x;
+    a.y /= b.y;
+}
+inline __host__ __device__ float2 operator/(float2 a, float b)
+{
+    return make_float2(a.x / b, a.y / b);
+}
+inline __host__ __device__ void operator/=(float2 &a, float b)
+{
+    a.x /= b;
+    a.y /= b;
+}
+inline __host__ __device__ float2 operator/(float b, float2 a)
+{
+    return make_float2(b / a.x, b / a.y);
+}
+
+inline __host__ __device__ float3 operator/(float3 a, float3 b)
+{
+    return make_float3(a.x / b.x, a.y / b.y, a.z / b.z);
+}
+inline __host__ __device__ void operator/=(float3 &a, float3 b)
+{
+    a.x /= b.x;
+    a.y /= b.y;
+    a.z /= b.z;
+}
+inline __host__ __device__ float3 operator/(float3 a, float b)
+{
+    return make_float3(a.x / b, a.y / b, a.z / b);
+}
+inline __host__ __device__ void operator/=(float3 &a, float b)
+{
+    a.x /= b;
+    a.y /= b;
+    a.z /= b;
+}
+inline __host__ __device__ float3 operator/(float b, float3 a)
+{
+    return make_float3(b / a.x, b / a.y, b / a.z);
+}
+
+inline __host__ __device__ float4 operator/(float4 a, float4 b)
+{
+    return make_float4(a.x / b.x, a.y / b.y, a.z / b.z,  a.w / b.w);
+}
+inline __host__ __device__ void operator/=(float4 &a, float4 b)
+{
+    a.x /= b.x;
+    a.y /= b.y;
+    a.z /= b.z;
+    a.w /= b.w;
+}
+inline __host__ __device__ float4 operator/(float4 a, float b)
+{
+    return make_float4(a.x / b, a.y / b, a.z / b,  a.w / b);
+}
+inline __host__ __device__ void operator/=(float4 &a, float b)
+{
+    a.x /= b;
+    a.y /= b;
+    a.z /= b;
+    a.w /= b;
+}
+inline __host__ __device__ float4 operator/(float b, float4 a)
+{
+    return make_float4(b / a.x, b / a.y, b / a.z, b / a.w);
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// min
+////////////////////////////////////////////////////////////////////////////////
+
+inline  __host__ __device__ float2 fminf(float2 a, float2 b)
+{
+    return make_float2(fminf(a.x,b.x), fminf(a.y,b.y));
+}
+inline __host__ __device__ float3 fminf(float3 a, float3 b)
+{
+    return make_float3(fminf(a.x,b.x), fminf(a.y,b.y), fminf(a.z,b.z));
+}
+inline  __host__ __device__ float4 fminf(float4 a, float4 b)
+{
+    return make_float4(fminf(a.x,b.x), fminf(a.y,b.y), fminf(a.z,b.z), fminf(a.w,b.w));
+}
+
+inline __host__ __device__ int2 min(int2 a, int2 b)
+{
+    return make_int2(min(a.x,b.x), min(a.y,b.y));
+}
+inline __host__ __device__ int3 min(int3 a, int3 b)
+{
+    return make_int3(min(a.x,b.x), min(a.y,b.y), min(a.z,b.z));
+}
+inline __host__ __device__ int4 min(int4 a, int4 b)
+{
+    return make_int4(min(a.x,b.x), min(a.y,b.y), min(a.z,b.z), min(a.w,b.w));
+}
+
+inline __host__ __device__ uint2 min(uint2 a, uint2 b)
+{
+    return make_uint2(min(a.x,b.x), min(a.y,b.y));
+}
+inline __host__ __device__ uint3 min(uint3 a, uint3 b)
+{
+    return make_uint3(min(a.x,b.x), min(a.y,b.y), min(a.z,b.z));
+}
+inline __host__ __device__ uint4 min(uint4 a, uint4 b)
+{
+    return make_uint4(min(a.x,b.x), min(a.y,b.y), min(a.z,b.z), min(a.w,b.w));
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// max
+////////////////////////////////////////////////////////////////////////////////
+
+inline __host__ __device__ float2 fmaxf(float2 a, float2 b)
+{
+    return make_float2(fmaxf(a.x,b.x), fmaxf(a.y,b.y));
+}
+inline __host__ __device__ float3 fmaxf(float3 a, float3 b)
+{
+    return make_float3(fmaxf(a.x,b.x), fmaxf(a.y,b.y), fmaxf(a.z,b.z));
+}
+inline __host__ __device__ float4 fmaxf(float4 a, float4 b)
+{
+    return make_float4(fmaxf(a.x,b.x), fmaxf(a.y,b.y), fmaxf(a.z,b.z), fmaxf(a.w,b.w));
+}
+
+inline __host__ __device__ int2 max(int2 a, int2 b)
+{
+    return make_int2(max(a.x,b.x), max(a.y,b.y));
+}
+inline __host__ __device__ int3 max(int3 a, int3 b)
+{
+    return make_int3(max(a.x,b.x), max(a.y,b.y), max(a.z,b.z));
+}
+inline __host__ __device__ int4 max(int4 a, int4 b)
+{
+    return make_int4(max(a.x,b.x), max(a.y,b.y), max(a.z,b.z), max(a.w,b.w));
+}
+
+inline __host__ __device__ uint2 max(uint2 a, uint2 b)
+{
+    return make_uint2(max(a.x,b.x), max(a.y,b.y));
+}
+inline __host__ __device__ uint3 max(uint3 a, uint3 b)
+{
+    return make_uint3(max(a.x,b.x), max(a.y,b.y), max(a.z,b.z));
+}
+inline __host__ __device__ uint4 max(uint4 a, uint4 b)
+{
+    return make_uint4(max(a.x,b.x), max(a.y,b.y), max(a.z,b.z), max(a.w,b.w));
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// lerp
+// - linear interpolation between a and b, based on value t in [0, 1] range
+////////////////////////////////////////////////////////////////////////////////
+
+inline __device__ __host__ float lerp(float a, float b, float t)
+{
+    return a + t*(b-a);
+}
+inline __device__ __host__ float2 lerp(float2 a, float2 b, float t)
+{
+    return a + t*(b-a);
+}
+inline __device__ __host__ float3 lerp(float3 a, float3 b, float t)
+{
+    return a + t*(b-a);
+}
+inline __device__ __host__ float4 lerp(float4 a, float4 b, float t)
+{
+    return a + t*(b-a);
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// clamp
+// - clamp the value v to be in the range [a, b]
+////////////////////////////////////////////////////////////////////////////////
+
+inline __device__ __host__ float clamp(float f, float a, float b)
+{
+    return fmaxf(a, fminf(f, b));
+}
+inline __device__ __host__ int clamp(int f, int a, int b)
+{
+    return max(a, min(f, b));
+}
+inline __device__ __host__ uint clamp(uint f, uint a, uint b)
+{
+    return max(a, min(f, b));
+}
+
+inline __device__ __host__ float2 clamp(float2 v, float a, float b)
+{
+    return make_float2(clamp(v.x, a, b), clamp(v.y, a, b));
+}
+inline __device__ __host__ float2 clamp(float2 v, float2 a, float2 b)
+{
+    return make_float2(clamp(v.x, a.x, b.x), clamp(v.y, a.y, b.y));
+}
+inline __device__ __host__ float3 clamp(float3 v, float a, float b)
+{
+    return make_float3(clamp(v.x, a, b), clamp(v.y, a, b), clamp(v.z, a, b));
+}
+inline __device__ __host__ float3 clamp(float3 v, float3 a, float3 b)
+{
+    return make_float3(clamp(v.x, a.x, b.x), clamp(v.y, a.y, b.y), clamp(v.z, a.z, b.z));
+}
+inline __device__ __host__ float4 clamp(float4 v, float a, float b)
+{
+    return make_float4(clamp(v.x, a, b), clamp(v.y, a, b), clamp(v.z, a, b), clamp(v.w, a, b));
+}
+inline __device__ __host__ float4 clamp(float4 v, float4 a, float4 b)
+{
+    return make_float4(clamp(v.x, a.x, b.x), clamp(v.y, a.y, b.y), clamp(v.z, a.z, b.z), clamp(v.w, a.w, b.w));
+}
+
+inline __device__ __host__ int2 clamp(int2 v, int a, int b)
+{
+    return make_int2(clamp(v.x, a, b), clamp(v.y, a, b));
+}
+inline __device__ __host__ int2 clamp(int2 v, int2 a, int2 b)
+{
+    return make_int2(clamp(v.x, a.x, b.x), clamp(v.y, a.y, b.y));
+}
+inline __device__ __host__ int3 clamp(int3 v, int a, int b)
+{
+    return make_int3(clamp(v.x, a, b), clamp(v.y, a, b), clamp(v.z, a, b));
+}
+inline __device__ __host__ int3 clamp(int3 v, int3 a, int3 b)
+{
+    return make_int3(clamp(v.x, a.x, b.x), clamp(v.y, a.y, b.y), clamp(v.z, a.z, b.z));
+}
+inline __device__ __host__ int4 clamp(int4 v, int a, int b)
+{
+    return make_int4(clamp(v.x, a, b), clamp(v.y, a, b), clamp(v.z, a, b), clamp(v.w, a, b));
+}
+inline __device__ __host__ int4 clamp(int4 v, int4 a, int4 b)
+{
+    return make_int4(clamp(v.x, a.x, b.x), clamp(v.y, a.y, b.y), clamp(v.z, a.z, b.z), clamp(v.w, a.w, b.w));
+}
+
+inline __device__ __host__ uint2 clamp(uint2 v, uint a, uint b)
+{
+    return make_uint2(clamp(v.x, a, b), clamp(v.y, a, b));
+}
+inline __device__ __host__ uint2 clamp(uint2 v, uint2 a, uint2 b)
+{
+    return make_uint2(clamp(v.x, a.x, b.x), clamp(v.y, a.y, b.y));
+}
+inline __device__ __host__ uint3 clamp(uint3 v, uint a, uint b)
+{
+    return make_uint3(clamp(v.x, a, b), clamp(v.y, a, b), clamp(v.z, a, b));
+}
+inline __device__ __host__ uint3 clamp(uint3 v, uint3 a, uint3 b)
+{
+    return make_uint3(clamp(v.x, a.x, b.x), clamp(v.y, a.y, b.y), clamp(v.z, a.z, b.z));
+}
+inline __device__ __host__ uint4 clamp(uint4 v, uint a, uint b)
+{
+    return make_uint4(clamp(v.x, a, b), clamp(v.y, a, b), clamp(v.z, a, b), clamp(v.w, a, b));
+}
+inline __device__ __host__ uint4 clamp(uint4 v, uint4 a, uint4 b)
+{
+    return make_uint4(clamp(v.x, a.x, b.x), clamp(v.y, a.y, b.y), clamp(v.z, a.z, b.z), clamp(v.w, a.w, b.w));
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// dot product
+////////////////////////////////////////////////////////////////////////////////
+
+inline __host__ __device__ float dot(float2 a, float2 b)
+{
+    return a.x * b.x + a.y * b.y;
+}
+inline __host__ __device__ float dot(float3 a, float3 b)
+{
+    return a.x * b.x + a.y * b.y + a.z * b.z;
+}
+inline __host__ __device__ float dot(float4 a, float4 b)
+{
+    return a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w;
+}
+
+inline __host__ __device__ int dot(int2 a, int2 b)
+{
+    return a.x * b.x + a.y * b.y;
+}
+inline __host__ __device__ int dot(int3 a, int3 b)
+{
+    return a.x * b.x + a.y * b.y + a.z * b.z;
+}
+inline __host__ __device__ int dot(int4 a, int4 b)
+{
+    return a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w;
+}
+
+inline __host__ __device__ uint dot(uint2 a, uint2 b)
+{
+    return a.x * b.x + a.y * b.y;
+}
+inline __host__ __device__ uint dot(uint3 a, uint3 b)
+{
+    return a.x * b.x + a.y * b.y + a.z * b.z;
+}
+inline __host__ __device__ uint dot(uint4 a, uint4 b)
+{
+    return a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// length
+////////////////////////////////////////////////////////////////////////////////
+
+inline __host__ __device__ float length(float2 v)
+{
+    return sqrtf(dot(v, v));
+}
+inline __host__ __device__ float length(float3 v)
+{
+    return sqrtf(dot(v, v));
+}
+inline __host__ __device__ float length(float4 v)
+{
+    return sqrtf(dot(v, v));
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// normalize
+////////////////////////////////////////////////////////////////////////////////
+
+inline __host__ __device__ float2 normalize(float2 v)
+{
+    float invLen = rsqrtf(dot(v, v));
+    return v * invLen;
+}
+inline __host__ __device__ float3 normalize(float3 v)
+{
+    float invLen = rsqrtf(dot(v, v));
+    return v * invLen;
+}
+inline __host__ __device__ float4 normalize(float4 v)
+{
+    float invLen = rsqrtf(dot(v, v));
+    return v * invLen;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// floor
+////////////////////////////////////////////////////////////////////////////////
+
+inline __host__ __device__ float2 floorf(float2 v)
+{
+    return make_float2(floorf(v.x), floorf(v.y));
+}
+inline __host__ __device__ float3 floorf(float3 v)
+{
+    return make_float3(floorf(v.x), floorf(v.y), floorf(v.z));
+}
+inline __host__ __device__ float4 floorf(float4 v)
+{
+    return make_float4(floorf(v.x), floorf(v.y), floorf(v.z), floorf(v.w));
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// frac - returns the fractional portion of a scalar or each vector component
+////////////////////////////////////////////////////////////////////////////////
+
+inline __host__ __device__ float fracf(float v)
+{
+    return v - floorf(v);
+}
+inline __host__ __device__ float2 fracf(float2 v)
+{
+    return make_float2(fracf(v.x), fracf(v.y));
+}
+inline __host__ __device__ float3 fracf(float3 v)
+{
+    return make_float3(fracf(v.x), fracf(v.y), fracf(v.z));
+}
+inline __host__ __device__ float4 fracf(float4 v)
+{
+    return make_float4(fracf(v.x), fracf(v.y), fracf(v.z), fracf(v.w));
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// fmod
+////////////////////////////////////////////////////////////////////////////////
+
+inline __host__ __device__ float2 fmodf(float2 a, float2 b)
+{
+    return make_float2(fmodf(a.x, b.x), fmodf(a.y, b.y));
+}
+inline __host__ __device__ float3 fmodf(float3 a, float3 b)
+{
+    return make_float3(fmodf(a.x, b.x), fmodf(a.y, b.y), fmodf(a.z, b.z));
+}
+inline __host__ __device__ float4 fmodf(float4 a, float4 b)
+{
+    return make_float4(fmodf(a.x, b.x), fmodf(a.y, b.y), fmodf(a.z, b.z), fmodf(a.w, b.w));
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// absolute value
+////////////////////////////////////////////////////////////////////////////////
+
+inline __host__ __device__ float2 fabs(float2 v)
+{
+    return make_float2(fabs(v.x), fabs(v.y));
+}
+inline __host__ __device__ float3 fabs(float3 v)
+{
+    return make_float3(fabs(v.x), fabs(v.y), fabs(v.z));
+}
+inline __host__ __device__ float4 fabs(float4 v)
+{
+    return make_float4(fabs(v.x), fabs(v.y), fabs(v.z), fabs(v.w));
+}
+
+inline __host__ __device__ int2 abs(int2 v)
+{
+    return make_int2(abs(v.x), abs(v.y));
+}
+inline __host__ __device__ int3 abs(int3 v)
+{
+    return make_int3(abs(v.x), abs(v.y), abs(v.z));
+}
+inline __host__ __device__ int4 abs(int4 v)
+{
+    return make_int4(abs(v.x), abs(v.y), abs(v.z), abs(v.w));
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// reflect
+// - returns reflection of incident ray I around surface normal N
+// - N should be normalized, reflected vector's length is equal to length of I
+////////////////////////////////////////////////////////////////////////////////
+
+inline __host__ __device__ float3 reflect(float3 i, float3 n)
+{
+    return i - 2.0f * n * dot(n,i);
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// cross product
+////////////////////////////////////////////////////////////////////////////////
+
+inline __host__ __device__ float3 cross(float3 a, float3 b)
+{
+    return make_float3(a.y*b.z - a.z*b.y, a.z*b.x - a.x*b.z, a.x*b.y - a.y*b.x);
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// smoothstep
+// - returns 0 if x < a
+// - returns 1 if x > b
+// - otherwise returns smooth interpolation between 0 and 1 based on x
+////////////////////////////////////////////////////////////////////////////////
+
+inline __device__ __host__ float smoothstep(float a, float b, float x)
+{
+    float y = clamp((x - a) / (b - a), 0.0f, 1.0f);
+    return (y*y*(3.0f - (2.0f*y)));
+}
+inline __device__ __host__ float2 smoothstep(float2 a, float2 b, float2 x)
+{
+    float2 y = clamp((x - a) / (b - a), 0.0f, 1.0f);
+    return (y*y*(make_float2(3.0f) - (make_float2(2.0f)*y)));
+}
+inline __device__ __host__ float3 smoothstep(float3 a, float3 b, float3 x)
+{
+    float3 y = clamp((x - a) / (b - a), 0.0f, 1.0f);
+    return (y*y*(make_float3(3.0f) - (make_float3(2.0f)*y)));
+}
+inline __device__ __host__ float4 smoothstep(float4 a, float4 b, float4 x)
+{
+    float4 y = clamp((x - a) / (b - a), 0.0f, 1.0f);
+    return (y*y*(make_float4(3.0f) - (make_float4(2.0f)*y)));
+}
+
+#endif
diff --git a/RWR/src/ze_oss/imp_3rdparty_cuda_toolkit/7.5/include/imp/cuda_toolkit/helper_math.hpp b/RWR/src/ze_oss/imp_3rdparty_cuda_toolkit/7.5/include/imp/cuda_toolkit/helper_math.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..9f02edb95b555cd89516751d07f94e8404f846b4
--- /dev/null
+++ b/RWR/src/ze_oss/imp_3rdparty_cuda_toolkit/7.5/include/imp/cuda_toolkit/helper_math.hpp
@@ -0,0 +1,1467 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+/*
+ *  This file implements common mathematical operations on vector types
+ *  (float3, float4 etc.) since these are not provided as standard by CUDA.
+ *
+ *  The syntax is modeled on the Cg standard library.
+ *
+ *  This is part of the Helper library includes
+ *
+ *    Thanks to Linh Hah for additions and fixes.
+ */
+
+#ifndef HELPER_MATH_H
+#define HELPER_MATH_H
+
+#include "cuda_runtime.h"
+
+typedef unsigned int uint;
+typedef unsigned short ushort;
+
+#ifndef EXIT_WAIVED
+#define EXIT_WAIVED 2
+#endif
+
+#ifndef __CUDACC__
+#include <math.h>
+
+////////////////////////////////////////////////////////////////////////////////
+// host implementations of CUDA functions
+////////////////////////////////////////////////////////////////////////////////
+
+inline float fminf(float a, float b)
+{
+    return a < b ? a : b;
+}
+
+inline float fmaxf(float a, float b)
+{
+    return a > b ? a : b;
+}
+
+inline int max(int a, int b)
+{
+    return a > b ? a : b;
+}
+
+inline int min(int a, int b)
+{
+    return a < b ? a : b;
+}
+
+inline float rsqrtf(float x)
+{
+    return 1.0f / sqrtf(x);
+}
+#endif
+
+////////////////////////////////////////////////////////////////////////////////
+// constructors
+////////////////////////////////////////////////////////////////////////////////
+
+inline __host__ __device__ float2 make_float2(float s)
+{
+    return make_float2(s, s);
+}
+inline __host__ __device__ float2 make_float2(float3 a)
+{
+    return make_float2(a.x, a.y);
+}
+inline __host__ __device__ float2 make_float2(int2 a)
+{
+    return make_float2(float(a.x), float(a.y));
+}
+inline __host__ __device__ float2 make_float2(uint2 a)
+{
+    return make_float2(float(a.x), float(a.y));
+}
+
+inline __host__ __device__ int2 make_int2(int s)
+{
+    return make_int2(s, s);
+}
+inline __host__ __device__ int2 make_int2(int3 a)
+{
+    return make_int2(a.x, a.y);
+}
+inline __host__ __device__ int2 make_int2(uint2 a)
+{
+    return make_int2(int(a.x), int(a.y));
+}
+inline __host__ __device__ int2 make_int2(float2 a)
+{
+    return make_int2(int(a.x), int(a.y));
+}
+
+inline __host__ __device__ uint2 make_uint2(uint s)
+{
+    return make_uint2(s, s);
+}
+inline __host__ __device__ uint2 make_uint2(uint3 a)
+{
+    return make_uint2(a.x, a.y);
+}
+inline __host__ __device__ uint2 make_uint2(int2 a)
+{
+    return make_uint2(uint(a.x), uint(a.y));
+}
+
+inline __host__ __device__ float3 make_float3(float s)
+{
+    return make_float3(s, s, s);
+}
+inline __host__ __device__ float3 make_float3(float2 a)
+{
+    return make_float3(a.x, a.y, 0.0f);
+}
+inline __host__ __device__ float3 make_float3(float2 a, float s)
+{
+    return make_float3(a.x, a.y, s);
+}
+inline __host__ __device__ float3 make_float3(float4 a)
+{
+    return make_float3(a.x, a.y, a.z);
+}
+inline __host__ __device__ float3 make_float3(int3 a)
+{
+    return make_float3(float(a.x), float(a.y), float(a.z));
+}
+inline __host__ __device__ float3 make_float3(uint3 a)
+{
+    return make_float3(float(a.x), float(a.y), float(a.z));
+}
+
+inline __host__ __device__ int3 make_int3(int s)
+{
+    return make_int3(s, s, s);
+}
+inline __host__ __device__ int3 make_int3(int2 a)
+{
+    return make_int3(a.x, a.y, 0);
+}
+inline __host__ __device__ int3 make_int3(int2 a, int s)
+{
+    return make_int3(a.x, a.y, s);
+}
+inline __host__ __device__ int3 make_int3(uint3 a)
+{
+    return make_int3(int(a.x), int(a.y), int(a.z));
+}
+inline __host__ __device__ int3 make_int3(float3 a)
+{
+    return make_int3(int(a.x), int(a.y), int(a.z));
+}
+
+inline __host__ __device__ uint3 make_uint3(uint s)
+{
+    return make_uint3(s, s, s);
+}
+inline __host__ __device__ uint3 make_uint3(uint2 a)
+{
+    return make_uint3(a.x, a.y, 0);
+}
+inline __host__ __device__ uint3 make_uint3(uint2 a, uint s)
+{
+    return make_uint3(a.x, a.y, s);
+}
+inline __host__ __device__ uint3 make_uint3(uint4 a)
+{
+    return make_uint3(a.x, a.y, a.z);
+}
+inline __host__ __device__ uint3 make_uint3(int3 a)
+{
+    return make_uint3(uint(a.x), uint(a.y), uint(a.z));
+}
+
+inline __host__ __device__ float4 make_float4(float s)
+{
+    return make_float4(s, s, s, s);
+}
+inline __host__ __device__ float4 make_float4(float3 a)
+{
+    return make_float4(a.x, a.y, a.z, 0.0f);
+}
+inline __host__ __device__ float4 make_float4(float3 a, float w)
+{
+    return make_float4(a.x, a.y, a.z, w);
+}
+inline __host__ __device__ float4 make_float4(int4 a)
+{
+    return make_float4(float(a.x), float(a.y), float(a.z), float(a.w));
+}
+inline __host__ __device__ float4 make_float4(uint4 a)
+{
+    return make_float4(float(a.x), float(a.y), float(a.z), float(a.w));
+}
+
+inline __host__ __device__ int4 make_int4(int s)
+{
+    return make_int4(s, s, s, s);
+}
+inline __host__ __device__ int4 make_int4(int3 a)
+{
+    return make_int4(a.x, a.y, a.z, 0);
+}
+inline __host__ __device__ int4 make_int4(int3 a, int w)
+{
+    return make_int4(a.x, a.y, a.z, w);
+}
+inline __host__ __device__ int4 make_int4(uint4 a)
+{
+    return make_int4(int(a.x), int(a.y), int(a.z), int(a.w));
+}
+inline __host__ __device__ int4 make_int4(float4 a)
+{
+    return make_int4(int(a.x), int(a.y), int(a.z), int(a.w));
+}
+
+
+inline __host__ __device__ uint4 make_uint4(uint s)
+{
+    return make_uint4(s, s, s, s);
+}
+inline __host__ __device__ uint4 make_uint4(uint3 a)
+{
+    return make_uint4(a.x, a.y, a.z, 0);
+}
+inline __host__ __device__ uint4 make_uint4(uint3 a, uint w)
+{
+    return make_uint4(a.x, a.y, a.z, w);
+}
+inline __host__ __device__ uint4 make_uint4(int4 a)
+{
+    return make_uint4(uint(a.x), uint(a.y), uint(a.z), uint(a.w));
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// negate
+////////////////////////////////////////////////////////////////////////////////
+
+inline __host__ __device__ float2 operator-(float2 &a)
+{
+    return make_float2(-a.x, -a.y);
+}
+inline __host__ __device__ int2 operator-(int2 &a)
+{
+    return make_int2(-a.x, -a.y);
+}
+inline __host__ __device__ float3 operator-(float3 &a)
+{
+    return make_float3(-a.x, -a.y, -a.z);
+}
+inline __host__ __device__ int3 operator-(int3 &a)
+{
+    return make_int3(-a.x, -a.y, -a.z);
+}
+inline __host__ __device__ float4 operator-(float4 &a)
+{
+    return make_float4(-a.x, -a.y, -a.z, -a.w);
+}
+inline __host__ __device__ int4 operator-(int4 &a)
+{
+    return make_int4(-a.x, -a.y, -a.z, -a.w);
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// addition
+////////////////////////////////////////////////////////////////////////////////
+
+inline __host__ __device__ float2 operator+(float2 a, float2 b)
+{
+    return make_float2(a.x + b.x, a.y + b.y);
+}
+inline __host__ __device__ void operator+=(float2 &a, float2 b)
+{
+    a.x += b.x;
+    a.y += b.y;
+}
+inline __host__ __device__ float2 operator+(float2 a, float b)
+{
+    return make_float2(a.x + b, a.y + b);
+}
+inline __host__ __device__ float2 operator+(float b, float2 a)
+{
+    return make_float2(a.x + b, a.y + b);
+}
+inline __host__ __device__ void operator+=(float2 &a, float b)
+{
+    a.x += b;
+    a.y += b;
+}
+
+inline __host__ __device__ int2 operator+(int2 a, int2 b)
+{
+    return make_int2(a.x + b.x, a.y + b.y);
+}
+inline __host__ __device__ void operator+=(int2 &a, int2 b)
+{
+    a.x += b.x;
+    a.y += b.y;
+}
+inline __host__ __device__ int2 operator+(int2 a, int b)
+{
+    return make_int2(a.x + b, a.y + b);
+}
+inline __host__ __device__ int2 operator+(int b, int2 a)
+{
+    return make_int2(a.x + b, a.y + b);
+}
+inline __host__ __device__ void operator+=(int2 &a, int b)
+{
+    a.x += b;
+    a.y += b;
+}
+
+inline __host__ __device__ uint2 operator+(uint2 a, uint2 b)
+{
+    return make_uint2(a.x + b.x, a.y + b.y);
+}
+inline __host__ __device__ void operator+=(uint2 &a, uint2 b)
+{
+    a.x += b.x;
+    a.y += b.y;
+}
+inline __host__ __device__ uint2 operator+(uint2 a, uint b)
+{
+    return make_uint2(a.x + b, a.y + b);
+}
+inline __host__ __device__ uint2 operator+(uint b, uint2 a)
+{
+    return make_uint2(a.x + b, a.y + b);
+}
+inline __host__ __device__ void operator+=(uint2 &a, uint b)
+{
+    a.x += b;
+    a.y += b;
+}
+
+
+inline __host__ __device__ float3 operator+(float3 a, float3 b)
+{
+    return make_float3(a.x + b.x, a.y + b.y, a.z + b.z);
+}
+inline __host__ __device__ void operator+=(float3 &a, float3 b)
+{
+    a.x += b.x;
+    a.y += b.y;
+    a.z += b.z;
+}
+inline __host__ __device__ float3 operator+(float3 a, float b)
+{
+    return make_float3(a.x + b, a.y + b, a.z + b);
+}
+inline __host__ __device__ void operator+=(float3 &a, float b)
+{
+    a.x += b;
+    a.y += b;
+    a.z += b;
+}
+
+inline __host__ __device__ int3 operator+(int3 a, int3 b)
+{
+    return make_int3(a.x + b.x, a.y + b.y, a.z + b.z);
+}
+inline __host__ __device__ void operator+=(int3 &a, int3 b)
+{
+    a.x += b.x;
+    a.y += b.y;
+    a.z += b.z;
+}
+inline __host__ __device__ int3 operator+(int3 a, int b)
+{
+    return make_int3(a.x + b, a.y + b, a.z + b);
+}
+inline __host__ __device__ void operator+=(int3 &a, int b)
+{
+    a.x += b;
+    a.y += b;
+    a.z += b;
+}
+
+inline __host__ __device__ uint3 operator+(uint3 a, uint3 b)
+{
+    return make_uint3(a.x + b.x, a.y + b.y, a.z + b.z);
+}
+inline __host__ __device__ void operator+=(uint3 &a, uint3 b)
+{
+    a.x += b.x;
+    a.y += b.y;
+    a.z += b.z;
+}
+inline __host__ __device__ uint3 operator+(uint3 a, uint b)
+{
+    return make_uint3(a.x + b, a.y + b, a.z + b);
+}
+inline __host__ __device__ void operator+=(uint3 &a, uint b)
+{
+    a.x += b;
+    a.y += b;
+    a.z += b;
+}
+
+inline __host__ __device__ int3 operator+(int b, int3 a)
+{
+    return make_int3(a.x + b, a.y + b, a.z + b);
+}
+inline __host__ __device__ uint3 operator+(uint b, uint3 a)
+{
+    return make_uint3(a.x + b, a.y + b, a.z + b);
+}
+inline __host__ __device__ float3 operator+(float b, float3 a)
+{
+    return make_float3(a.x + b, a.y + b, a.z + b);
+}
+
+inline __host__ __device__ float4 operator+(float4 a, float4 b)
+{
+    return make_float4(a.x + b.x, a.y + b.y, a.z + b.z,  a.w + b.w);
+}
+inline __host__ __device__ void operator+=(float4 &a, float4 b)
+{
+    a.x += b.x;
+    a.y += b.y;
+    a.z += b.z;
+    a.w += b.w;
+}
+inline __host__ __device__ float4 operator+(float4 a, float b)
+{
+    return make_float4(a.x + b, a.y + b, a.z + b, a.w + b);
+}
+inline __host__ __device__ float4 operator+(float b, float4 a)
+{
+    return make_float4(a.x + b, a.y + b, a.z + b, a.w + b);
+}
+inline __host__ __device__ void operator+=(float4 &a, float b)
+{
+    a.x += b;
+    a.y += b;
+    a.z += b;
+    a.w += b;
+}
+
+inline __host__ __device__ int4 operator+(int4 a, int4 b)
+{
+    return make_int4(a.x + b.x, a.y + b.y, a.z + b.z,  a.w + b.w);
+}
+inline __host__ __device__ void operator+=(int4 &a, int4 b)
+{
+    a.x += b.x;
+    a.y += b.y;
+    a.z += b.z;
+    a.w += b.w;
+}
+inline __host__ __device__ int4 operator+(int4 a, int b)
+{
+    return make_int4(a.x + b, a.y + b, a.z + b,  a.w + b);
+}
+inline __host__ __device__ int4 operator+(int b, int4 a)
+{
+    return make_int4(a.x + b, a.y + b, a.z + b,  a.w + b);
+}
+inline __host__ __device__ void operator+=(int4 &a, int b)
+{
+    a.x += b;
+    a.y += b;
+    a.z += b;
+    a.w += b;
+}
+
+inline __host__ __device__ uint4 operator+(uint4 a, uint4 b)
+{
+    return make_uint4(a.x + b.x, a.y + b.y, a.z + b.z,  a.w + b.w);
+}
+inline __host__ __device__ void operator+=(uint4 &a, uint4 b)
+{
+    a.x += b.x;
+    a.y += b.y;
+    a.z += b.z;
+    a.w += b.w;
+}
+inline __host__ __device__ uint4 operator+(uint4 a, uint b)
+{
+    return make_uint4(a.x + b, a.y + b, a.z + b,  a.w + b);
+}
+inline __host__ __device__ uint4 operator+(uint b, uint4 a)
+{
+    return make_uint4(a.x + b, a.y + b, a.z + b,  a.w + b);
+}
+inline __host__ __device__ void operator+=(uint4 &a, uint b)
+{
+    a.x += b;
+    a.y += b;
+    a.z += b;
+    a.w += b;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// subtract
+////////////////////////////////////////////////////////////////////////////////
+
+inline __host__ __device__ float2 operator-(float2 a, float2 b)
+{
+    return make_float2(a.x - b.x, a.y - b.y);
+}
+inline __host__ __device__ void operator-=(float2 &a, float2 b)
+{
+    a.x -= b.x;
+    a.y -= b.y;
+}
+inline __host__ __device__ float2 operator-(float2 a, float b)
+{
+    return make_float2(a.x - b, a.y - b);
+}
+inline __host__ __device__ float2 operator-(float b, float2 a)
+{
+    return make_float2(b - a.x, b - a.y);
+}
+inline __host__ __device__ void operator-=(float2 &a, float b)
+{
+    a.x -= b;
+    a.y -= b;
+}
+
+inline __host__ __device__ int2 operator-(int2 a, int2 b)
+{
+    return make_int2(a.x - b.x, a.y - b.y);
+}
+inline __host__ __device__ void operator-=(int2 &a, int2 b)
+{
+    a.x -= b.x;
+    a.y -= b.y;
+}
+inline __host__ __device__ int2 operator-(int2 a, int b)
+{
+    return make_int2(a.x - b, a.y - b);
+}
+inline __host__ __device__ int2 operator-(int b, int2 a)
+{
+    return make_int2(b - a.x, b - a.y);
+}
+inline __host__ __device__ void operator-=(int2 &a, int b)
+{
+    a.x -= b;
+    a.y -= b;
+}
+
+inline __host__ __device__ uint2 operator-(uint2 a, uint2 b)
+{
+    return make_uint2(a.x - b.x, a.y - b.y);
+}
+inline __host__ __device__ void operator-=(uint2 &a, uint2 b)
+{
+    a.x -= b.x;
+    a.y -= b.y;
+}
+inline __host__ __device__ uint2 operator-(uint2 a, uint b)
+{
+    return make_uint2(a.x - b, a.y - b);
+}
+inline __host__ __device__ uint2 operator-(uint b, uint2 a)
+{
+    return make_uint2(b - a.x, b - a.y);
+}
+inline __host__ __device__ void operator-=(uint2 &a, uint b)
+{
+    a.x -= b;
+    a.y -= b;
+}
+
+inline __host__ __device__ float3 operator-(float3 a, float3 b)
+{
+    return make_float3(a.x - b.x, a.y - b.y, a.z - b.z);
+}
+inline __host__ __device__ void operator-=(float3 &a, float3 b)
+{
+    a.x -= b.x;
+    a.y -= b.y;
+    a.z -= b.z;
+}
+inline __host__ __device__ float3 operator-(float3 a, float b)
+{
+    return make_float3(a.x - b, a.y - b, a.z - b);
+}
+inline __host__ __device__ float3 operator-(float b, float3 a)
+{
+    return make_float3(b - a.x, b - a.y, b - a.z);
+}
+inline __host__ __device__ void operator-=(float3 &a, float b)
+{
+    a.x -= b;
+    a.y -= b;
+    a.z -= b;
+}
+
+inline __host__ __device__ int3 operator-(int3 a, int3 b)
+{
+    return make_int3(a.x - b.x, a.y - b.y, a.z - b.z);
+}
+inline __host__ __device__ void operator-=(int3 &a, int3 b)
+{
+    a.x -= b.x;
+    a.y -= b.y;
+    a.z -= b.z;
+}
+inline __host__ __device__ int3 operator-(int3 a, int b)
+{
+    return make_int3(a.x - b, a.y - b, a.z - b);
+}
+inline __host__ __device__ int3 operator-(int b, int3 a)
+{
+    return make_int3(b - a.x, b - a.y, b - a.z);
+}
+inline __host__ __device__ void operator-=(int3 &a, int b)
+{
+    a.x -= b;
+    a.y -= b;
+    a.z -= b;
+}
+
+inline __host__ __device__ uint3 operator-(uint3 a, uint3 b)
+{
+    return make_uint3(a.x - b.x, a.y - b.y, a.z - b.z);
+}
+inline __host__ __device__ void operator-=(uint3 &a, uint3 b)
+{
+    a.x -= b.x;
+    a.y -= b.y;
+    a.z -= b.z;
+}
+inline __host__ __device__ uint3 operator-(uint3 a, uint b)
+{
+    return make_uint3(a.x - b, a.y - b, a.z - b);
+}
+inline __host__ __device__ uint3 operator-(uint b, uint3 a)
+{
+    return make_uint3(b - a.x, b - a.y, b - a.z);
+}
+inline __host__ __device__ void operator-=(uint3 &a, uint b)
+{
+    a.x -= b;
+    a.y -= b;
+    a.z -= b;
+}
+
+inline __host__ __device__ float4 operator-(float4 a, float4 b)
+{
+    return make_float4(a.x - b.x, a.y - b.y, a.z - b.z,  a.w - b.w);
+}
+inline __host__ __device__ void operator-=(float4 &a, float4 b)
+{
+    a.x -= b.x;
+    a.y -= b.y;
+    a.z -= b.z;
+    a.w -= b.w;
+}
+inline __host__ __device__ float4 operator-(float4 a, float b)
+{
+    return make_float4(a.x - b, a.y - b, a.z - b,  a.w - b);
+}
+inline __host__ __device__ void operator-=(float4 &a, float b)
+{
+    a.x -= b;
+    a.y -= b;
+    a.z -= b;
+    a.w -= b;
+}
+
+inline __host__ __device__ int4 operator-(int4 a, int4 b)
+{
+    return make_int4(a.x - b.x, a.y - b.y, a.z - b.z,  a.w - b.w);
+}
+inline __host__ __device__ void operator-=(int4 &a, int4 b)
+{
+    a.x -= b.x;
+    a.y -= b.y;
+    a.z -= b.z;
+    a.w -= b.w;
+}
+inline __host__ __device__ int4 operator-(int4 a, int b)
+{
+    return make_int4(a.x - b, a.y - b, a.z - b,  a.w - b);
+}
+inline __host__ __device__ int4 operator-(int b, int4 a)
+{
+    return make_int4(b - a.x, b - a.y, b - a.z, b - a.w);
+}
+inline __host__ __device__ void operator-=(int4 &a, int b)
+{
+    a.x -= b;
+    a.y -= b;
+    a.z -= b;
+    a.w -= b;
+}
+
+inline __host__ __device__ uint4 operator-(uint4 a, uint4 b)
+{
+    return make_uint4(a.x - b.x, a.y - b.y, a.z - b.z,  a.w - b.w);
+}
+inline __host__ __device__ void operator-=(uint4 &a, uint4 b)
+{
+    a.x -= b.x;
+    a.y -= b.y;
+    a.z -= b.z;
+    a.w -= b.w;
+}
+inline __host__ __device__ uint4 operator-(uint4 a, uint b)
+{
+    return make_uint4(a.x - b, a.y - b, a.z - b,  a.w - b);
+}
+inline __host__ __device__ uint4 operator-(uint b, uint4 a)
+{
+    return make_uint4(b - a.x, b - a.y, b - a.z, b - a.w);
+}
+inline __host__ __device__ void operator-=(uint4 &a, uint b)
+{
+    a.x -= b;
+    a.y -= b;
+    a.z -= b;
+    a.w -= b;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// multiply
+////////////////////////////////////////////////////////////////////////////////
+
+inline __host__ __device__ float2 operator*(float2 a, float2 b)
+{
+    return make_float2(a.x * b.x, a.y * b.y);
+}
+inline __host__ __device__ void operator*=(float2 &a, float2 b)
+{
+    a.x *= b.x;
+    a.y *= b.y;
+}
+inline __host__ __device__ float2 operator*(float2 a, float b)
+{
+    return make_float2(a.x * b, a.y * b);
+}
+inline __host__ __device__ float2 operator*(float b, float2 a)
+{
+    return make_float2(b * a.x, b * a.y);
+}
+inline __host__ __device__ void operator*=(float2 &a, float b)
+{
+    a.x *= b;
+    a.y *= b;
+}
+
+inline __host__ __device__ int2 operator*(int2 a, int2 b)
+{
+    return make_int2(a.x * b.x, a.y * b.y);
+}
+inline __host__ __device__ void operator*=(int2 &a, int2 b)
+{
+    a.x *= b.x;
+    a.y *= b.y;
+}
+inline __host__ __device__ int2 operator*(int2 a, int b)
+{
+    return make_int2(a.x * b, a.y * b);
+}
+inline __host__ __device__ int2 operator*(int b, int2 a)
+{
+    return make_int2(b * a.x, b * a.y);
+}
+inline __host__ __device__ void operator*=(int2 &a, int b)
+{
+    a.x *= b;
+    a.y *= b;
+}
+
+inline __host__ __device__ uint2 operator*(uint2 a, uint2 b)
+{
+    return make_uint2(a.x * b.x, a.y * b.y);
+}
+inline __host__ __device__ void operator*=(uint2 &a, uint2 b)
+{
+    a.x *= b.x;
+    a.y *= b.y;
+}
+inline __host__ __device__ uint2 operator*(uint2 a, uint b)
+{
+    return make_uint2(a.x * b, a.y * b);
+}
+inline __host__ __device__ uint2 operator*(uint b, uint2 a)
+{
+    return make_uint2(b * a.x, b * a.y);
+}
+inline __host__ __device__ void operator*=(uint2 &a, uint b)
+{
+    a.x *= b;
+    a.y *= b;
+}
+
+inline __host__ __device__ float3 operator*(float3 a, float3 b)
+{
+    return make_float3(a.x * b.x, a.y * b.y, a.z * b.z);
+}
+inline __host__ __device__ void operator*=(float3 &a, float3 b)
+{
+    a.x *= b.x;
+    a.y *= b.y;
+    a.z *= b.z;
+}
+inline __host__ __device__ float3 operator*(float3 a, float b)
+{
+    return make_float3(a.x * b, a.y * b, a.z * b);
+}
+inline __host__ __device__ float3 operator*(float b, float3 a)
+{
+    return make_float3(b * a.x, b * a.y, b * a.z);
+}
+inline __host__ __device__ void operator*=(float3 &a, float b)
+{
+    a.x *= b;
+    a.y *= b;
+    a.z *= b;
+}
+
+inline __host__ __device__ int3 operator*(int3 a, int3 b)
+{
+    return make_int3(a.x * b.x, a.y * b.y, a.z * b.z);
+}
+inline __host__ __device__ void operator*=(int3 &a, int3 b)
+{
+    a.x *= b.x;
+    a.y *= b.y;
+    a.z *= b.z;
+}
+inline __host__ __device__ int3 operator*(int3 a, int b)
+{
+    return make_int3(a.x * b, a.y * b, a.z * b);
+}
+inline __host__ __device__ int3 operator*(int b, int3 a)
+{
+    return make_int3(b * a.x, b * a.y, b * a.z);
+}
+inline __host__ __device__ void operator*=(int3 &a, int b)
+{
+    a.x *= b;
+    a.y *= b;
+    a.z *= b;
+}
+
+inline __host__ __device__ uint3 operator*(uint3 a, uint3 b)
+{
+    return make_uint3(a.x * b.x, a.y * b.y, a.z * b.z);
+}
+inline __host__ __device__ void operator*=(uint3 &a, uint3 b)
+{
+    a.x *= b.x;
+    a.y *= b.y;
+    a.z *= b.z;
+}
+inline __host__ __device__ uint3 operator*(uint3 a, uint b)
+{
+    return make_uint3(a.x * b, a.y * b, a.z * b);
+}
+inline __host__ __device__ uint3 operator*(uint b, uint3 a)
+{
+    return make_uint3(b * a.x, b * a.y, b * a.z);
+}
+inline __host__ __device__ void operator*=(uint3 &a, uint b)
+{
+    a.x *= b;
+    a.y *= b;
+    a.z *= b;
+}
+
+inline __host__ __device__ float4 operator*(float4 a, float4 b)
+{
+    return make_float4(a.x * b.x, a.y * b.y, a.z * b.z,  a.w * b.w);
+}
+inline __host__ __device__ void operator*=(float4 &a, float4 b)
+{
+    a.x *= b.x;
+    a.y *= b.y;
+    a.z *= b.z;
+    a.w *= b.w;
+}
+inline __host__ __device__ float4 operator*(float4 a, float b)
+{
+    return make_float4(a.x * b, a.y * b, a.z * b,  a.w * b);
+}
+inline __host__ __device__ float4 operator*(float b, float4 a)
+{
+    return make_float4(b * a.x, b * a.y, b * a.z, b * a.w);
+}
+inline __host__ __device__ void operator*=(float4 &a, float b)
+{
+    a.x *= b;
+    a.y *= b;
+    a.z *= b;
+    a.w *= b;
+}
+
+inline __host__ __device__ int4 operator*(int4 a, int4 b)
+{
+    return make_int4(a.x * b.x, a.y * b.y, a.z * b.z,  a.w * b.w);
+}
+inline __host__ __device__ void operator*=(int4 &a, int4 b)
+{
+    a.x *= b.x;
+    a.y *= b.y;
+    a.z *= b.z;
+    a.w *= b.w;
+}
+inline __host__ __device__ int4 operator*(int4 a, int b)
+{
+    return make_int4(a.x * b, a.y * b, a.z * b,  a.w * b);
+}
+inline __host__ __device__ int4 operator*(int b, int4 a)
+{
+    return make_int4(b * a.x, b * a.y, b * a.z, b * a.w);
+}
+inline __host__ __device__ void operator*=(int4 &a, int b)
+{
+    a.x *= b;
+    a.y *= b;
+    a.z *= b;
+    a.w *= b;
+}
+
+inline __host__ __device__ uint4 operator*(uint4 a, uint4 b)
+{
+    return make_uint4(a.x * b.x, a.y * b.y, a.z * b.z,  a.w * b.w);
+}
+inline __host__ __device__ void operator*=(uint4 &a, uint4 b)
+{
+    a.x *= b.x;
+    a.y *= b.y;
+    a.z *= b.z;
+    a.w *= b.w;
+}
+inline __host__ __device__ uint4 operator*(uint4 a, uint b)
+{
+    return make_uint4(a.x * b, a.y * b, a.z * b,  a.w * b);
+}
+inline __host__ __device__ uint4 operator*(uint b, uint4 a)
+{
+    return make_uint4(b * a.x, b * a.y, b * a.z, b * a.w);
+}
+inline __host__ __device__ void operator*=(uint4 &a, uint b)
+{
+    a.x *= b;
+    a.y *= b;
+    a.z *= b;
+    a.w *= b;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// divide
+////////////////////////////////////////////////////////////////////////////////
+
+inline __host__ __device__ float2 operator/(float2 a, float2 b)
+{
+    return make_float2(a.x / b.x, a.y / b.y);
+}
+inline __host__ __device__ void operator/=(float2 &a, float2 b)
+{
+    a.x /= b.x;
+    a.y /= b.y;
+}
+inline __host__ __device__ float2 operator/(float2 a, float b)
+{
+    return make_float2(a.x / b, a.y / b);
+}
+inline __host__ __device__ void operator/=(float2 &a, float b)
+{
+    a.x /= b;
+    a.y /= b;
+}
+inline __host__ __device__ float2 operator/(float b, float2 a)
+{
+    return make_float2(b / a.x, b / a.y);
+}
+
+inline __host__ __device__ float3 operator/(float3 a, float3 b)
+{
+    return make_float3(a.x / b.x, a.y / b.y, a.z / b.z);
+}
+inline __host__ __device__ void operator/=(float3 &a, float3 b)
+{
+    a.x /= b.x;
+    a.y /= b.y;
+    a.z /= b.z;
+}
+inline __host__ __device__ float3 operator/(float3 a, float b)
+{
+    return make_float3(a.x / b, a.y / b, a.z / b);
+}
+inline __host__ __device__ void operator/=(float3 &a, float b)
+{
+    a.x /= b;
+    a.y /= b;
+    a.z /= b;
+}
+inline __host__ __device__ float3 operator/(float b, float3 a)
+{
+    return make_float3(b / a.x, b / a.y, b / a.z);
+}
+
+inline __host__ __device__ float4 operator/(float4 a, float4 b)
+{
+    return make_float4(a.x / b.x, a.y / b.y, a.z / b.z,  a.w / b.w);
+}
+inline __host__ __device__ void operator/=(float4 &a, float4 b)
+{
+    a.x /= b.x;
+    a.y /= b.y;
+    a.z /= b.z;
+    a.w /= b.w;
+}
+inline __host__ __device__ float4 operator/(float4 a, float b)
+{
+    return make_float4(a.x / b, a.y / b, a.z / b,  a.w / b);
+}
+inline __host__ __device__ void operator/=(float4 &a, float b)
+{
+    a.x /= b;
+    a.y /= b;
+    a.z /= b;
+    a.w /= b;
+}
+inline __host__ __device__ float4 operator/(float b, float4 a)
+{
+    return make_float4(b / a.x, b / a.y, b / a.z, b / a.w);
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// min
+////////////////////////////////////////////////////////////////////////////////
+
+inline  __host__ __device__ float2 fminf(float2 a, float2 b)
+{
+    return make_float2(fminf(a.x,b.x), fminf(a.y,b.y));
+}
+inline __host__ __device__ float3 fminf(float3 a, float3 b)
+{
+    return make_float3(fminf(a.x,b.x), fminf(a.y,b.y), fminf(a.z,b.z));
+}
+inline  __host__ __device__ float4 fminf(float4 a, float4 b)
+{
+    return make_float4(fminf(a.x,b.x), fminf(a.y,b.y), fminf(a.z,b.z), fminf(a.w,b.w));
+}
+
+inline __host__ __device__ int2 min(int2 a, int2 b)
+{
+    return make_int2(min(a.x,b.x), min(a.y,b.y));
+}
+inline __host__ __device__ int3 min(int3 a, int3 b)
+{
+    return make_int3(min(a.x,b.x), min(a.y,b.y), min(a.z,b.z));
+}
+inline __host__ __device__ int4 min(int4 a, int4 b)
+{
+    return make_int4(min(a.x,b.x), min(a.y,b.y), min(a.z,b.z), min(a.w,b.w));
+}
+
+inline __host__ __device__ uint2 min(uint2 a, uint2 b)
+{
+    return make_uint2(min(a.x,b.x), min(a.y,b.y));
+}
+inline __host__ __device__ uint3 min(uint3 a, uint3 b)
+{
+    return make_uint3(min(a.x,b.x), min(a.y,b.y), min(a.z,b.z));
+}
+inline __host__ __device__ uint4 min(uint4 a, uint4 b)
+{
+    return make_uint4(min(a.x,b.x), min(a.y,b.y), min(a.z,b.z), min(a.w,b.w));
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// max
+////////////////////////////////////////////////////////////////////////////////
+
+inline __host__ __device__ float2 fmaxf(float2 a, float2 b)
+{
+    return make_float2(fmaxf(a.x,b.x), fmaxf(a.y,b.y));
+}
+inline __host__ __device__ float3 fmaxf(float3 a, float3 b)
+{
+    return make_float3(fmaxf(a.x,b.x), fmaxf(a.y,b.y), fmaxf(a.z,b.z));
+}
+inline __host__ __device__ float4 fmaxf(float4 a, float4 b)
+{
+    return make_float4(fmaxf(a.x,b.x), fmaxf(a.y,b.y), fmaxf(a.z,b.z), fmaxf(a.w,b.w));
+}
+
+inline __host__ __device__ int2 max(int2 a, int2 b)
+{
+    return make_int2(max(a.x,b.x), max(a.y,b.y));
+}
+inline __host__ __device__ int3 max(int3 a, int3 b)
+{
+    return make_int3(max(a.x,b.x), max(a.y,b.y), max(a.z,b.z));
+}
+inline __host__ __device__ int4 max(int4 a, int4 b)
+{
+    return make_int4(max(a.x,b.x), max(a.y,b.y), max(a.z,b.z), max(a.w,b.w));
+}
+
+inline __host__ __device__ uint2 max(uint2 a, uint2 b)
+{
+    return make_uint2(max(a.x,b.x), max(a.y,b.y));
+}
+inline __host__ __device__ uint3 max(uint3 a, uint3 b)
+{
+    return make_uint3(max(a.x,b.x), max(a.y,b.y), max(a.z,b.z));
+}
+inline __host__ __device__ uint4 max(uint4 a, uint4 b)
+{
+    return make_uint4(max(a.x,b.x), max(a.y,b.y), max(a.z,b.z), max(a.w,b.w));
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// lerp
+// - linear interpolation between a and b, based on value t in [0, 1] range
+////////////////////////////////////////////////////////////////////////////////
+
+inline __device__ __host__ float lerp(float a, float b, float t)
+{
+    return a + t*(b-a);
+}
+inline __device__ __host__ float2 lerp(float2 a, float2 b, float t)
+{
+    return a + t*(b-a);
+}
+inline __device__ __host__ float3 lerp(float3 a, float3 b, float t)
+{
+    return a + t*(b-a);
+}
+inline __device__ __host__ float4 lerp(float4 a, float4 b, float t)
+{
+    return a + t*(b-a);
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// clamp
+// - clamp the value v to be in the range [a, b]
+////////////////////////////////////////////////////////////////////////////////
+
+inline __device__ __host__ float clamp(float f, float a, float b)
+{
+    return fmaxf(a, fminf(f, b));
+}
+inline __device__ __host__ int clamp(int f, int a, int b)
+{
+    return max(a, min(f, b));
+}
+inline __device__ __host__ uint clamp(uint f, uint a, uint b)
+{
+    return max(a, min(f, b));
+}
+
+inline __device__ __host__ float2 clamp(float2 v, float a, float b)
+{
+    return make_float2(clamp(v.x, a, b), clamp(v.y, a, b));
+}
+inline __device__ __host__ float2 clamp(float2 v, float2 a, float2 b)
+{
+    return make_float2(clamp(v.x, a.x, b.x), clamp(v.y, a.y, b.y));
+}
+inline __device__ __host__ float3 clamp(float3 v, float a, float b)
+{
+    return make_float3(clamp(v.x, a, b), clamp(v.y, a, b), clamp(v.z, a, b));
+}
+inline __device__ __host__ float3 clamp(float3 v, float3 a, float3 b)
+{
+    return make_float3(clamp(v.x, a.x, b.x), clamp(v.y, a.y, b.y), clamp(v.z, a.z, b.z));
+}
+inline __device__ __host__ float4 clamp(float4 v, float a, float b)
+{
+    return make_float4(clamp(v.x, a, b), clamp(v.y, a, b), clamp(v.z, a, b), clamp(v.w, a, b));
+}
+inline __device__ __host__ float4 clamp(float4 v, float4 a, float4 b)
+{
+    return make_float4(clamp(v.x, a.x, b.x), clamp(v.y, a.y, b.y), clamp(v.z, a.z, b.z), clamp(v.w, a.w, b.w));
+}
+
+inline __device__ __host__ int2 clamp(int2 v, int a, int b)
+{
+    return make_int2(clamp(v.x, a, b), clamp(v.y, a, b));
+}
+inline __device__ __host__ int2 clamp(int2 v, int2 a, int2 b)
+{
+    return make_int2(clamp(v.x, a.x, b.x), clamp(v.y, a.y, b.y));
+}
+inline __device__ __host__ int3 clamp(int3 v, int a, int b)
+{
+    return make_int3(clamp(v.x, a, b), clamp(v.y, a, b), clamp(v.z, a, b));
+}
+inline __device__ __host__ int3 clamp(int3 v, int3 a, int3 b)
+{
+    return make_int3(clamp(v.x, a.x, b.x), clamp(v.y, a.y, b.y), clamp(v.z, a.z, b.z));
+}
+inline __device__ __host__ int4 clamp(int4 v, int a, int b)
+{
+    return make_int4(clamp(v.x, a, b), clamp(v.y, a, b), clamp(v.z, a, b), clamp(v.w, a, b));
+}
+inline __device__ __host__ int4 clamp(int4 v, int4 a, int4 b)
+{
+    return make_int4(clamp(v.x, a.x, b.x), clamp(v.y, a.y, b.y), clamp(v.z, a.z, b.z), clamp(v.w, a.w, b.w));
+}
+
+inline __device__ __host__ uint2 clamp(uint2 v, uint a, uint b)
+{
+    return make_uint2(clamp(v.x, a, b), clamp(v.y, a, b));
+}
+inline __device__ __host__ uint2 clamp(uint2 v, uint2 a, uint2 b)
+{
+    return make_uint2(clamp(v.x, a.x, b.x), clamp(v.y, a.y, b.y));
+}
+inline __device__ __host__ uint3 clamp(uint3 v, uint a, uint b)
+{
+    return make_uint3(clamp(v.x, a, b), clamp(v.y, a, b), clamp(v.z, a, b));
+}
+inline __device__ __host__ uint3 clamp(uint3 v, uint3 a, uint3 b)
+{
+    return make_uint3(clamp(v.x, a.x, b.x), clamp(v.y, a.y, b.y), clamp(v.z, a.z, b.z));
+}
+inline __device__ __host__ uint4 clamp(uint4 v, uint a, uint b)
+{
+    return make_uint4(clamp(v.x, a, b), clamp(v.y, a, b), clamp(v.z, a, b), clamp(v.w, a, b));
+}
+inline __device__ __host__ uint4 clamp(uint4 v, uint4 a, uint4 b)
+{
+    return make_uint4(clamp(v.x, a.x, b.x), clamp(v.y, a.y, b.y), clamp(v.z, a.z, b.z), clamp(v.w, a.w, b.w));
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// dot product
+////////////////////////////////////////////////////////////////////////////////
+
+inline __host__ __device__ float dot(float2 a, float2 b)
+{
+    return a.x * b.x + a.y * b.y;
+}
+inline __host__ __device__ float dot(float3 a, float3 b)
+{
+    return a.x * b.x + a.y * b.y + a.z * b.z;
+}
+inline __host__ __device__ float dot(float4 a, float4 b)
+{
+    return a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w;
+}
+
+inline __host__ __device__ int dot(int2 a, int2 b)
+{
+    return a.x * b.x + a.y * b.y;
+}
+inline __host__ __device__ int dot(int3 a, int3 b)
+{
+    return a.x * b.x + a.y * b.y + a.z * b.z;
+}
+inline __host__ __device__ int dot(int4 a, int4 b)
+{
+    return a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w;
+}
+
+inline __host__ __device__ uint dot(uint2 a, uint2 b)
+{
+    return a.x * b.x + a.y * b.y;
+}
+inline __host__ __device__ uint dot(uint3 a, uint3 b)
+{
+    return a.x * b.x + a.y * b.y + a.z * b.z;
+}
+inline __host__ __device__ uint dot(uint4 a, uint4 b)
+{
+    return a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// length
+////////////////////////////////////////////////////////////////////////////////
+
+inline __host__ __device__ float length(float2 v)
+{
+    return sqrtf(dot(v, v));
+}
+inline __host__ __device__ float length(float3 v)
+{
+    return sqrtf(dot(v, v));
+}
+inline __host__ __device__ float length(float4 v)
+{
+    return sqrtf(dot(v, v));
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// normalize
+////////////////////////////////////////////////////////////////////////////////
+
+inline __host__ __device__ float2 normalize(float2 v)
+{
+    float invLen = rsqrtf(dot(v, v));
+    return v * invLen;
+}
+inline __host__ __device__ float3 normalize(float3 v)
+{
+    float invLen = rsqrtf(dot(v, v));
+    return v * invLen;
+}
+inline __host__ __device__ float4 normalize(float4 v)
+{
+    float invLen = rsqrtf(dot(v, v));
+    return v * invLen;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// floor
+////////////////////////////////////////////////////////////////////////////////
+
+inline __host__ __device__ float2 floorf(float2 v)
+{
+    return make_float2(floorf(v.x), floorf(v.y));
+}
+inline __host__ __device__ float3 floorf(float3 v)
+{
+    return make_float3(floorf(v.x), floorf(v.y), floorf(v.z));
+}
+inline __host__ __device__ float4 floorf(float4 v)
+{
+    return make_float4(floorf(v.x), floorf(v.y), floorf(v.z), floorf(v.w));
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// frac - returns the fractional portion of a scalar or each vector component
+////////////////////////////////////////////////////////////////////////////////
+
+inline __host__ __device__ float fracf(float v)
+{
+    return v - floorf(v);
+}
+inline __host__ __device__ float2 fracf(float2 v)
+{
+    return make_float2(fracf(v.x), fracf(v.y));
+}
+inline __host__ __device__ float3 fracf(float3 v)
+{
+    return make_float3(fracf(v.x), fracf(v.y), fracf(v.z));
+}
+inline __host__ __device__ float4 fracf(float4 v)
+{
+    return make_float4(fracf(v.x), fracf(v.y), fracf(v.z), fracf(v.w));
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// fmod
+////////////////////////////////////////////////////////////////////////////////
+
+inline __host__ __device__ float2 fmodf(float2 a, float2 b)
+{
+    return make_float2(fmodf(a.x, b.x), fmodf(a.y, b.y));
+}
+inline __host__ __device__ float3 fmodf(float3 a, float3 b)
+{
+    return make_float3(fmodf(a.x, b.x), fmodf(a.y, b.y), fmodf(a.z, b.z));
+}
+inline __host__ __device__ float4 fmodf(float4 a, float4 b)
+{
+    return make_float4(fmodf(a.x, b.x), fmodf(a.y, b.y), fmodf(a.z, b.z), fmodf(a.w, b.w));
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// absolute value
+////////////////////////////////////////////////////////////////////////////////
+
+inline __host__ __device__ float2 fabs(float2 v)
+{
+    return make_float2(fabs(v.x), fabs(v.y));
+}
+inline __host__ __device__ float3 fabs(float3 v)
+{
+    return make_float3(fabs(v.x), fabs(v.y), fabs(v.z));
+}
+inline __host__ __device__ float4 fabs(float4 v)
+{
+    return make_float4(fabs(v.x), fabs(v.y), fabs(v.z), fabs(v.w));
+}
+
+inline __host__ __device__ int2 abs(int2 v)
+{
+    return make_int2(abs(v.x), abs(v.y));
+}
+inline __host__ __device__ int3 abs(int3 v)
+{
+    return make_int3(abs(v.x), abs(v.y), abs(v.z));
+}
+inline __host__ __device__ int4 abs(int4 v)
+{
+    return make_int4(abs(v.x), abs(v.y), abs(v.z), abs(v.w));
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// reflect
+// - returns reflection of incident ray I around surface normal N
+// - N should be normalized, reflected vector's length is equal to length of I
+////////////////////////////////////////////////////////////////////////////////
+
+inline __host__ __device__ float3 reflect(float3 i, float3 n)
+{
+    return i - 2.0f * n * dot(n,i);
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// cross product
+////////////////////////////////////////////////////////////////////////////////
+
+inline __host__ __device__ float3 cross(float3 a, float3 b)
+{
+    return make_float3(a.y*b.z - a.z*b.y, a.z*b.x - a.x*b.z, a.x*b.y - a.y*b.x);
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// smoothstep
+// - returns 0 if x < a
+// - returns 1 if x > b
+// - otherwise returns smooth interpolation between 0 and 1 based on x
+////////////////////////////////////////////////////////////////////////////////
+
+inline __device__ __host__ float smoothstep(float a, float b, float x)
+{
+    float y = clamp((x - a) / (b - a), 0.0f, 1.0f);
+    return (y*y*(3.0f - (2.0f*y)));
+}
+inline __device__ __host__ float2 smoothstep(float2 a, float2 b, float2 x)
+{
+    float2 y = clamp((x - a) / (b - a), 0.0f, 1.0f);
+    return (y*y*(make_float2(3.0f) - (make_float2(2.0f)*y)));
+}
+inline __device__ __host__ float3 smoothstep(float3 a, float3 b, float3 x)
+{
+    float3 y = clamp((x - a) / (b - a), 0.0f, 1.0f);
+    return (y*y*(make_float3(3.0f) - (make_float3(2.0f)*y)));
+}
+inline __device__ __host__ float4 smoothstep(float4 a, float4 b, float4 x)
+{
+    float4 y = clamp((x - a) / (b - a), 0.0f, 1.0f);
+    return (y*y*(make_float4(3.0f) - (make_float4(2.0f)*y)));
+}
+
+#endif
diff --git a/RWR/src/ze_oss/imp_3rdparty_cuda_toolkit/8.0/include/imp/cuda_toolkit/helper_math.hpp b/RWR/src/ze_oss/imp_3rdparty_cuda_toolkit/8.0/include/imp/cuda_toolkit/helper_math.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..9f02edb95b555cd89516751d07f94e8404f846b4
--- /dev/null
+++ b/RWR/src/ze_oss/imp_3rdparty_cuda_toolkit/8.0/include/imp/cuda_toolkit/helper_math.hpp
@@ -0,0 +1,1467 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+/*
+ *  This file implements common mathematical operations on vector types
+ *  (float3, float4 etc.) since these are not provided as standard by CUDA.
+ *
+ *  The syntax is modeled on the Cg standard library.
+ *
+ *  This is part of the Helper library includes
+ *
+ *    Thanks to Linh Hah for additions and fixes.
+ */
+
+#ifndef HELPER_MATH_H
+#define HELPER_MATH_H
+
+#include "cuda_runtime.h"
+
+typedef unsigned int uint;
+typedef unsigned short ushort;
+
+#ifndef EXIT_WAIVED
+#define EXIT_WAIVED 2
+#endif
+
+#ifndef __CUDACC__
+#include <math.h>
+
+////////////////////////////////////////////////////////////////////////////////
+// host implementations of CUDA functions
+////////////////////////////////////////////////////////////////////////////////
+
+inline float fminf(float a, float b)
+{
+    return a < b ? a : b;
+}
+
+inline float fmaxf(float a, float b)
+{
+    return a > b ? a : b;
+}
+
+inline int max(int a, int b)
+{
+    return a > b ? a : b;
+}
+
+inline int min(int a, int b)
+{
+    return a < b ? a : b;
+}
+
+inline float rsqrtf(float x)
+{
+    return 1.0f / sqrtf(x);
+}
+#endif
+
+////////////////////////////////////////////////////////////////////////////////
+// constructors
+////////////////////////////////////////////////////////////////////////////////
+
+inline __host__ __device__ float2 make_float2(float s)
+{
+    return make_float2(s, s);
+}
+inline __host__ __device__ float2 make_float2(float3 a)
+{
+    return make_float2(a.x, a.y);
+}
+inline __host__ __device__ float2 make_float2(int2 a)
+{
+    return make_float2(float(a.x), float(a.y));
+}
+inline __host__ __device__ float2 make_float2(uint2 a)
+{
+    return make_float2(float(a.x), float(a.y));
+}
+
+inline __host__ __device__ int2 make_int2(int s)
+{
+    return make_int2(s, s);
+}
+inline __host__ __device__ int2 make_int2(int3 a)
+{
+    return make_int2(a.x, a.y);
+}
+inline __host__ __device__ int2 make_int2(uint2 a)
+{
+    return make_int2(int(a.x), int(a.y));
+}
+inline __host__ __device__ int2 make_int2(float2 a)
+{
+    return make_int2(int(a.x), int(a.y));
+}
+
+inline __host__ __device__ uint2 make_uint2(uint s)
+{
+    return make_uint2(s, s);
+}
+inline __host__ __device__ uint2 make_uint2(uint3 a)
+{
+    return make_uint2(a.x, a.y);
+}
+inline __host__ __device__ uint2 make_uint2(int2 a)
+{
+    return make_uint2(uint(a.x), uint(a.y));
+}
+
+inline __host__ __device__ float3 make_float3(float s)
+{
+    return make_float3(s, s, s);
+}
+inline __host__ __device__ float3 make_float3(float2 a)
+{
+    return make_float3(a.x, a.y, 0.0f);
+}
+inline __host__ __device__ float3 make_float3(float2 a, float s)
+{
+    return make_float3(a.x, a.y, s);
+}
+inline __host__ __device__ float3 make_float3(float4 a)
+{
+    return make_float3(a.x, a.y, a.z);
+}
+inline __host__ __device__ float3 make_float3(int3 a)
+{
+    return make_float3(float(a.x), float(a.y), float(a.z));
+}
+inline __host__ __device__ float3 make_float3(uint3 a)
+{
+    return make_float3(float(a.x), float(a.y), float(a.z));
+}
+
+inline __host__ __device__ int3 make_int3(int s)
+{
+    return make_int3(s, s, s);
+}
+inline __host__ __device__ int3 make_int3(int2 a)
+{
+    return make_int3(a.x, a.y, 0);
+}
+inline __host__ __device__ int3 make_int3(int2 a, int s)
+{
+    return make_int3(a.x, a.y, s);
+}
+inline __host__ __device__ int3 make_int3(uint3 a)
+{
+    return make_int3(int(a.x), int(a.y), int(a.z));
+}
+inline __host__ __device__ int3 make_int3(float3 a)
+{
+    return make_int3(int(a.x), int(a.y), int(a.z));
+}
+
+inline __host__ __device__ uint3 make_uint3(uint s)
+{
+    return make_uint3(s, s, s);
+}
+inline __host__ __device__ uint3 make_uint3(uint2 a)
+{
+    return make_uint3(a.x, a.y, 0);
+}
+inline __host__ __device__ uint3 make_uint3(uint2 a, uint s)
+{
+    return make_uint3(a.x, a.y, s);
+}
+inline __host__ __device__ uint3 make_uint3(uint4 a)
+{
+    return make_uint3(a.x, a.y, a.z);
+}
+inline __host__ __device__ uint3 make_uint3(int3 a)
+{
+    return make_uint3(uint(a.x), uint(a.y), uint(a.z));
+}
+
+inline __host__ __device__ float4 make_float4(float s)
+{
+    return make_float4(s, s, s, s);
+}
+inline __host__ __device__ float4 make_float4(float3 a)
+{
+    return make_float4(a.x, a.y, a.z, 0.0f);
+}
+inline __host__ __device__ float4 make_float4(float3 a, float w)
+{
+    return make_float4(a.x, a.y, a.z, w);
+}
+inline __host__ __device__ float4 make_float4(int4 a)
+{
+    return make_float4(float(a.x), float(a.y), float(a.z), float(a.w));
+}
+inline __host__ __device__ float4 make_float4(uint4 a)
+{
+    return make_float4(float(a.x), float(a.y), float(a.z), float(a.w));
+}
+
+inline __host__ __device__ int4 make_int4(int s)
+{
+    return make_int4(s, s, s, s);
+}
+inline __host__ __device__ int4 make_int4(int3 a)
+{
+    return make_int4(a.x, a.y, a.z, 0);
+}
+inline __host__ __device__ int4 make_int4(int3 a, int w)
+{
+    return make_int4(a.x, a.y, a.z, w);
+}
+inline __host__ __device__ int4 make_int4(uint4 a)
+{
+    return make_int4(int(a.x), int(a.y), int(a.z), int(a.w));
+}
+inline __host__ __device__ int4 make_int4(float4 a)
+{
+    return make_int4(int(a.x), int(a.y), int(a.z), int(a.w));
+}
+
+
+inline __host__ __device__ uint4 make_uint4(uint s)
+{
+    return make_uint4(s, s, s, s);
+}
+inline __host__ __device__ uint4 make_uint4(uint3 a)
+{
+    return make_uint4(a.x, a.y, a.z, 0);
+}
+inline __host__ __device__ uint4 make_uint4(uint3 a, uint w)
+{
+    return make_uint4(a.x, a.y, a.z, w);
+}
+inline __host__ __device__ uint4 make_uint4(int4 a)
+{
+    return make_uint4(uint(a.x), uint(a.y), uint(a.z), uint(a.w));
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// negate
+////////////////////////////////////////////////////////////////////////////////
+
+inline __host__ __device__ float2 operator-(float2 &a)
+{
+    return make_float2(-a.x, -a.y);
+}
+inline __host__ __device__ int2 operator-(int2 &a)
+{
+    return make_int2(-a.x, -a.y);
+}
+inline __host__ __device__ float3 operator-(float3 &a)
+{
+    return make_float3(-a.x, -a.y, -a.z);
+}
+inline __host__ __device__ int3 operator-(int3 &a)
+{
+    return make_int3(-a.x, -a.y, -a.z);
+}
+inline __host__ __device__ float4 operator-(float4 &a)
+{
+    return make_float4(-a.x, -a.y, -a.z, -a.w);
+}
+inline __host__ __device__ int4 operator-(int4 &a)
+{
+    return make_int4(-a.x, -a.y, -a.z, -a.w);
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// addition
+////////////////////////////////////////////////////////////////////////////////
+
+inline __host__ __device__ float2 operator+(float2 a, float2 b)
+{
+    return make_float2(a.x + b.x, a.y + b.y);
+}
+inline __host__ __device__ void operator+=(float2 &a, float2 b)
+{
+    a.x += b.x;
+    a.y += b.y;
+}
+inline __host__ __device__ float2 operator+(float2 a, float b)
+{
+    return make_float2(a.x + b, a.y + b);
+}
+inline __host__ __device__ float2 operator+(float b, float2 a)
+{
+    return make_float2(a.x + b, a.y + b);
+}
+inline __host__ __device__ void operator+=(float2 &a, float b)
+{
+    a.x += b;
+    a.y += b;
+}
+
+inline __host__ __device__ int2 operator+(int2 a, int2 b)
+{
+    return make_int2(a.x + b.x, a.y + b.y);
+}
+inline __host__ __device__ void operator+=(int2 &a, int2 b)
+{
+    a.x += b.x;
+    a.y += b.y;
+}
+inline __host__ __device__ int2 operator+(int2 a, int b)
+{
+    return make_int2(a.x + b, a.y + b);
+}
+inline __host__ __device__ int2 operator+(int b, int2 a)
+{
+    return make_int2(a.x + b, a.y + b);
+}
+inline __host__ __device__ void operator+=(int2 &a, int b)
+{
+    a.x += b;
+    a.y += b;
+}
+
+inline __host__ __device__ uint2 operator+(uint2 a, uint2 b)
+{
+    return make_uint2(a.x + b.x, a.y + b.y);
+}
+inline __host__ __device__ void operator+=(uint2 &a, uint2 b)
+{
+    a.x += b.x;
+    a.y += b.y;
+}
+inline __host__ __device__ uint2 operator+(uint2 a, uint b)
+{
+    return make_uint2(a.x + b, a.y + b);
+}
+inline __host__ __device__ uint2 operator+(uint b, uint2 a)
+{
+    return make_uint2(a.x + b, a.y + b);
+}
+inline __host__ __device__ void operator+=(uint2 &a, uint b)
+{
+    a.x += b;
+    a.y += b;
+}
+
+
+inline __host__ __device__ float3 operator+(float3 a, float3 b)
+{
+    return make_float3(a.x + b.x, a.y + b.y, a.z + b.z);
+}
+inline __host__ __device__ void operator+=(float3 &a, float3 b)
+{
+    a.x += b.x;
+    a.y += b.y;
+    a.z += b.z;
+}
+inline __host__ __device__ float3 operator+(float3 a, float b)
+{
+    return make_float3(a.x + b, a.y + b, a.z + b);
+}
+inline __host__ __device__ void operator+=(float3 &a, float b)
+{
+    a.x += b;
+    a.y += b;
+    a.z += b;
+}
+
+inline __host__ __device__ int3 operator+(int3 a, int3 b)
+{
+    return make_int3(a.x + b.x, a.y + b.y, a.z + b.z);
+}
+inline __host__ __device__ void operator+=(int3 &a, int3 b)
+{
+    a.x += b.x;
+    a.y += b.y;
+    a.z += b.z;
+}
+inline __host__ __device__ int3 operator+(int3 a, int b)
+{
+    return make_int3(a.x + b, a.y + b, a.z + b);
+}
+inline __host__ __device__ void operator+=(int3 &a, int b)
+{
+    a.x += b;
+    a.y += b;
+    a.z += b;
+}
+
+inline __host__ __device__ uint3 operator+(uint3 a, uint3 b)
+{
+    return make_uint3(a.x + b.x, a.y + b.y, a.z + b.z);
+}
+inline __host__ __device__ void operator+=(uint3 &a, uint3 b)
+{
+    a.x += b.x;
+    a.y += b.y;
+    a.z += b.z;
+}
+inline __host__ __device__ uint3 operator+(uint3 a, uint b)
+{
+    return make_uint3(a.x + b, a.y + b, a.z + b);
+}
+inline __host__ __device__ void operator+=(uint3 &a, uint b)
+{
+    a.x += b;
+    a.y += b;
+    a.z += b;
+}
+
+inline __host__ __device__ int3 operator+(int b, int3 a)
+{
+    return make_int3(a.x + b, a.y + b, a.z + b);
+}
+inline __host__ __device__ uint3 operator+(uint b, uint3 a)
+{
+    return make_uint3(a.x + b, a.y + b, a.z + b);
+}
+inline __host__ __device__ float3 operator+(float b, float3 a)
+{
+    return make_float3(a.x + b, a.y + b, a.z + b);
+}
+
+inline __host__ __device__ float4 operator+(float4 a, float4 b)
+{
+    return make_float4(a.x + b.x, a.y + b.y, a.z + b.z,  a.w + b.w);
+}
+inline __host__ __device__ void operator+=(float4 &a, float4 b)
+{
+    a.x += b.x;
+    a.y += b.y;
+    a.z += b.z;
+    a.w += b.w;
+}
+inline __host__ __device__ float4 operator+(float4 a, float b)
+{
+    return make_float4(a.x + b, a.y + b, a.z + b, a.w + b);
+}
+inline __host__ __device__ float4 operator+(float b, float4 a)
+{
+    return make_float4(a.x + b, a.y + b, a.z + b, a.w + b);
+}
+inline __host__ __device__ void operator+=(float4 &a, float b)
+{
+    a.x += b;
+    a.y += b;
+    a.z += b;
+    a.w += b;
+}
+
+inline __host__ __device__ int4 operator+(int4 a, int4 b)
+{
+    return make_int4(a.x + b.x, a.y + b.y, a.z + b.z,  a.w + b.w);
+}
+inline __host__ __device__ void operator+=(int4 &a, int4 b)
+{
+    a.x += b.x;
+    a.y += b.y;
+    a.z += b.z;
+    a.w += b.w;
+}
+inline __host__ __device__ int4 operator+(int4 a, int b)
+{
+    return make_int4(a.x + b, a.y + b, a.z + b,  a.w + b);
+}
+inline __host__ __device__ int4 operator+(int b, int4 a)
+{
+    return make_int4(a.x + b, a.y + b, a.z + b,  a.w + b);
+}
+inline __host__ __device__ void operator+=(int4 &a, int b)
+{
+    a.x += b;
+    a.y += b;
+    a.z += b;
+    a.w += b;
+}
+
+inline __host__ __device__ uint4 operator+(uint4 a, uint4 b)
+{
+    return make_uint4(a.x + b.x, a.y + b.y, a.z + b.z,  a.w + b.w);
+}
+inline __host__ __device__ void operator+=(uint4 &a, uint4 b)
+{
+    a.x += b.x;
+    a.y += b.y;
+    a.z += b.z;
+    a.w += b.w;
+}
+inline __host__ __device__ uint4 operator+(uint4 a, uint b)
+{
+    return make_uint4(a.x + b, a.y + b, a.z + b,  a.w + b);
+}
+inline __host__ __device__ uint4 operator+(uint b, uint4 a)
+{
+    return make_uint4(a.x + b, a.y + b, a.z + b,  a.w + b);
+}
+inline __host__ __device__ void operator+=(uint4 &a, uint b)
+{
+    a.x += b;
+    a.y += b;
+    a.z += b;
+    a.w += b;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// subtract
+////////////////////////////////////////////////////////////////////////////////
+
+inline __host__ __device__ float2 operator-(float2 a, float2 b)
+{
+    return make_float2(a.x - b.x, a.y - b.y);
+}
+inline __host__ __device__ void operator-=(float2 &a, float2 b)
+{
+    a.x -= b.x;
+    a.y -= b.y;
+}
+inline __host__ __device__ float2 operator-(float2 a, float b)
+{
+    return make_float2(a.x - b, a.y - b);
+}
+inline __host__ __device__ float2 operator-(float b, float2 a)
+{
+    return make_float2(b - a.x, b - a.y);
+}
+inline __host__ __device__ void operator-=(float2 &a, float b)
+{
+    a.x -= b;
+    a.y -= b;
+}
+
+inline __host__ __device__ int2 operator-(int2 a, int2 b)
+{
+    return make_int2(a.x - b.x, a.y - b.y);
+}
+inline __host__ __device__ void operator-=(int2 &a, int2 b)
+{
+    a.x -= b.x;
+    a.y -= b.y;
+}
+inline __host__ __device__ int2 operator-(int2 a, int b)
+{
+    return make_int2(a.x - b, a.y - b);
+}
+inline __host__ __device__ int2 operator-(int b, int2 a)
+{
+    return make_int2(b - a.x, b - a.y);
+}
+inline __host__ __device__ void operator-=(int2 &a, int b)
+{
+    a.x -= b;
+    a.y -= b;
+}
+
+inline __host__ __device__ uint2 operator-(uint2 a, uint2 b)
+{
+    return make_uint2(a.x - b.x, a.y - b.y);
+}
+inline __host__ __device__ void operator-=(uint2 &a, uint2 b)
+{
+    a.x -= b.x;
+    a.y -= b.y;
+}
+inline __host__ __device__ uint2 operator-(uint2 a, uint b)
+{
+    return make_uint2(a.x - b, a.y - b);
+}
+inline __host__ __device__ uint2 operator-(uint b, uint2 a)
+{
+    return make_uint2(b - a.x, b - a.y);
+}
+inline __host__ __device__ void operator-=(uint2 &a, uint b)
+{
+    a.x -= b;
+    a.y -= b;
+}
+
+inline __host__ __device__ float3 operator-(float3 a, float3 b)
+{
+    return make_float3(a.x - b.x, a.y - b.y, a.z - b.z);
+}
+inline __host__ __device__ void operator-=(float3 &a, float3 b)
+{
+    a.x -= b.x;
+    a.y -= b.y;
+    a.z -= b.z;
+}
+inline __host__ __device__ float3 operator-(float3 a, float b)
+{
+    return make_float3(a.x - b, a.y - b, a.z - b);
+}
+inline __host__ __device__ float3 operator-(float b, float3 a)
+{
+    return make_float3(b - a.x, b - a.y, b - a.z);
+}
+inline __host__ __device__ void operator-=(float3 &a, float b)
+{
+    a.x -= b;
+    a.y -= b;
+    a.z -= b;
+}
+
+inline __host__ __device__ int3 operator-(int3 a, int3 b)
+{
+    return make_int3(a.x - b.x, a.y - b.y, a.z - b.z);
+}
+inline __host__ __device__ void operator-=(int3 &a, int3 b)
+{
+    a.x -= b.x;
+    a.y -= b.y;
+    a.z -= b.z;
+}
+inline __host__ __device__ int3 operator-(int3 a, int b)
+{
+    return make_int3(a.x - b, a.y - b, a.z - b);
+}
+inline __host__ __device__ int3 operator-(int b, int3 a)
+{
+    return make_int3(b - a.x, b - a.y, b - a.z);
+}
+inline __host__ __device__ void operator-=(int3 &a, int b)
+{
+    a.x -= b;
+    a.y -= b;
+    a.z -= b;
+}
+
+inline __host__ __device__ uint3 operator-(uint3 a, uint3 b)
+{
+    return make_uint3(a.x - b.x, a.y - b.y, a.z - b.z);
+}
+inline __host__ __device__ void operator-=(uint3 &a, uint3 b)
+{
+    a.x -= b.x;
+    a.y -= b.y;
+    a.z -= b.z;
+}
+inline __host__ __device__ uint3 operator-(uint3 a, uint b)
+{
+    return make_uint3(a.x - b, a.y - b, a.z - b);
+}
+inline __host__ __device__ uint3 operator-(uint b, uint3 a)
+{
+    return make_uint3(b - a.x, b - a.y, b - a.z);
+}
+inline __host__ __device__ void operator-=(uint3 &a, uint b)
+{
+    a.x -= b;
+    a.y -= b;
+    a.z -= b;
+}
+
+inline __host__ __device__ float4 operator-(float4 a, float4 b)
+{
+    return make_float4(a.x - b.x, a.y - b.y, a.z - b.z,  a.w - b.w);
+}
+inline __host__ __device__ void operator-=(float4 &a, float4 b)
+{
+    a.x -= b.x;
+    a.y -= b.y;
+    a.z -= b.z;
+    a.w -= b.w;
+}
+inline __host__ __device__ float4 operator-(float4 a, float b)
+{
+    return make_float4(a.x - b, a.y - b, a.z - b,  a.w - b);
+}
+inline __host__ __device__ void operator-=(float4 &a, float b)
+{
+    a.x -= b;
+    a.y -= b;
+    a.z -= b;
+    a.w -= b;
+}
+
+inline __host__ __device__ int4 operator-(int4 a, int4 b)
+{
+    return make_int4(a.x - b.x, a.y - b.y, a.z - b.z,  a.w - b.w);
+}
+inline __host__ __device__ void operator-=(int4 &a, int4 b)
+{
+    a.x -= b.x;
+    a.y -= b.y;
+    a.z -= b.z;
+    a.w -= b.w;
+}
+inline __host__ __device__ int4 operator-(int4 a, int b)
+{
+    return make_int4(a.x - b, a.y - b, a.z - b,  a.w - b);
+}
+inline __host__ __device__ int4 operator-(int b, int4 a)
+{
+    return make_int4(b - a.x, b - a.y, b - a.z, b - a.w);
+}
+inline __host__ __device__ void operator-=(int4 &a, int b)
+{
+    a.x -= b;
+    a.y -= b;
+    a.z -= b;
+    a.w -= b;
+}
+
+inline __host__ __device__ uint4 operator-(uint4 a, uint4 b)
+{
+    return make_uint4(a.x - b.x, a.y - b.y, a.z - b.z,  a.w - b.w);
+}
+inline __host__ __device__ void operator-=(uint4 &a, uint4 b)
+{
+    a.x -= b.x;
+    a.y -= b.y;
+    a.z -= b.z;
+    a.w -= b.w;
+}
+inline __host__ __device__ uint4 operator-(uint4 a, uint b)
+{
+    return make_uint4(a.x - b, a.y - b, a.z - b,  a.w - b);
+}
+inline __host__ __device__ uint4 operator-(uint b, uint4 a)
+{
+    return make_uint4(b - a.x, b - a.y, b - a.z, b - a.w);
+}
+inline __host__ __device__ void operator-=(uint4 &a, uint b)
+{
+    a.x -= b;
+    a.y -= b;
+    a.z -= b;
+    a.w -= b;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// multiply
+////////////////////////////////////////////////////////////////////////////////
+
+inline __host__ __device__ float2 operator*(float2 a, float2 b)
+{
+    return make_float2(a.x * b.x, a.y * b.y);
+}
+inline __host__ __device__ void operator*=(float2 &a, float2 b)
+{
+    a.x *= b.x;
+    a.y *= b.y;
+}
+inline __host__ __device__ float2 operator*(float2 a, float b)
+{
+    return make_float2(a.x * b, a.y * b);
+}
+inline __host__ __device__ float2 operator*(float b, float2 a)
+{
+    return make_float2(b * a.x, b * a.y);
+}
+inline __host__ __device__ void operator*=(float2 &a, float b)
+{
+    a.x *= b;
+    a.y *= b;
+}
+
+inline __host__ __device__ int2 operator*(int2 a, int2 b)
+{
+    return make_int2(a.x * b.x, a.y * b.y);
+}
+inline __host__ __device__ void operator*=(int2 &a, int2 b)
+{
+    a.x *= b.x;
+    a.y *= b.y;
+}
+inline __host__ __device__ int2 operator*(int2 a, int b)
+{
+    return make_int2(a.x * b, a.y * b);
+}
+inline __host__ __device__ int2 operator*(int b, int2 a)
+{
+    return make_int2(b * a.x, b * a.y);
+}
+inline __host__ __device__ void operator*=(int2 &a, int b)
+{
+    a.x *= b;
+    a.y *= b;
+}
+
+inline __host__ __device__ uint2 operator*(uint2 a, uint2 b)
+{
+    return make_uint2(a.x * b.x, a.y * b.y);
+}
+inline __host__ __device__ void operator*=(uint2 &a, uint2 b)
+{
+    a.x *= b.x;
+    a.y *= b.y;
+}
+inline __host__ __device__ uint2 operator*(uint2 a, uint b)
+{
+    return make_uint2(a.x * b, a.y * b);
+}
+inline __host__ __device__ uint2 operator*(uint b, uint2 a)
+{
+    return make_uint2(b * a.x, b * a.y);
+}
+inline __host__ __device__ void operator*=(uint2 &a, uint b)
+{
+    a.x *= b;
+    a.y *= b;
+}
+
+inline __host__ __device__ float3 operator*(float3 a, float3 b)
+{
+    return make_float3(a.x * b.x, a.y * b.y, a.z * b.z);
+}
+inline __host__ __device__ void operator*=(float3 &a, float3 b)
+{
+    a.x *= b.x;
+    a.y *= b.y;
+    a.z *= b.z;
+}
+inline __host__ __device__ float3 operator*(float3 a, float b)
+{
+    return make_float3(a.x * b, a.y * b, a.z * b);
+}
+inline __host__ __device__ float3 operator*(float b, float3 a)
+{
+    return make_float3(b * a.x, b * a.y, b * a.z);
+}
+inline __host__ __device__ void operator*=(float3 &a, float b)
+{
+    a.x *= b;
+    a.y *= b;
+    a.z *= b;
+}
+
+inline __host__ __device__ int3 operator*(int3 a, int3 b)
+{
+    return make_int3(a.x * b.x, a.y * b.y, a.z * b.z);
+}
+inline __host__ __device__ void operator*=(int3 &a, int3 b)
+{
+    a.x *= b.x;
+    a.y *= b.y;
+    a.z *= b.z;
+}
+inline __host__ __device__ int3 operator*(int3 a, int b)
+{
+    return make_int3(a.x * b, a.y * b, a.z * b);
+}
+inline __host__ __device__ int3 operator*(int b, int3 a)
+{
+    return make_int3(b * a.x, b * a.y, b * a.z);
+}
+inline __host__ __device__ void operator*=(int3 &a, int b)
+{
+    a.x *= b;
+    a.y *= b;
+    a.z *= b;
+}
+
+inline __host__ __device__ uint3 operator*(uint3 a, uint3 b)
+{
+    return make_uint3(a.x * b.x, a.y * b.y, a.z * b.z);
+}
+inline __host__ __device__ void operator*=(uint3 &a, uint3 b)
+{
+    a.x *= b.x;
+    a.y *= b.y;
+    a.z *= b.z;
+}
+inline __host__ __device__ uint3 operator*(uint3 a, uint b)
+{
+    return make_uint3(a.x * b, a.y * b, a.z * b);
+}
+inline __host__ __device__ uint3 operator*(uint b, uint3 a)
+{
+    return make_uint3(b * a.x, b * a.y, b * a.z);
+}
+inline __host__ __device__ void operator*=(uint3 &a, uint b)
+{
+    a.x *= b;
+    a.y *= b;
+    a.z *= b;
+}
+
+inline __host__ __device__ float4 operator*(float4 a, float4 b)
+{
+    return make_float4(a.x * b.x, a.y * b.y, a.z * b.z,  a.w * b.w);
+}
+inline __host__ __device__ void operator*=(float4 &a, float4 b)
+{
+    a.x *= b.x;
+    a.y *= b.y;
+    a.z *= b.z;
+    a.w *= b.w;
+}
+inline __host__ __device__ float4 operator*(float4 a, float b)
+{
+    return make_float4(a.x * b, a.y * b, a.z * b,  a.w * b);
+}
+inline __host__ __device__ float4 operator*(float b, float4 a)
+{
+    return make_float4(b * a.x, b * a.y, b * a.z, b * a.w);
+}
+inline __host__ __device__ void operator*=(float4 &a, float b)
+{
+    a.x *= b;
+    a.y *= b;
+    a.z *= b;
+    a.w *= b;
+}
+
+inline __host__ __device__ int4 operator*(int4 a, int4 b)
+{
+    return make_int4(a.x * b.x, a.y * b.y, a.z * b.z,  a.w * b.w);
+}
+inline __host__ __device__ void operator*=(int4 &a, int4 b)
+{
+    a.x *= b.x;
+    a.y *= b.y;
+    a.z *= b.z;
+    a.w *= b.w;
+}
+inline __host__ __device__ int4 operator*(int4 a, int b)
+{
+    return make_int4(a.x * b, a.y * b, a.z * b,  a.w * b);
+}
+inline __host__ __device__ int4 operator*(int b, int4 a)
+{
+    return make_int4(b * a.x, b * a.y, b * a.z, b * a.w);
+}
+inline __host__ __device__ void operator*=(int4 &a, int b)
+{
+    a.x *= b;
+    a.y *= b;
+    a.z *= b;
+    a.w *= b;
+}
+
+inline __host__ __device__ uint4 operator*(uint4 a, uint4 b)
+{
+    return make_uint4(a.x * b.x, a.y * b.y, a.z * b.z,  a.w * b.w);
+}
+inline __host__ __device__ void operator*=(uint4 &a, uint4 b)
+{
+    a.x *= b.x;
+    a.y *= b.y;
+    a.z *= b.z;
+    a.w *= b.w;
+}
+inline __host__ __device__ uint4 operator*(uint4 a, uint b)
+{
+    return make_uint4(a.x * b, a.y * b, a.z * b,  a.w * b);
+}
+inline __host__ __device__ uint4 operator*(uint b, uint4 a)
+{
+    return make_uint4(b * a.x, b * a.y, b * a.z, b * a.w);
+}
+inline __host__ __device__ void operator*=(uint4 &a, uint b)
+{
+    a.x *= b;
+    a.y *= b;
+    a.z *= b;
+    a.w *= b;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// divide
+////////////////////////////////////////////////////////////////////////////////
+
+inline __host__ __device__ float2 operator/(float2 a, float2 b)
+{
+    return make_float2(a.x / b.x, a.y / b.y);
+}
+inline __host__ __device__ void operator/=(float2 &a, float2 b)
+{
+    a.x /= b.x;
+    a.y /= b.y;
+}
+inline __host__ __device__ float2 operator/(float2 a, float b)
+{
+    return make_float2(a.x / b, a.y / b);
+}
+inline __host__ __device__ void operator/=(float2 &a, float b)
+{
+    a.x /= b;
+    a.y /= b;
+}
+inline __host__ __device__ float2 operator/(float b, float2 a)
+{
+    return make_float2(b / a.x, b / a.y);
+}
+
+inline __host__ __device__ float3 operator/(float3 a, float3 b)
+{
+    return make_float3(a.x / b.x, a.y / b.y, a.z / b.z);
+}
+inline __host__ __device__ void operator/=(float3 &a, float3 b)
+{
+    a.x /= b.x;
+    a.y /= b.y;
+    a.z /= b.z;
+}
+inline __host__ __device__ float3 operator/(float3 a, float b)
+{
+    return make_float3(a.x / b, a.y / b, a.z / b);
+}
+inline __host__ __device__ void operator/=(float3 &a, float b)
+{
+    a.x /= b;
+    a.y /= b;
+    a.z /= b;
+}
+inline __host__ __device__ float3 operator/(float b, float3 a)
+{
+    return make_float3(b / a.x, b / a.y, b / a.z);
+}
+
+inline __host__ __device__ float4 operator/(float4 a, float4 b)
+{
+    return make_float4(a.x / b.x, a.y / b.y, a.z / b.z,  a.w / b.w);
+}
+inline __host__ __device__ void operator/=(float4 &a, float4 b)
+{
+    a.x /= b.x;
+    a.y /= b.y;
+    a.z /= b.z;
+    a.w /= b.w;
+}
+inline __host__ __device__ float4 operator/(float4 a, float b)
+{
+    return make_float4(a.x / b, a.y / b, a.z / b,  a.w / b);
+}
+inline __host__ __device__ void operator/=(float4 &a, float b)
+{
+    a.x /= b;
+    a.y /= b;
+    a.z /= b;
+    a.w /= b;
+}
+inline __host__ __device__ float4 operator/(float b, float4 a)
+{
+    return make_float4(b / a.x, b / a.y, b / a.z, b / a.w);
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// min
+////////////////////////////////////////////////////////////////////////////////
+
+inline  __host__ __device__ float2 fminf(float2 a, float2 b)
+{
+    return make_float2(fminf(a.x,b.x), fminf(a.y,b.y));
+}
+inline __host__ __device__ float3 fminf(float3 a, float3 b)
+{
+    return make_float3(fminf(a.x,b.x), fminf(a.y,b.y), fminf(a.z,b.z));
+}
+inline  __host__ __device__ float4 fminf(float4 a, float4 b)
+{
+    return make_float4(fminf(a.x,b.x), fminf(a.y,b.y), fminf(a.z,b.z), fminf(a.w,b.w));
+}
+
+inline __host__ __device__ int2 min(int2 a, int2 b)
+{
+    return make_int2(min(a.x,b.x), min(a.y,b.y));
+}
+inline __host__ __device__ int3 min(int3 a, int3 b)
+{
+    return make_int3(min(a.x,b.x), min(a.y,b.y), min(a.z,b.z));
+}
+inline __host__ __device__ int4 min(int4 a, int4 b)
+{
+    return make_int4(min(a.x,b.x), min(a.y,b.y), min(a.z,b.z), min(a.w,b.w));
+}
+
+inline __host__ __device__ uint2 min(uint2 a, uint2 b)
+{
+    return make_uint2(min(a.x,b.x), min(a.y,b.y));
+}
+inline __host__ __device__ uint3 min(uint3 a, uint3 b)
+{
+    return make_uint3(min(a.x,b.x), min(a.y,b.y), min(a.z,b.z));
+}
+inline __host__ __device__ uint4 min(uint4 a, uint4 b)
+{
+    return make_uint4(min(a.x,b.x), min(a.y,b.y), min(a.z,b.z), min(a.w,b.w));
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// max
+////////////////////////////////////////////////////////////////////////////////
+
+inline __host__ __device__ float2 fmaxf(float2 a, float2 b)
+{
+    return make_float2(fmaxf(a.x,b.x), fmaxf(a.y,b.y));
+}
+inline __host__ __device__ float3 fmaxf(float3 a, float3 b)
+{
+    return make_float3(fmaxf(a.x,b.x), fmaxf(a.y,b.y), fmaxf(a.z,b.z));
+}
+inline __host__ __device__ float4 fmaxf(float4 a, float4 b)
+{
+    return make_float4(fmaxf(a.x,b.x), fmaxf(a.y,b.y), fmaxf(a.z,b.z), fmaxf(a.w,b.w));
+}
+
+inline __host__ __device__ int2 max(int2 a, int2 b)
+{
+    return make_int2(max(a.x,b.x), max(a.y,b.y));
+}
+inline __host__ __device__ int3 max(int3 a, int3 b)
+{
+    return make_int3(max(a.x,b.x), max(a.y,b.y), max(a.z,b.z));
+}
+inline __host__ __device__ int4 max(int4 a, int4 b)
+{
+    return make_int4(max(a.x,b.x), max(a.y,b.y), max(a.z,b.z), max(a.w,b.w));
+}
+
+inline __host__ __device__ uint2 max(uint2 a, uint2 b)
+{
+    return make_uint2(max(a.x,b.x), max(a.y,b.y));
+}
+inline __host__ __device__ uint3 max(uint3 a, uint3 b)
+{
+    return make_uint3(max(a.x,b.x), max(a.y,b.y), max(a.z,b.z));
+}
+inline __host__ __device__ uint4 max(uint4 a, uint4 b)
+{
+    return make_uint4(max(a.x,b.x), max(a.y,b.y), max(a.z,b.z), max(a.w,b.w));
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// lerp
+// - linear interpolation between a and b, based on value t in [0, 1] range
+////////////////////////////////////////////////////////////////////////////////
+
+inline __device__ __host__ float lerp(float a, float b, float t)
+{
+    return a + t*(b-a);
+}
+inline __device__ __host__ float2 lerp(float2 a, float2 b, float t)
+{
+    return a + t*(b-a);
+}
+inline __device__ __host__ float3 lerp(float3 a, float3 b, float t)
+{
+    return a + t*(b-a);
+}
+inline __device__ __host__ float4 lerp(float4 a, float4 b, float t)
+{
+    return a + t*(b-a);
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// clamp
+// - clamp the value v to be in the range [a, b]
+////////////////////////////////////////////////////////////////////////////////
+
+inline __device__ __host__ float clamp(float f, float a, float b)
+{
+    return fmaxf(a, fminf(f, b));
+}
+inline __device__ __host__ int clamp(int f, int a, int b)
+{
+    return max(a, min(f, b));
+}
+inline __device__ __host__ uint clamp(uint f, uint a, uint b)
+{
+    return max(a, min(f, b));
+}
+
+inline __device__ __host__ float2 clamp(float2 v, float a, float b)
+{
+    return make_float2(clamp(v.x, a, b), clamp(v.y, a, b));
+}
+inline __device__ __host__ float2 clamp(float2 v, float2 a, float2 b)
+{
+    return make_float2(clamp(v.x, a.x, b.x), clamp(v.y, a.y, b.y));
+}
+inline __device__ __host__ float3 clamp(float3 v, float a, float b)
+{
+    return make_float3(clamp(v.x, a, b), clamp(v.y, a, b), clamp(v.z, a, b));
+}
+inline __device__ __host__ float3 clamp(float3 v, float3 a, float3 b)
+{
+    return make_float3(clamp(v.x, a.x, b.x), clamp(v.y, a.y, b.y), clamp(v.z, a.z, b.z));
+}
+inline __device__ __host__ float4 clamp(float4 v, float a, float b)
+{
+    return make_float4(clamp(v.x, a, b), clamp(v.y, a, b), clamp(v.z, a, b), clamp(v.w, a, b));
+}
+inline __device__ __host__ float4 clamp(float4 v, float4 a, float4 b)
+{
+    return make_float4(clamp(v.x, a.x, b.x), clamp(v.y, a.y, b.y), clamp(v.z, a.z, b.z), clamp(v.w, a.w, b.w));
+}
+
+inline __device__ __host__ int2 clamp(int2 v, int a, int b)
+{
+    return make_int2(clamp(v.x, a, b), clamp(v.y, a, b));
+}
+inline __device__ __host__ int2 clamp(int2 v, int2 a, int2 b)
+{
+    return make_int2(clamp(v.x, a.x, b.x), clamp(v.y, a.y, b.y));
+}
+inline __device__ __host__ int3 clamp(int3 v, int a, int b)
+{
+    return make_int3(clamp(v.x, a, b), clamp(v.y, a, b), clamp(v.z, a, b));
+}
+inline __device__ __host__ int3 clamp(int3 v, int3 a, int3 b)
+{
+    return make_int3(clamp(v.x, a.x, b.x), clamp(v.y, a.y, b.y), clamp(v.z, a.z, b.z));
+}
+inline __device__ __host__ int4 clamp(int4 v, int a, int b)
+{
+    return make_int4(clamp(v.x, a, b), clamp(v.y, a, b), clamp(v.z, a, b), clamp(v.w, a, b));
+}
+inline __device__ __host__ int4 clamp(int4 v, int4 a, int4 b)
+{
+    return make_int4(clamp(v.x, a.x, b.x), clamp(v.y, a.y, b.y), clamp(v.z, a.z, b.z), clamp(v.w, a.w, b.w));
+}
+
+inline __device__ __host__ uint2 clamp(uint2 v, uint a, uint b)
+{
+    return make_uint2(clamp(v.x, a, b), clamp(v.y, a, b));
+}
+inline __device__ __host__ uint2 clamp(uint2 v, uint2 a, uint2 b)
+{
+    return make_uint2(clamp(v.x, a.x, b.x), clamp(v.y, a.y, b.y));
+}
+inline __device__ __host__ uint3 clamp(uint3 v, uint a, uint b)
+{
+    return make_uint3(clamp(v.x, a, b), clamp(v.y, a, b), clamp(v.z, a, b));
+}
+inline __device__ __host__ uint3 clamp(uint3 v, uint3 a, uint3 b)
+{
+    return make_uint3(clamp(v.x, a.x, b.x), clamp(v.y, a.y, b.y), clamp(v.z, a.z, b.z));
+}
+inline __device__ __host__ uint4 clamp(uint4 v, uint a, uint b)
+{
+    return make_uint4(clamp(v.x, a, b), clamp(v.y, a, b), clamp(v.z, a, b), clamp(v.w, a, b));
+}
+inline __device__ __host__ uint4 clamp(uint4 v, uint4 a, uint4 b)
+{
+    return make_uint4(clamp(v.x, a.x, b.x), clamp(v.y, a.y, b.y), clamp(v.z, a.z, b.z), clamp(v.w, a.w, b.w));
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// dot product
+////////////////////////////////////////////////////////////////////////////////
+
+inline __host__ __device__ float dot(float2 a, float2 b)
+{
+    return a.x * b.x + a.y * b.y;
+}
+inline __host__ __device__ float dot(float3 a, float3 b)
+{
+    return a.x * b.x + a.y * b.y + a.z * b.z;
+}
+inline __host__ __device__ float dot(float4 a, float4 b)
+{
+    return a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w;
+}
+
+inline __host__ __device__ int dot(int2 a, int2 b)
+{
+    return a.x * b.x + a.y * b.y;
+}
+inline __host__ __device__ int dot(int3 a, int3 b)
+{
+    return a.x * b.x + a.y * b.y + a.z * b.z;
+}
+inline __host__ __device__ int dot(int4 a, int4 b)
+{
+    return a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w;
+}
+
+inline __host__ __device__ uint dot(uint2 a, uint2 b)
+{
+    return a.x * b.x + a.y * b.y;
+}
+inline __host__ __device__ uint dot(uint3 a, uint3 b)
+{
+    return a.x * b.x + a.y * b.y + a.z * b.z;
+}
+inline __host__ __device__ uint dot(uint4 a, uint4 b)
+{
+    return a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// length
+////////////////////////////////////////////////////////////////////////////////
+
+inline __host__ __device__ float length(float2 v)
+{
+    return sqrtf(dot(v, v));
+}
+inline __host__ __device__ float length(float3 v)
+{
+    return sqrtf(dot(v, v));
+}
+inline __host__ __device__ float length(float4 v)
+{
+    return sqrtf(dot(v, v));
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// normalize
+////////////////////////////////////////////////////////////////////////////////
+
+inline __host__ __device__ float2 normalize(float2 v)
+{
+    float invLen = rsqrtf(dot(v, v));
+    return v * invLen;
+}
+inline __host__ __device__ float3 normalize(float3 v)
+{
+    float invLen = rsqrtf(dot(v, v));
+    return v * invLen;
+}
+inline __host__ __device__ float4 normalize(float4 v)
+{
+    float invLen = rsqrtf(dot(v, v));
+    return v * invLen;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// floor
+////////////////////////////////////////////////////////////////////////////////
+
+inline __host__ __device__ float2 floorf(float2 v)
+{
+    return make_float2(floorf(v.x), floorf(v.y));
+}
+inline __host__ __device__ float3 floorf(float3 v)
+{
+    return make_float3(floorf(v.x), floorf(v.y), floorf(v.z));
+}
+inline __host__ __device__ float4 floorf(float4 v)
+{
+    return make_float4(floorf(v.x), floorf(v.y), floorf(v.z), floorf(v.w));
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// frac - returns the fractional portion of a scalar or each vector component
+////////////////////////////////////////////////////////////////////////////////
+
+inline __host__ __device__ float fracf(float v)
+{
+    return v - floorf(v);
+}
+inline __host__ __device__ float2 fracf(float2 v)
+{
+    return make_float2(fracf(v.x), fracf(v.y));
+}
+inline __host__ __device__ float3 fracf(float3 v)
+{
+    return make_float3(fracf(v.x), fracf(v.y), fracf(v.z));
+}
+inline __host__ __device__ float4 fracf(float4 v)
+{
+    return make_float4(fracf(v.x), fracf(v.y), fracf(v.z), fracf(v.w));
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// fmod
+////////////////////////////////////////////////////////////////////////////////
+
+inline __host__ __device__ float2 fmodf(float2 a, float2 b)
+{
+    return make_float2(fmodf(a.x, b.x), fmodf(a.y, b.y));
+}
+inline __host__ __device__ float3 fmodf(float3 a, float3 b)
+{
+    return make_float3(fmodf(a.x, b.x), fmodf(a.y, b.y), fmodf(a.z, b.z));
+}
+inline __host__ __device__ float4 fmodf(float4 a, float4 b)
+{
+    return make_float4(fmodf(a.x, b.x), fmodf(a.y, b.y), fmodf(a.z, b.z), fmodf(a.w, b.w));
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// absolute value
+////////////////////////////////////////////////////////////////////////////////
+
+inline __host__ __device__ float2 fabs(float2 v)
+{
+    return make_float2(fabs(v.x), fabs(v.y));
+}
+inline __host__ __device__ float3 fabs(float3 v)
+{
+    return make_float3(fabs(v.x), fabs(v.y), fabs(v.z));
+}
+inline __host__ __device__ float4 fabs(float4 v)
+{
+    return make_float4(fabs(v.x), fabs(v.y), fabs(v.z), fabs(v.w));
+}
+
+inline __host__ __device__ int2 abs(int2 v)
+{
+    return make_int2(abs(v.x), abs(v.y));
+}
+inline __host__ __device__ int3 abs(int3 v)
+{
+    return make_int3(abs(v.x), abs(v.y), abs(v.z));
+}
+inline __host__ __device__ int4 abs(int4 v)
+{
+    return make_int4(abs(v.x), abs(v.y), abs(v.z), abs(v.w));
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// reflect
+// - returns reflection of incident ray I around surface normal N
+// - N should be normalized, reflected vector's length is equal to length of I
+////////////////////////////////////////////////////////////////////////////////
+
+inline __host__ __device__ float3 reflect(float3 i, float3 n)
+{
+    return i - 2.0f * n * dot(n,i);
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// cross product
+////////////////////////////////////////////////////////////////////////////////
+
+inline __host__ __device__ float3 cross(float3 a, float3 b)
+{
+    return make_float3(a.y*b.z - a.z*b.y, a.z*b.x - a.x*b.z, a.x*b.y - a.y*b.x);
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// smoothstep
+// - returns 0 if x < a
+// - returns 1 if x > b
+// - otherwise returns smooth interpolation between 0 and 1 based on x
+////////////////////////////////////////////////////////////////////////////////
+
+inline __device__ __host__ float smoothstep(float a, float b, float x)
+{
+    float y = clamp((x - a) / (b - a), 0.0f, 1.0f);
+    return (y*y*(3.0f - (2.0f*y)));
+}
+inline __device__ __host__ float2 smoothstep(float2 a, float2 b, float2 x)
+{
+    float2 y = clamp((x - a) / (b - a), 0.0f, 1.0f);
+    return (y*y*(make_float2(3.0f) - (make_float2(2.0f)*y)));
+}
+inline __device__ __host__ float3 smoothstep(float3 a, float3 b, float3 x)
+{
+    float3 y = clamp((x - a) / (b - a), 0.0f, 1.0f);
+    return (y*y*(make_float3(3.0f) - (make_float3(2.0f)*y)));
+}
+inline __device__ __host__ float4 smoothstep(float4 a, float4 b, float4 x)
+{
+    float4 y = clamp((x - a) / (b - a), 0.0f, 1.0f);
+    return (y*y*(make_float4(3.0f) - (make_float4(2.0f)*y)));
+}
+
+#endif
diff --git a/RWR/src/ze_oss/imp_3rdparty_cuda_toolkit/CATKIN_IGNORE b/RWR/src/ze_oss/imp_3rdparty_cuda_toolkit/CATKIN_IGNORE
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/RWR/src/ze_oss/imp_3rdparty_cuda_toolkit/CMakeLists.txt b/RWR/src/ze_oss/imp_3rdparty_cuda_toolkit/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..2dd98f82efc480faf892e2d78841c17881fc3140
--- /dev/null
+++ b/RWR/src/ze_oss/imp_3rdparty_cuda_toolkit/CMakeLists.txt
@@ -0,0 +1,24 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(imp_3rdparty_cuda_toolkit)
+
+find_package(catkin_simple REQUIRED)
+catkin_simple(ALL_DEPS_REQUIRED)
+
+include(ze_macros_cuda)
+find_cuda()
+
+
+file(MAKE_DIRECTORY ${CATKIN_DEVEL_PREFIX}/include)
+message(STATUS "INSTALL FROM: ${CMAKE_CURRENT_SOURCE_DIR}/${CUDA_VERSION}/include")
+message(STATUS "INSTALL TO:   ${CATKIN_DEVEL_PREFIX}/include")
+# install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/${CUDA_VERSION}/include/ DESTINATION ${CATKIN_DEVEL_PREFIX}/include
+#    FILES_MATCHING PATTERN "*.h")
+cs_add_library(${PROJECT_NAME} src/empty.cpp)
+
+#include_directories(${CMAKE_CURRENT_SOURCE_DIR}/${CUDA_VERSION}/include)
+cs_install()
+cs_export(
+  #   INCLUDE_DIRS ${CATKIN_DEVEL_PREFIX}/include/imp_3rdparty_cuda_toolkit
+  #   CFG_EXTRAS imp_3rdparty_cuda_toolkit-extras.cmake.in
+  INCLUDE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/${CUDA_VERSION}/include
+  )
diff --git a/RWR/src/ze_oss/imp_3rdparty_cuda_toolkit/cmake/#imp_3rdparty_cuda_toolkit-extras.cmake.in# b/RWR/src/ze_oss/imp_3rdparty_cuda_toolkit/cmake/#imp_3rdparty_cuda_toolkit-extras.cmake.in#
new file mode 100644
index 0000000000000000000000000000000000000000..5f5275042a1d4803fbd07a0c0a989224f066df66
--- /dev/null
+++ b/RWR/src/ze_oss/imp_3rdparty_cuda_toolkit/cmake/#imp_3rdparty_cuda_toolkit-extras.cmake.in#
@@ -0,0 +1 @@
+set(@PROJECT_NAME@_INCLUDE_DIRS @CATKIN_DEVEL_PREFIX@/include/@CUDA_VERSION@o)
diff --git a/RWR/src/ze_oss/imp_3rdparty_cuda_toolkit/cmake/imp_3rdparty_cuda_toolkit-extras.cmake.in b/RWR/src/ze_oss/imp_3rdparty_cuda_toolkit/cmake/imp_3rdparty_cuda_toolkit-extras.cmake.in
new file mode 100644
index 0000000000000000000000000000000000000000..e96c0e93f343715ae5664cfd4be54e7a28861e1f
--- /dev/null
+++ b/RWR/src/ze_oss/imp_3rdparty_cuda_toolkit/cmake/imp_3rdparty_cuda_toolkit-extras.cmake.in
@@ -0,0 +1 @@
+set(@PROJECT_NAME@_INCLUDE_DIRS @CATKIN_DEVEL_PREFIX@/include)
diff --git a/RWR/src/ze_oss/imp_3rdparty_cuda_toolkit/package.xml b/RWR/src/ze_oss/imp_3rdparty_cuda_toolkit/package.xml
new file mode 100644
index 0000000000000000000000000000000000000000..9cc5beeea34e1d95e1453d28ecb27416f0604677
--- /dev/null
+++ b/RWR/src/ze_oss/imp_3rdparty_cuda_toolkit/package.xml
@@ -0,0 +1,15 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>imp_3rdparty_cuda_toolkit</name>
+  <version>0.1.4</version>
+  <description>catkin package for cuda helper stuff shiped with cuda samples</description>
+  <license>See cuda distribution</license>
+
+  <maintainer email="code@werlberger.org">Manuel Werlberger</maintainer>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <buildtool_depend>catkin_simple</buildtool_depend>
+
+  <depend>ze_cmake</depend>
+</package>
+  
diff --git a/RWR/src/ze_oss/imp_3rdparty_cuda_toolkit/src/empty.cpp b/RWR/src/ze_oss/imp_3rdparty_cuda_toolkit/src/empty.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f39643ef48d93f63122e747821cd658b4b012c34
--- /dev/null
+++ b/RWR/src/ze_oss/imp_3rdparty_cuda_toolkit/src/empty.cpp
@@ -0,0 +1,24 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
diff --git a/RWR/src/ze_oss/imp_app_pangolin_example/CATKIN_IGNORE b/RWR/src/ze_oss/imp_app_pangolin_example/CATKIN_IGNORE
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/RWR/src/ze_oss/imp_app_pangolin_example/CMakeLists.txt b/RWR/src/ze_oss/imp_app_pangolin_example/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..9f72b06b938f14be31554a041f4edf4ddaa4d105
--- /dev/null
+++ b/RWR/src/ze_oss/imp_app_pangolin_example/CMakeLists.txt
@@ -0,0 +1,20 @@
+project(imp_app_pangolin_example)
+cmake_minimum_required(VERSION 2.8.0)
+
+if(${CMAKE_MAJOR_VERSION} VERSION_GREATER 3.0)
+  cmake_policy(SET CMP0054 OLD)
+endif(${CMAKE_MAJOR_VERSION} VERSION_GREATER 3.0)
+
+find_package(catkin_simple REQUIRED)
+catkin_simple(ALL_DEPS_REQUIRED)
+
+include(ze_setup)
+include(ze_macros_cuda)
+find_cuda()
+
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-unused-local-typedefs" )
+
+cs_add_executable(pangolin_load_test src/pangolin_load_test.cpp)
+
+cs_install()
+cs_export()
diff --git a/RWR/src/ze_oss/imp_app_pangolin_example/package.xml b/RWR/src/ze_oss/imp_app_pangolin_example/package.xml
new file mode 100644
index 0000000000000000000000000000000000000000..8ba3634b9a18f87001a93ac3d9f898fd83f886e2
--- /dev/null
+++ b/RWR/src/ze_oss/imp_app_pangolin_example/package.xml
@@ -0,0 +1,21 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>imp_app_pangolin_example</name>
+  <description>
+    Examples on how to use pangolin
+  </description>
+  <version>0.1.4</version>
+  <license>ZE</license>
+
+  <maintainer email="code@werlberger.org">Manuel Werlberger</maintainer>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <buildtool_depend>catkin_simple</buildtool_depend>
+
+  <depend>glog_catkin</depend>
+  <depend>ze_cmake</depend>
+  <depend>imp_core</depend>
+  <depend>imp_bridge_pangolin</depend>
+
+  <test_depend>gtest</test_depend>
+</package>
diff --git a/RWR/src/ze_oss/imp_app_pangolin_example/src/pangolin_load_test.cpp b/RWR/src/ze_oss/imp_app_pangolin_example/src/pangolin_load_test.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..be01f854e27c4d3952d410b1bb65fad2daddda6c
--- /dev/null
+++ b/RWR/src/ze_oss/imp_app_pangolin_example/src/pangolin_load_test.cpp
@@ -0,0 +1,72 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#include <assert.h>
+#include <cstdint>
+#include <iostream>
+#include <memory>
+
+#include <glog/logging.h>
+
+#include <opencv2/core/core.hpp>
+#include <opencv2/highgui/highgui.hpp>
+
+//#include <pangolin/image.h>
+//#include <pangolin/image_load.h>
+//#define BUILD_PANGOLIN_GUI
+//#include <pangolin/config.h>
+#include <pangolin/pangolin.h>
+
+#include <imp/core/roi.hpp>
+#include <imp/core/image_raw.hpp>
+//#include <imp/bridge/opencv/image_cv.hpp>
+//#include <imp/cu_core/cu_image_gpu.cuh>
+#include <imp/bridge/pangolin/imread.hpp>
+#include <imp/bridge/pangolin/pangolin_display.hpp>
+
+
+
+void setImageData(unsigned char * imageArray, int size){
+  for(int i = 0 ; i < size;i++) {
+    imageArray[i] = (unsigned char)(rand()/(RAND_MAX/255.0));
+  }
+}
+
+int main( int argc, char* argv[] )
+{
+  google::InitGoogleLogging(argv[0]);
+  CHECK_GE(argc,2) << "Usage: pangolin_load_test image_location";
+  const std::string filename(argv[1]);
+
+  std::shared_ptr<ze::ImageRaw8uC1> im_8uC1;
+  ze::pangolinBridgeLoad(im_8uC1, filename, ze::PixelOrder::gray);
+
+  VLOG(2) << "Read Lena (png) from " << filename
+          << ": " << im_8uC1->width() << "x" << im_8uC1->height() << "(" << im_8uC1->pitch() << ")";
+
+  ze::imshow(*im_8uC1, "Lena");
+
+  return EXIT_SUCCESS;
+}
+
diff --git a/RWR/src/ze_oss/imp_benchmark_aligned_allocator/CATKIN_IGNORE b/RWR/src/ze_oss/imp_benchmark_aligned_allocator/CATKIN_IGNORE
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/RWR/src/ze_oss/imp_benchmark_aligned_allocator/CMakeLists.txt b/RWR/src/ze_oss/imp_benchmark_aligned_allocator/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..0b9fb30c3ffe8f2071097402ab01c8fdb48c3111
--- /dev/null
+++ b/RWR/src/ze_oss/imp_benchmark_aligned_allocator/CMakeLists.txt
@@ -0,0 +1,21 @@
+project(imp_benchmark_aligned_allocator)
+cmake_minimum_required(VERSION 2.8.0)
+
+if(${CMAKE_MAJOR_VERSION} VERSION_GREATER 3.0)
+  cmake_policy(SET CMP0054 OLD)
+endif(${CMAKE_MAJOR_VERSION} VERSION_GREATER 3.0)
+
+find_package(catkin_simple REQUIRED)
+catkin_simple(ALL_DEPS_REQUIRED)
+
+include(ze_setup)
+include(ze_macros_cuda)
+find_cuda()
+
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-unused-local-typedefs" )
+
+cs_add_executable(aligned_allocator_benchmark src/aligned_allocator_benchmark.cpp)
+target_link_libraries(aligned_allocator_benchmark)
+
+cs_install()
+cs_export()
diff --git a/RWR/src/ze_oss/imp_benchmark_aligned_allocator/package.xml b/RWR/src/ze_oss/imp_benchmark_aligned_allocator/package.xml
new file mode 100644
index 0000000000000000000000000000000000000000..0d31a5580eeb17651d9a1bd7fa041344c12903dd
--- /dev/null
+++ b/RWR/src/ze_oss/imp_benchmark_aligned_allocator/package.xml
@@ -0,0 +1,19 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>imp_benchmark_aligned_allocator</name>
+  <description>
+    Benchmark to check the duration of aligned allocation
+  </description>
+  <version>0.1.4</version>
+  <license>ZE</license>
+
+  <maintainer email="code@werlberger.org">Manuel Werlberger</maintainer>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <buildtool_depend>catkin_simple</buildtool_depend>
+
+  <depend>glog_catkin</depend>
+  <depend>ze_cmake</depend>
+  <depend>imp_core</depend>
+
+</package>
diff --git a/RWR/src/ze_oss/imp_benchmark_aligned_allocator/src/aligned_allocator_benchmark.cpp b/RWR/src/ze_oss/imp_benchmark_aligned_allocator/src/aligned_allocator_benchmark.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..603eda540589258a74d6450033a415282cf9c568
--- /dev/null
+++ b/RWR/src/ze_oss/imp_benchmark_aligned_allocator/src/aligned_allocator_benchmark.cpp
@@ -0,0 +1,71 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#include <cstdint>
+#include <iostream>
+#include <stdlib.h>
+#include <ze/common/logging.hpp>
+#include <ze/common/timer.hpp>
+#include <ze/common/timer_statistics.hpp>
+#include <imp/core/image_raw.hpp>
+
+int main(int argc, char* argv[])
+{
+  google::InitGoogleLogging(argv[0]);
+  google::ParseCommandLineFlags(&argc, &argv, true);
+  google::InstallFailureSignalHandler();
+  FLAGS_alsologtostderr = true;
+  FLAGS_colorlogtostderr = true;
+  VLOG(1) << "Starting aligned allocator benchmarking";
+
+
+  const size_t memory_size = 1e6;
+  int memaddr_align = 32;
+  std::uint64_t num_rounds = 1e6;
+
+  {
+    ze::TimerStatistics timer;
+    for (std::uint64_t i=0; i<num_rounds; ++i)
+    {
+      __attribute__((unused)) auto t = timer.timeScope();
+      std::uint8_t* p_data_aligned;
+      int ret = posix_memalign((void**)&p_data_aligned, memaddr_align, memory_size);
+      free(p_data_aligned);
+      (void)ret;
+    }
+    VLOG(1) << "posix_memalign: " << timer.mean() << "ms";
+  }
+
+  {
+    ze::TimerStatistics timer;
+    for (std::uint64_t i=0; i<num_rounds; ++i)
+    {
+      __attribute__((unused)) auto t = timer.timeScope();
+      std::uint8_t* p_data_aligned = (std::uint8_t*)aligned_alloc(memaddr_align, memory_size);
+      free(p_data_aligned);
+    }
+    VLOG(1) << "posix_memalign: " << timer.mean() << "ms";
+  }
+
+}
diff --git a/RWR/src/ze_oss/imp_bridge_af/CATKIN_IGNORE b/RWR/src/ze_oss/imp_bridge_af/CATKIN_IGNORE
new file mode 100644
index 0000000000000000000000000000000000000000..0519ecba6ea913e21689ec692e81e9e4973fbf73
--- /dev/null
+++ b/RWR/src/ze_oss/imp_bridge_af/CATKIN_IGNORE
@@ -0,0 +1 @@
+ 
\ No newline at end of file
diff --git a/RWR/src/ze_oss/imp_bridge_af/CMakeLists.txt b/RWR/src/ze_oss/imp_bridge_af/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..1bd3531e044a94fca4e123c6278da30d33b8232c
--- /dev/null
+++ b/RWR/src/ze_oss/imp_bridge_af/CMakeLists.txt
@@ -0,0 +1,55 @@
+if(NOT ${ZE_USE_ARRAYFIRE})
+  return()
+endif()
+
+project(imp_bridge_af)
+cmake_minimum_required(VERSION 2.8.0)
+
+if(${CMAKE_MAJOR_VERSION} VERSION_GREATER 3.0)
+  cmake_policy(SET CMP0054 OLD)
+endif(${CMAKE_MAJOR_VERSION} VERSION_GREATER 3.0)
+
+find_package(catkin_simple REQUIRED)
+catkin_simple(ALL_DEPS_REQUIRED)
+
+include(ze_setup)
+
+include(ze_macros_cuda)
+find_cuda()
+
+FIND_LIBRARY(CUDA_NVVM_LIBRARY
+  NAMES "nvvm"
+  PATH_SUFFIXES "nvvm/lib64" "nvvm/lib"
+  PATHS ${CUDA_TOOLKIT_ROOT_DIR}
+  DOC "CUDA NVVM Library"
+)
+
+set(HEADERS
+  include/imp/bridge/af/image_af.hpp
+  include/imp/bridge/af/orb_detector_af.hpp
+  )
+
+set(SOURCES
+  src/image_af.cpp
+  src/orb_detector_af.cpp
+  )
+
+cs_add_library(${PROJECT_NAME} ${SOURCES} ${HEADERS})
+target_link_libraries(${PROJECT_NAME}
+  ${CUDA_LIBRARIES}
+  ${catkin_LIBRARIES}
+  ${CUDA_NVVM_LIBRARY}
+)
+
+###
+### GTESTS
+###
+
+catkin_add_gtest(${PROJECT_NAME}-test
+    test/imp_bridge_af_test.cpp
+ )
+target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME} pthread)
+
+cs_install()
+cs_export()
+
diff --git a/RWR/src/ze_oss/imp_bridge_af/include/imp/bridge/af/image_af.hpp b/RWR/src/ze_oss/imp_bridge_af/include/imp/bridge/af/image_af.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..d502090f3d10346334e5bf5c5722e587d7aef59c
--- /dev/null
+++ b/RWR/src/ze_oss/imp_bridge_af/include/imp/bridge/af/image_af.hpp
@@ -0,0 +1,64 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#pragma once
+
+#include <arrayfire.h>
+#include <af/internal.h>
+#include <imp/core/image.hpp>
+
+namespace ze {
+
+template<typename Pixel>
+class ImageAF : public ze::Image<Pixel>
+{
+public:
+  using Ptr = typename std::shared_ptr<ImageAF<Pixel>>;
+  using ConstPtrRef = const Ptr&;
+  using ConstPtr = typename std::shared_ptr<ImageAF<Pixel> const>;
+
+public:
+  ImageAF() = delete;
+  virtual ~ImageAF() = default;
+
+  ImageAF(const Image<Pixel>& from);
+  ImageAF(const af::array& from);
+
+  virtual Pixel* data(uint32_t ox = 0, uint32_t oy = 0) override;
+  virtual const Pixel* data(uint32_t ox = 0, uint32_t oy = 0) const override;
+
+  const af::array& afArray() const;
+
+protected:
+  af::array arr_;
+
+private:
+  static af::dtype pixelTypeToAF(ze::PixelType type);
+};
+
+typedef ImageAF<ze::Pixel8uC1> ImageAF8uC1;
+typedef ImageAF<ze::Pixel32sC1> ImageAF32sC1;
+typedef ImageAF<ze::Pixel32fC1> ImageAF32fC1;
+
+} // ze namespace
diff --git a/RWR/src/ze_oss/imp_bridge_af/include/imp/bridge/af/orb_detector_af.hpp b/RWR/src/ze_oss/imp_bridge_af/include/imp/bridge/af/orb_detector_af.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..f6bf8f8041f7294f44d156e66a909716f0c29c57
--- /dev/null
+++ b/RWR/src/ze_oss/imp_bridge_af/include/imp/bridge/af/orb_detector_af.hpp
@@ -0,0 +1,142 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#pragma once
+
+#include <imp/features/feature_detector.hpp>
+#include <imp/bridge/af/image_af.hpp>
+
+namespace ze {
+
+using OrbDescriptors = Eigen::Matrix<unsigned, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>;
+
+struct OrbDetectorOptions
+{
+  //! FAST threshold for which a pixel of the circle around the
+  //! central pixel is considered to be brighter or darker
+  float fast_threshold{20.0f};
+  //! Maximum number of features to hold
+  //! (will only keep the max_feat features with higher Harris responses)
+  uint16_t max_num_features{400};
+  //! Image downsample factor in interval (1, 2].
+  //! A value of 2 corresponds to half-sampling the input image
+  float scale_factor{1.5};
+  //! Number of pyramid levels for feature esxtraction
+  uint8_t pyramid_levels{4};
+  //! Gaussian blur input image with sigma=2
+  bool blur_input_image{false};
+};
+
+struct OrbKeypointWrapper
+{
+  static constexpr uint8_t c_descriptor_length{8};
+  using Ptr = typename std::shared_ptr<OrbKeypointWrapper>;
+
+  void allocate(uint32_t num)
+  {
+    num_detected = num;
+    x.reset(new float[num_detected]);
+    y.reset(new float[num_detected]);
+    score.reset(new float[num_detected]);
+    orient.reset(new float[num_detected]);
+    size.reset(new float[num_detected]);
+    descr.reset(new unsigned[c_descriptor_length*num_detected]);
+  }
+
+  inline Keypoints getKeypoints() const
+  {
+    Keypoints keypoints(2, num_detected);
+    for (size_t k = 0; k < num_detected; ++k)
+    {
+      keypoints(0, k) = x.get()[k];
+      keypoints(1, k) = y.get()[k];
+    }
+    return keypoints;
+  }
+
+  inline KeypointScores getKeypointScores() const
+  {
+    KeypointScores scores(num_detected);
+    for (size_t k = 0; k < num_detected; ++k)
+    {
+      scores(k) = score.get()[k];
+    }
+    return scores;
+  }
+
+  inline KeypointSizes getKeypointSizes() const
+  {
+    KeypointSizes sizes(num_detected);
+    for (size_t k = 0; k < num_detected; ++k)
+    {
+      sizes(k) = size.get()[k];
+    }
+    return sizes;
+  }
+
+  inline KeypointAngles getKeypointAngles() const
+  {
+    KeypointAngles angles(num_detected);
+    for (size_t k = 0; k < num_detected; ++k)
+    {
+      angles(k) = orient.get()[k];
+    }
+    return angles;
+  }
+
+  inline OrbDescriptors getDescriptors() const
+  {
+    OrbDescriptors descriptors(c_descriptor_length, num_detected);
+    for (size_t k = 0; k < num_detected; ++k)
+    {
+      for (uint8_t d = 0; d < c_descriptor_length; ++d)
+      {
+        descriptors(d, k) = descr.get()[k*c_descriptor_length+d];
+      }
+    }
+    return descriptors;
+  }
+
+  uint32_t num_detected{0};
+  std::unique_ptr<float[]> x;
+  std::unique_ptr<float[]> y;
+  std::unique_ptr<float[]> score;
+  std::unique_ptr<float[]> orient;
+  std::unique_ptr<float[]> size;
+  std::unique_ptr<unsigned[]> descr;
+};
+
+class OrbDetectorAF : public AbstractDetector
+{
+public:
+  virtual ~OrbDetectorAF() = default;
+  OrbDetectorAF(const OrbDetectorOptions& options, const Size2u& image_size);
+  virtual uint32_t detect(const ImagePyramid8uC1& pyr, KeypointsWrapper& keypoints) override;
+  virtual uint32_t detect(const ImageAF32fC1& im, OrbKeypointWrapper& keypoints);
+
+private:
+  OrbDetectorOptions options_;
+};
+
+} // ze namespace
diff --git a/RWR/src/ze_oss/imp_bridge_af/package.xml b/RWR/src/ze_oss/imp_bridge_af/package.xml
new file mode 100644
index 0000000000000000000000000000000000000000..1e59245e8be10eee8ee8fce63c61db7876f0baa6
--- /dev/null
+++ b/RWR/src/ze_oss/imp_bridge_af/package.xml
@@ -0,0 +1,24 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>imp_bridge_af</name>
+  <description>
+    IMP bridge to ArrayFire
+  </description>
+  <version>0.1.0</version>
+  <license>ZE</license>
+
+  <maintainer email="matia.pizzoli@wysszurich.ch">Matia Pizzoli</maintainer>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <buildtool_depend>catkin_simple</buildtool_depend>
+
+  <depend>ze_cmake</depend>
+  <depend>imp_core</depend>
+  <depend>imp_cu_core</depend>
+  <depend>imp_bridge_opencv</depend>
+  <depend>imp_3rdparty_cuda_toolkit</depend>
+  <depend>imp_features</depend>
+  <depend>arrayfire_catkin</depend>
+
+  <test_depend>gtest</test_depend>
+</package>
diff --git a/RWR/src/ze_oss/imp_bridge_af/src/image_af.cpp b/RWR/src/ze_oss/imp_bridge_af/src/image_af.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..119169acb5a4c8e749e42088608b3e50be8b90e1
--- /dev/null
+++ b/RWR/src/ze_oss/imp_bridge_af/src/image_af.cpp
@@ -0,0 +1,121 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#include <imp/bridge/af/image_af.hpp>
+#include <imp/core/linearmemory.hpp>
+#include <imp/cu_core/cu_image_gpu.cuh>
+
+namespace ze {
+
+template<typename Pixel>
+struct ImpAfConversionBuffer
+{
+  ze::LinearMemory<Pixel> h_buff;
+
+  ImpAfConversionBuffer(const Image<Pixel>& from)
+    : h_buff(from.numel())
+  {
+    for (size_t y = 0; y < from.height(); ++y)
+    {
+      for (size_t x = 0; x < from.width(); ++x)
+      {
+        h_buff(x*from.height()+y) = from.pixel(x, y);  //! AF array is column-major
+      }
+    }
+  }
+
+  auto cuData() -> decltype(ze::cu::toCudaVectorType(this->h_buff.data()))
+  {
+    return ze::cu::toCudaVectorType(this->h_buff.data());
+  }
+
+  auto cuData() const -> decltype(ze::cu::toConstCudaVectorType(this->h_buff.data()))
+  {
+    return ze::cu::toConstCudaVectorType(this->h_buff.data());
+  }
+};
+
+template<typename Pixel>
+ImageAF<Pixel>::ImageAF(const Image<Pixel>& from)
+  : Image<Pixel>(from)
+{
+  ImpAfConversionBuffer<Pixel> buffer(from);
+  arr_ = af::array(from.height(), from.width(), buffer.cuData());
+}
+
+template<typename Pixel>
+ImageAF<Pixel>::ImageAF(const af::array& from)
+  : Image<Pixel>(ze::Size2u(from.dims()[1], from.dims()[0])),
+    arr_(from)
+{ }
+
+template<typename Pixel>
+Pixel* ImageAF<Pixel>::data(uint32_t ox, uint32_t oy)
+{
+  DEBUG_CHECK_EQ(ox, 0u);
+  DEBUG_CHECK_EQ(oy, 0u);
+  switch(pixel_type<Pixel>::type)
+  {
+  case PixelType::i8uC1:
+    return reinterpret_cast<Pixel*>(arr_.device<unsigned char>());
+  case PixelType::i32sC1:
+    return reinterpret_cast<Pixel*>(arr_.device<int>());
+  case PixelType::i32fC1:
+    return reinterpret_cast<Pixel*>(arr_.device<float>());
+  default:
+    LOG(FATAL) << "pixel type not supported";
+    break;
+  }
+}
+
+template<typename Pixel>
+const Pixel* ImageAF<Pixel>::data(uint32_t ox, uint32_t oy) const
+{
+  DEBUG_CHECK_EQ(ox, 0u);
+  DEBUG_CHECK_EQ(oy, 0u);
+  switch(pixel_type<Pixel>::type)
+  {
+  case PixelType::i8uC1:
+    return reinterpret_cast<const Pixel*>(arr_.device<unsigned char>());
+  case PixelType::i32sC1:
+    return reinterpret_cast<const Pixel*>(arr_.device<int>());
+  case PixelType::i32fC1:
+    return reinterpret_cast<const Pixel*>(arr_.device<float>());
+  default:
+    LOG(FATAL) << "pixel type not supported";
+    break;
+  }
+}
+
+template<typename Pixel>
+const af::array& ImageAF<Pixel>::afArray() const
+{
+  return arr_;
+}
+
+template class ImageAF<ze::Pixel8uC1>;
+template class ImageAF<ze::Pixel32sC1>;
+template class ImageAF<ze::Pixel32fC1>;
+
+} // ze namespace
diff --git a/RWR/src/ze_oss/imp_bridge_af/src/orb_detector_af.cpp b/RWR/src/ze_oss/imp_bridge_af/src/orb_detector_af.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b7d28b37533f04aa24ec9228a263852afa409d8f
--- /dev/null
+++ b/RWR/src/ze_oss/imp_bridge_af/src/orb_detector_af.cpp
@@ -0,0 +1,66 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#include <imp/bridge/af/orb_detector_af.hpp>
+
+namespace ze {
+
+OrbDetectorAF::OrbDetectorAF(const OrbDetectorOptions& options, const Size2u& image_size)
+  : AbstractDetector(image_size, DetectorType::Orb),
+    options_(options)
+{ }
+
+uint32_t OrbDetectorAF::detect(const ImagePyramid8uC1& pyr, KeypointsWrapper& keypoints)
+{
+  LOG(FATAL) << "Not implemented";
+  return 0;
+}
+
+uint32_t OrbDetectorAF::detect(const ImageAF32fC1& im, OrbKeypointWrapper& keypoints)
+{
+  af::features feat;
+  af::array desc;
+  af::orb(
+        feat,
+        desc,
+        im.afArray(),
+        options_.fast_threshold,
+        options_.max_num_features,
+        options_.scale_factor,
+        options_.pyramid_levels,
+        options_.blur_input_image);
+
+  const size_t num_detected = feat.getNumFeatures();
+  keypoints.allocate(num_detected);
+  feat.getX().host(keypoints.x.get());
+  feat.getY().host(keypoints.y.get());
+  feat.getScore().host(keypoints.score.get());
+  feat.getOrientation().host(keypoints.orient.get());
+  feat.getSize().host(keypoints.size.get());
+  desc.host(keypoints.descr.get());
+
+  return num_detected;
+}
+
+} // ze namespace
diff --git a/RWR/src/ze_oss/imp_bridge_af/test/imp_bridge_af_test.cpp b/RWR/src/ze_oss/imp_bridge_af/test/imp_bridge_af_test.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..524be61ea9c47ea801835b4c7d64bd6a03deae00
--- /dev/null
+++ b/RWR/src/ze_oss/imp_bridge_af/test/imp_bridge_af_test.cpp
@@ -0,0 +1,188 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#include <imp/bridge/af/orb_detector_af.hpp>
+
+#include <imp/bridge/opencv/cv_bridge.hpp>
+
+#include <ze/common/benchmark.hpp>
+#include <ze/common/file_utils.hpp>
+#include <ze/common/test_entrypoint.hpp>
+#include <ze/common/test_utils.hpp>
+
+using namespace ze;
+
+TEST(impBridgeAFTest, constructFromImpImage_32fC1)
+{
+  const std::string test_data_name{"ze_feature_detection"};
+  const std::string predefined_img_data_file_name{"752x480/pyr_0.png"};
+
+  std::string path(
+        joinPath(
+          getTestDataDir(test_data_name),
+          predefined_img_data_file_name));
+
+  ImageCv32fC1::Ptr cv_img;
+  cvBridgeLoad(cv_img, path, PixelOrder::gray);
+  VLOG(2) << "loaded image " << path
+          << ", size " << cv_img->size();
+  ImageAF32fC1 af_img(*cv_img);
+  double af_sum = af::sum<double>(af_img.afArray());
+  VLOG(1) << std::fixed << "AF sum: " << af_sum;
+  double cv_sum = cv::sum(cv_img->cvMat())[0];
+  VLOG(1) << std::fixed << "OpenCV sum: " << cv_sum;
+  EXPECT_NEAR(cv_sum, af_sum, 0.01);
+}
+
+TEST(impBridgeAFTest, constructFromImpImage_8uC1)
+{
+  const std::string test_data_name{"ze_feature_detection"};
+  const std::string predefined_img_data_file_name{"752x480/pyr_0.png"};
+
+  std::string path(
+        joinPath(
+          getTestDataDir(test_data_name),
+          predefined_img_data_file_name));
+
+  ImageCv8uC1::Ptr cv_img;
+  cvBridgeLoad(cv_img, path, PixelOrder::gray);
+  VLOG(2) << "loaded image " << path
+          << ", size " << cv_img->size();
+  ImageAF8uC1 af_img(*cv_img);
+  double af_sum = af::sum<double>(af_img.afArray());
+  VLOG(1) << std::fixed << "AF sum: " << af_sum;
+  double cv_sum = cv::sum(cv_img->cvMat())[0];
+  VLOG(1) << std::fixed << "OpenCV sum: " << cv_sum;
+  EXPECT_NEAR(cv_sum, af_sum, 0.01);
+}
+
+TEST(impBridgeAFTest, constructFromAFArray_32fC1)
+{
+  const std::string test_data_name{"ze_feature_detection"};
+  const std::string predefined_img_data_file_name{"752x480/pyr_0.png"};
+
+  std::string path(
+        joinPath(
+          getTestDataDir(test_data_name),
+          predefined_img_data_file_name));
+
+  ImageCv32fC1::Ptr cv_img;
+  cvBridgeLoad(cv_img, path, PixelOrder::gray);
+
+  std::unique_ptr<float[]> h_buffer(
+        new float[cv_img->width()*cv_img->height()]);
+  for (size_t y = 0; y < cv_img->height(); ++y)
+  {
+    for (size_t x = 0; x < cv_img->width(); ++x)
+    {
+      h_buffer.get()[x*cv_img->height()+y] = cv_img->pixel(x, y);
+    }
+  }
+  ImageAF32fC1 af_img(
+        af::array(
+          cv_img->height(),
+          cv_img->width(),
+          h_buffer.get()));
+  double af_sum = af::sum<double>(af_img.afArray());
+  VLOG(1) << std::fixed << "AF sum: " << af_sum;
+  double cv_sum = cv::sum(cv_img->cvMat())[0];
+  VLOG(1) << std::fixed << "OpenCV sum: " << cv_sum;
+  EXPECT_NEAR(cv_sum, af_sum, 0.01);
+}
+
+TEST(impBridgeAFTest, constructFromAFArray_8uC1)
+{
+  const std::string test_data_name{"ze_feature_detection"};
+  const std::string predefined_img_data_file_name{"752x480/pyr_0.png"};
+
+  std::string path(
+        joinPath(
+          getTestDataDir(test_data_name),
+          predefined_img_data_file_name));
+
+  ImageCv8uC1::Ptr cv_img;
+  cvBridgeLoad(cv_img, path, PixelOrder::gray);
+
+  std::unique_ptr<unsigned char[]> h_buffer(
+        new unsigned char[cv_img->width()*cv_img->height()]);
+  for (size_t y = 0; y < cv_img->height(); ++y)
+  {
+    for (size_t x = 0; x < cv_img->width(); ++x)
+    {
+      h_buffer.get()[x*cv_img->height()+y] = cv_img->pixel(x, y);
+    }
+  }
+  ImageAF8uC1 af_img(
+        af::array(
+          cv_img->height(),
+          cv_img->width(),
+          h_buffer.get()));
+  double af_sum = af::sum<double>(af_img.afArray());
+  VLOG(1) << std::fixed << "AF sum: " << af_sum;
+  double cv_sum = cv::sum(cv_img->cvMat())[0];
+  VLOG(1) << std::fixed << "OpenCV sum: " << cv_sum;
+  EXPECT_NEAR(cv_sum, af_sum, 0.01);
+}
+
+TEST(impBridgeAFTest, orbDetectorAF32fC1)
+{
+  const std::string test_data_name{"ze_feature_detection"};
+  const std::string predefined_img_data_file_name{"752x480/pyr_0.png"};
+
+  std::string path(joinPath(getTestDataDir(test_data_name),
+                            predefined_img_data_file_name));
+
+  ImageCv32fC1::Ptr cv_img;
+  cvBridgeLoad(cv_img, path, PixelOrder::gray);
+
+  ImageAF32fC1::Ptr im =
+      std::make_shared<ImageAF32fC1>(*cv_img);
+
+  OrbDetectorOptions options;
+  options.fast_threshold /= 255.f;
+  OrbDetectorAF detector(options, im->size());
+  OrbKeypointWrapper features;
+  detector.detect(*im, features); // GPU warm-up
+  auto detectLambda = [&](){
+    detector.detect(*im, features);
+  };
+  runTimingBenchmark(detectLambda, 10, 20, "AF ORB Detector", true);
+
+  Keypoints keypoints = features.getKeypoints();
+  KeypointScores scores = features.getKeypointScores();
+  KeypointSizes sizes = features.getKeypointSizes();
+  KeypointAngles angles = features.getKeypointAngles();
+  OrbDescriptors descriptors = features.getDescriptors();
+
+  for (int k = 0; k < keypoints.cols(); ++k)
+  {
+    EXPECT_GT(keypoints(0, k), 0);
+    EXPECT_LT(keypoints(0, k), im->width());
+    EXPECT_GT(keypoints(1, k), 0);
+    EXPECT_LT(keypoints(1, k), im->height());
+  }
+  VLOG(2) << "number of computed descriptors: " << descriptors.cols();
+}
+
+ZE_UNITTEST_ENTRYPOINT
diff --git a/RWR/src/ze_oss/imp_bridge_opencv/CMakeLists.txt b/RWR/src/ze_oss/imp_bridge_opencv/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..d8705f135f2dd02181340ddb548d694ad430fbef
--- /dev/null
+++ b/RWR/src/ze_oss/imp_bridge_opencv/CMakeLists.txt
@@ -0,0 +1,42 @@
+project(imp_bridge_opencv)
+cmake_minimum_required(VERSION 2.8.0)
+
+if(${CMAKE_MAJOR_VERSION} VERSION_GREATER 3.0)
+  cmake_policy(SET CMP0054 OLD)
+endif(${CMAKE_MAJOR_VERSION} VERSION_GREATER 3.0)
+
+find_package(catkin_simple REQUIRED)
+catkin_simple(ALL_DEPS_REQUIRED)
+
+include(ze_setup)
+
+find_package(OpenCV REQUIRED core highgui imgproc calib3d)
+list(APPEND catkin_LIBRARIES ${OpenCV_LIBS})
+
+set(HEADERS
+  include/imp/bridge/opencv/cv_connector_pixel_types.hpp
+  include/imp/bridge/opencv/image_cv.hpp
+  include/imp/bridge/opencv/cv_bridge.hpp
+  include/imp/bridge/opencv/cu_cv_bridge.hpp
+  )
+
+set(SOURCES
+  src/cv_bridge.cpp
+  src/cv_connector_pixel_types.cpp
+  src/image_cv.cpp
+  )
+
+set(IMPL_FILES
+  )
+
+cs_add_library(${PROJECT_NAME} ${SOURCES} ${HEADERS} ${IMPL_FILES})
+#target_link_libraries(${PROJECT_NAME} ${CUDA_LIBRARIES} ${catkin_LIBRARIES})
+
+#########
+# TESTS #
+#########
+catkin_add_gtest(test_load_image test/test_load_image.cpp)
+target_link_libraries(test_load_image ${PROJECT_NAME})
+
+cs_install()
+cs_export(CFG_EXTRAS ze_imp_bridge_opencv-extras.cmake)
diff --git a/RWR/src/ze_oss/imp_bridge_opencv/cmake/ze_imp_bridge_opencv-extras.cmake.in b/RWR/src/ze_oss/imp_bridge_opencv/cmake/ze_imp_bridge_opencv-extras.cmake.in
new file mode 100644
index 0000000000000000000000000000000000000000..185c77d84853cac03b6d0748ae5ad050b0e53680
--- /dev/null
+++ b/RWR/src/ze_oss/imp_bridge_opencv/cmake/ze_imp_bridge_opencv-extras.cmake.in
@@ -0,0 +1,2 @@
+list(INSERT ZE_LIBRARIES 0 "@OpenCV_LIBS@")
+
diff --git a/RWR/src/ze_oss/imp_bridge_opencv/include/imp/bridge/opencv/cu_cv_bridge.hpp b/RWR/src/ze_oss/imp_bridge_opencv/include/imp/bridge/opencv/cu_cv_bridge.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..9fe6b83319a55a8e0eb0fd87ceed65d7295ea8ca
--- /dev/null
+++ b/RWR/src/ze_oss/imp_bridge_opencv/include/imp/bridge/opencv/cu_cv_bridge.hpp
@@ -0,0 +1,66 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#pragma once
+
+#include <memory>
+
+#include <imp/bridge/opencv/cv_bridge.hpp>
+#include <imp/cu_core/cu_image_gpu.cuh>
+
+
+namespace ze {
+namespace cu {
+
+//------------------------------------------------------------------------------
+template<typename Pixel>
+void cvBridgeLoad(ze::cu::ImageGpuPtr<Pixel>& out, const std::string& filename,
+                  ze::PixelOrder pixel_order)
+{
+  ImageCvPtr<Pixel> cv_img;
+  ze::cvBridgeLoad<Pixel>(cv_img, filename, pixel_order);
+  out = std::make_shared<ze::cu::ImageGpu<Pixel>>(*cv_img);
+}
+
+//------------------------------------------------------------------------------
+template<typename Pixel>
+void cvBridgeShow(const std::string& winname,
+                  const ze::cu::ImageGpu<Pixel>& img, bool normalize=false)
+{
+  const ImageCv<Pixel> cv_img(img);
+  ze::cvBridgeShow(winname, cv_img, normalize);
+}
+
+//------------------------------------------------------------------------------
+template<typename Pixel, typename T>
+void cvBridgeShow(const std::string& winname,
+                  const ze::cu::ImageGpu<Pixel>& img,
+                  const T& min, const T& max)
+{
+  const ImageCv<Pixel> cv_img(img);
+  ze::cvBridgeShow(winname, cv_img, min, max);
+}
+
+} // namespace cu
+} // namespace ze
diff --git a/RWR/src/ze_oss/imp_bridge_opencv/include/imp/bridge/opencv/cv_bridge.hpp b/RWR/src/ze_oss/imp_bridge_opencv/include/imp/bridge/opencv/cv_bridge.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..c4cfd934a6d7e90eab73eabee05803a65e8cef10
--- /dev/null
+++ b/RWR/src/ze_oss/imp_bridge_opencv/include/imp/bridge/opencv/cv_bridge.hpp
@@ -0,0 +1,151 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#pragma once
+
+#include <memory>
+#include <glog/logging.h>
+
+#include <opencv2/highgui/highgui.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
+#include <imp/bridge/opencv/image_cv.hpp>
+#include <ze/common/file_utils.hpp>
+
+namespace ze {
+
+//------------------------------------------------------------------------------
+template<typename Pixel>
+void cvBridgeLoad(ImageCvPtr<Pixel>& out,
+                  const std::string& filename, PixelOrder pixel_order)
+{
+  CHECK(fileExists(filename)) << "File does not exist: " << filename;
+  cv::Mat mat;
+  if (pixel_order == PixelOrder::gray)
+  {
+    mat = cv::imread(filename, cv::IMREAD_GRAYSCALE);
+    CHECK(!mat.empty());
+  }
+  else
+  {
+    // everything else needs color information :)
+    mat = cv::imread(filename, cv::IMREAD_COLOR);
+    CHECK(!mat.empty());
+  }
+
+  switch(pixel_type<Pixel>::type)
+  {
+  case PixelType::i8uC1:
+    if (pixel_order == PixelOrder::gray)
+    {
+      out = std::make_shared<ImageCv<Pixel>>(mat);
+    }
+    else
+    {
+      out = std::make_shared<ImageCv<Pixel>>(mat.cols, mat.rows);
+      cv::cvtColor(mat, out->cvMat(), cv::COLOR_BGR2GRAY);
+    }
+  break;
+  case PixelType::i32fC1:
+    out = std::make_shared<ImageCv<Pixel>>(mat.cols, mat.rows);
+    if (mat.channels() > 1)
+    {
+      cv::cvtColor(mat, mat, cv::COLOR_BGR2GRAY);
+    }
+    mat.convertTo(out->cvMat(), CV_32F, 1./255.);
+  break;
+  default:
+    CHECK(false) << "Conversion for reading given pixel_type not supported yet.";
+    break;
+  }
+}
+
+//------------------------------------------------------------------------------
+template<typename Pixel>
+void cvBridgeSave(const std::string& filename, const ImageCv<Pixel>& img, bool normalize=false)
+{
+  if (normalize)
+  {
+    // TODO
+  }
+  cv::imwrite(filename, img.cvMat());
+}
+
+//------------------------------------------------------------------------------
+template<typename Pixel>
+void cvBridgeShow(const std::string& winname, const ImageCv<Pixel>& img,
+                  bool normalize=false)
+{
+  if (normalize)
+  {
+    int mat_type = (img.nChannels() > 1) ? CV_8UC3 : CV_8UC1;
+    cv::Mat norm_mat(img.height(), img.width(), mat_type);
+    cv::normalize(img.cvMat(), norm_mat, 0, 255, cv::NORM_MINMAX, CV_8U);
+    cv::imshow(winname, norm_mat);
+  }
+  else
+  {
+    cv::imshow(winname, img.cvMat());
+  }
+}
+
+//------------------------------------------------------------------------------
+template<typename Pixel, typename T>
+void cvBridgeShow(const std::string& winname, const ImageCv<Pixel>& img,
+                  const T& min, const T& max)
+{
+  cv::Mat norm_mat = img.cvMat().clone();
+  norm_mat = (norm_mat-min) / (max-min);
+  cv::imshow(winname, norm_mat);
+}
+
+//-----------------------------------------------------------------------------
+// Convenience functions.
+ImageCv8uC1::Ptr cvBridgeLoad8uC1(const std::string& filename);
+
+//------------------------------------------------------------------------------
+//enum class OcvBridgeLoadAs
+//{
+//  raw,
+//  cuda,
+//  cvmat
+//};
+//
+//template<typename Pixel>
+//std::shared_ptr<> ocv_bridge_imread(const std::string& filename, OcvBridgeLoadAs load_as=OcvBridgeLoadAs::raw)
+//{
+//  switch (load_as)
+//  {
+//  case OcvBridgeLoadAs::cvmat:
+//    break;
+//  case OcvBridgeLoadAs::cuda:
+//    break;
+//  case OcvBridgeLoadAs::raw:
+//  default:
+//    break;
+
+//  }
+//}
+
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/imp_bridge_opencv/include/imp/bridge/opencv/cv_connector_pixel_types.hpp b/RWR/src/ze_oss/imp_bridge_opencv/include/imp/bridge/opencv/cv_connector_pixel_types.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..f6d264a1497520fc9ef9615e0632e3b2abc91c46
--- /dev/null
+++ b/RWR/src/ze_oss/imp_bridge_opencv/include/imp/bridge/opencv/cv_connector_pixel_types.hpp
@@ -0,0 +1,47 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#pragma once
+
+#include <imp/core/pixel_enums.hpp>
+
+namespace ze {
+
+/**
+ * @brief pixelTypeFromCv converges OpenCV pixel type to IMP pixel types
+ * @param type OpenCV pixel type (e.g. CV_8UC1 for single channel 8-bit pixels)
+ * @return IMP pixel type (e.g. imp::PixelType::i8uC1 for single channel 8-bit pixels)
+ */
+ze::PixelType pixelTypeFromCv(int type);
+
+
+/**
+ * @brief pixelTypeFromCv converges IMP pixel types to OpenCV pixel type
+ * @param type IMP pixel type (e.g. imp::PixelType::i8uC1 for single channel 8-bit pixels)
+ * @return OpenCV pixel type (e.g. CV_8UC1 for single channel 8-bit pixels)
+ */
+int pixelTypeToCv(ze::PixelType type);
+
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/imp_bridge_opencv/include/imp/bridge/opencv/image_cv.hpp b/RWR/src/ze_oss/imp_bridge_opencv/include/imp/bridge/opencv/image_cv.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..d78966f416cefeb686c5a1fcd73dfa91e2b0371b
--- /dev/null
+++ b/RWR/src/ze_oss/imp_bridge_opencv/include/imp/bridge/opencv/image_cv.hpp
@@ -0,0 +1,133 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#pragma once
+
+#include <memory>
+#include <opencv2/core/core.hpp>
+#include <imp/core/pixel_enums.hpp>
+#include <imp/core/image.hpp>
+
+namespace ze {
+
+/**
+ * @brief The ImageCv class is an image holding an OpenCV matrix (cv::Mat) for the image data
+ *
+ * The ImageCv can be used to interface with OpenCV. The matrix can be directly
+ * accessed and used for calling OpenCV functions. Furthermore all getters/setters
+ * for the IMP image representations are available. You can also construct an
+ * ImageCv with a given cv::Mat in order to have a common data representation in
+ * your code.
+ *
+ */
+template<typename Pixel>
+class ImageCv : public ze::Image<Pixel>
+{
+public:
+  using Base = Image<Pixel>;
+
+  using Ptr = typename std::shared_ptr<ImageCv<Pixel>>;
+  using ConstPtrRef = const Ptr&;
+  using ConstPtr = typename std::shared_ptr<ImageCv<Pixel> const>;
+
+public:
+  ImageCv() = delete;
+  virtual ~ImageCv() = default;
+
+  ImageCv(const ze::Size2u& size, ze::PixelOrder pixel_order=ze::PixelOrder::undefined);
+  ImageCv(uint32_t width, uint32_t height, ze::PixelOrder pixel_order=ze::PixelOrder::undefined);
+  ImageCv(const ImageCv<Pixel>& from);
+  ImageCv(const Base& from);
+  ImageCv(cv::Mat mat, ze::PixelOrder pixel_order=ze::PixelOrder::undefined);
+//  ImageCv(Pixel* data, uint32_t width, uint32_t height,
+//          uint32_t pitch, bool use_ext_data_pointer = false);
+
+  /** Returns the internal OpenCV image/mat
+   */
+  virtual cv::Mat& cvMat();
+  virtual const cv::Mat& cvMat() const;
+
+  /** Returns a pointer to the pixel data.
+   * The pointer can be offset to position \a (ox/oy).
+   * @param[in] ox Horizontal offset of the pointer array.
+   * @param[in] oy Vertical offset of the pointer array.
+   * @return Pointer to the pixel array.
+   */
+  virtual Pixel* data(uint32_t ox = 0, uint32_t oy = 0) override;
+  virtual const Pixel* data(uint32_t ox = 0, uint32_t oy = 0) const override;
+
+  /**
+   * @brief setValue Sets image data to the specified \a value.
+   * @param value Value to be set to the whole image data.
+   */
+  virtual void setValue(const Pixel& value) override;
+
+protected:
+  cv::Mat mat_;
+};
+
+//-----------------------------------------------------------------------------
+// convenience typedefs
+// (sync with explicit template class instantiations at the end of the cpp file)
+typedef ImageCv<ze::Pixel8uC1> ImageCv8uC1;
+typedef ImageCv<ze::Pixel8uC2> ImageCv8uC2;
+typedef ImageCv<ze::Pixel8uC3> ImageCv8uC3;
+typedef ImageCv<ze::Pixel8uC4> ImageCv8uC4;
+
+typedef ImageCv<ze::Pixel16uC1> ImageCv16uC1;
+typedef ImageCv<ze::Pixel16uC2> ImageCv16uC2;
+typedef ImageCv<ze::Pixel16uC3> ImageCv16uC3;
+typedef ImageCv<ze::Pixel16uC4> ImageCv16uC4;
+
+typedef ImageCv<ze::Pixel32sC1> ImageCv32sC1;
+typedef ImageCv<ze::Pixel32sC2> ImageCv32sC2;
+typedef ImageCv<ze::Pixel32sC3> ImageCv32sC3;
+typedef ImageCv<ze::Pixel32sC4> ImageCv32sC4;
+
+typedef ImageCv<ze::Pixel32fC1> ImageCv32fC1;
+typedef ImageCv<ze::Pixel32fC2> ImageCv32fC2;
+typedef ImageCv<ze::Pixel32fC3> ImageCv32fC3;
+typedef ImageCv<ze::Pixel32fC4> ImageCv32fC4;
+
+
+//typedef ImageCv<std::uint8_t, imp::PixelType::i8uC1> ImageCv8uC1;
+//typedef ImageCv<std::uint16_t, imp::PixelType::i8uC1> ImageCv16uC1;
+//typedef ImageCv<sstd::int32_t, imp::PixelType::i8uC1> ImageCv32sC1;
+//typedef ImageCv<float, imp::PixelType::i8uC1> ImageCv32fC1;
+
+// shared pointers
+
+template <typename Pixel>
+using ImageCvPtr = typename std::shared_ptr<ImageCv<Pixel>>;
+
+//template <typename Pixel>
+//using ConstImageCvPtrRef = typename ImageCv<Pixel>::ConstPtrRef;
+
+
+template <typename Pixel>
+using ImageCvConstPtr = typename ImageCv<Pixel>::ConstPtr;
+
+
+} // namespace ze
+
diff --git a/RWR/src/ze_oss/imp_bridge_opencv/package.xml b/RWR/src/ze_oss/imp_bridge_opencv/package.xml
new file mode 100644
index 0000000000000000000000000000000000000000..82f34d8b46563bde43f0cd469aba57b388df7b02
--- /dev/null
+++ b/RWR/src/ze_oss/imp_bridge_opencv/package.xml
@@ -0,0 +1,20 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>imp_bridge_opencv</name>
+  <description>
+    IMP bridge to OpenCV adds the ability to interconnect with the OpenCV
+    library.
+  </description>
+  <version>0.1.4</version>
+  <license>ZE</license>
+
+  <maintainer email="code@werlberger.org">Manuel Werlberger</maintainer>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <buildtool_depend>catkin_simple</buildtool_depend>
+
+  <depend>ze_cmake</depend>
+  <depend>imp_core</depend>
+  
+  <test_depend>gtest</test_depend>
+</package>
diff --git a/RWR/src/ze_oss/imp_bridge_opencv/src/cv_bridge.cpp b/RWR/src/ze_oss/imp_bridge_opencv/src/cv_bridge.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a7f317b2ae78c6ce64d026743c4191faae753f5b
--- /dev/null
+++ b/RWR/src/ze_oss/imp_bridge_opencv/src/cv_bridge.cpp
@@ -0,0 +1,36 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#include <imp/bridge/opencv/cv_bridge.hpp>
+
+namespace ze {
+
+ImageCv8uC1::Ptr cvBridgeLoad8uC1(const std::string& filename)
+{
+  ImageCv8uC1::Ptr img;
+  cvBridgeLoad<Pixel8uC1>(img, filename, PixelOrder::gray);
+  return img;
+}
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/imp_bridge_opencv/src/cv_connector_pixel_types.cpp b/RWR/src/ze_oss/imp_bridge_opencv/src/cv_connector_pixel_types.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6a864692be6900628b65a2850a7feabe0a2d96b9
--- /dev/null
+++ b/RWR/src/ze_oss/imp_bridge_opencv/src/cv_connector_pixel_types.cpp
@@ -0,0 +1,88 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#include <imp/bridge/opencv/cv_connector_pixel_types.hpp>
+
+#include <opencv2/core/core.hpp>
+
+namespace ze {
+
+//------------------------------------------------------------------------------
+ze::PixelType pixelTypeFromCv(int type)
+{
+  switch (type)
+  {
+  case CV_8UC1: return ze::PixelType::i8uC1;
+  case CV_8UC2: return ze::PixelType::i8uC2;
+  case CV_8UC3: return ze::PixelType::i8uC3;
+  case CV_8UC4: return ze::PixelType::i8uC4;
+  //
+  case CV_16UC1: return ze::PixelType::i16uC1;
+  case CV_16UC2: return ze::PixelType::i16uC2;
+  case CV_16UC3: return ze::PixelType::i16uC3;
+  case CV_16UC4: return ze::PixelType::i16uC4;
+  //
+  case CV_32SC1: return ze::PixelType::i32sC1;
+  case CV_32SC2: return ze::PixelType::i32sC2;
+  case CV_32SC3: return ze::PixelType::i32sC3;
+  case CV_32SC4: return ze::PixelType::i32sC4; //
+  case CV_32FC1: return ze::PixelType::i32fC1;
+  case CV_32FC2: return ze::PixelType::i32fC2;
+  case CV_32FC3: return ze::PixelType::i32fC3;
+  case CV_32FC4: return ze::PixelType::i32fC4;
+  //
+  default: return ze::PixelType::undefined;
+  }
+}
+
+//------------------------------------------------------------------------------
+int pixelTypeToCv(ze::PixelType type)
+{
+  switch (type)
+  {
+  case ze::PixelType::i8uC1: return CV_8UC1;
+  case ze::PixelType::i8uC2: return CV_8UC2;
+  case ze::PixelType::i8uC3: return CV_8UC3;
+  case ze::PixelType::i8uC4: return CV_8UC4;
+  //
+  case ze::PixelType::i16uC1: return CV_16UC1;
+  case ze::PixelType::i16uC2: return CV_16UC2;
+  case ze::PixelType::i16uC3: return CV_16UC3;
+  case ze::PixelType::i16uC4: return CV_16UC4;
+  //
+  case ze::PixelType::i32sC1: return CV_32SC1;
+  case ze::PixelType::i32sC2: return CV_32SC2;
+  case ze::PixelType::i32sC3: return CV_32SC3;
+  case ze::PixelType::i32sC4: return CV_32SC4;
+  //
+  case ze::PixelType::i32fC1: return CV_32FC1;
+  case ze::PixelType::i32fC2: return CV_32FC2;
+  case ze::PixelType::i32fC3: return CV_32FC3;
+  case ze::PixelType::i32fC4: return CV_32FC4;
+  //
+  default: return 0;
+  }
+}
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/imp_bridge_opencv/src/image_cv.cpp b/RWR/src/ze_oss/imp_bridge_opencv/src/image_cv.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..fdb8c9f1854f747348ce72942bc082a336ccd511
--- /dev/null
+++ b/RWR/src/ze_oss/imp_bridge_opencv/src/image_cv.cpp
@@ -0,0 +1,232 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#include <imp/bridge/opencv/image_cv.hpp>
+
+#include <iostream>
+#include <imp/core/memory_storage.hpp>
+#include <imp/bridge/opencv/cv_connector_pixel_types.hpp>
+
+namespace ze {
+
+//-----------------------------------------------------------------------------
+template<typename Pixel>
+ImageCv<Pixel>::ImageCv(const ze::Size2u& size, ze::PixelOrder pixel_order)
+  : Base(size, pixel_order)
+  , mat_(size[1], size[0], ze::pixelTypeToCv(pixel_type<Pixel>::type))
+{
+  this->header_.pitch = mat_.step;
+  this->header_.memory_type = (MemoryStorage<Pixel>::isAligned(data())) ?
+        MemoryType::CpuAligned : MemoryType::Cpu;}
+
+
+//-----------------------------------------------------------------------------
+template<typename Pixel>
+ImageCv<Pixel>::ImageCv(uint32_t width, uint32_t height,
+                        ze::PixelOrder pixel_order)
+  : ImageCv(ze::Size2u(width, height), pixel_order)
+{
+}
+
+
+//-----------------------------------------------------------------------------
+template<typename Pixel>
+ImageCv<Pixel>::ImageCv(const ImageCv<Pixel>& from)
+  : Base(from)
+  , mat_(from.cvMat())
+{
+  this->header_.pitch = mat_.step;
+  this->header_.memory_type = (MemoryStorage<Pixel>::isAligned(data())) ?
+        MemoryType::CpuAligned : MemoryType::Cpu;
+}
+
+//-----------------------------------------------------------------------------
+template<typename Pixel>
+ImageCv<Pixel>::ImageCv(const Image<Pixel>& from)
+  : Base(from)
+  , mat_(from.height(), from.width(), ze::pixelTypeToCv(pixel_type<Pixel>::type))
+{
+  this->header_.pitch = mat_.step;
+  this->header_.memory_type = (MemoryStorage<Pixel>::isAligned(data())) ?
+        MemoryType::CpuAligned : MemoryType::Cpu;
+  from.copyTo(*this);
+}
+
+//-----------------------------------------------------------------------------
+template<typename Pixel>
+ImageCv<Pixel>::ImageCv(cv::Mat mat, ze::PixelOrder pixel_order)
+  : Base(ze::Size2u(mat.cols, mat.rows), pixel_order)
+  , mat_(mat)
+{
+  this->header_.pitch = mat_.step;
+  this->header_.memory_type = (MemoryStorage<Pixel>::isAligned(data())) ?
+        MemoryType::CpuAligned : MemoryType::Cpu;
+
+  CHECK(this->pixelType() == ze::pixelTypeFromCv(mat_.type()))
+      << "OpenCV pixel type does not match to the internally used one.";
+
+  if (this->pixelOrder() == ze::PixelOrder::undefined)
+  {
+    switch (this->pixelType())
+    {
+    case ze::PixelType::i8uC1:
+    case ze::PixelType::i16uC1:
+    case ze::PixelType::i32fC1:
+    case ze::PixelType::i32sC1:
+      this->header_.pixel_order = ze::PixelOrder::gray;
+      break;
+    case ze::PixelType::i8uC3:
+    case ze::PixelType::i16uC3:
+    case ze::PixelType::i32fC3:
+    case ze::PixelType::i32sC3:
+      this->header_.pixel_order = ze::PixelOrder::bgr;
+      break;
+    case ze::PixelType::i8uC4:
+    case ze::PixelType::i16uC4:
+    case ze::PixelType::i32fC4:
+    case ze::PixelType::i32sC4:
+      this->header_.pixel_order = ze::PixelOrder::bgra;
+      break;
+    default:
+      // if we have something else than 1,3 or 4-channel images, we do not set the
+      // pixel order automatically.
+      VLOG(100) << "Undefined default order for given pixel type. "
+                << "Only 1, 3 and 4 channel images have a default order.";
+      break;
+    }
+  }
+}
+
+
+////-----------------------------------------------------------------------------
+//template<typename Pixel, imp::PixelType pixel_type>
+//ImageCv<Pixel>
+//::ImageCv(Pixel* data, uint32_t width, uint32_t height,
+//           uint32_t pitch, bool use_ext_data_pointer)
+//  : Base(width, height)
+//{
+//  if (data == nullptr)
+//  {
+//    throw imp::Exception("input data not valid", __FILE__, __FUNCTION__, __LINE__);
+//  }
+
+//  if(use_ext_data_pointer)
+//  {
+//    // This uses the external data pointer as internal data pointer.
+//    auto dealloc_nop = [](Pixel* p) { ; };
+//    data_ = std::unique_ptr<pixel_storage_t, Deallocator>(
+//          data, Deallocator(dealloc_nop));
+//    pitch_ = pitch;
+//  }
+//  else
+//  {
+//    data_.reset(MemoryStorage<Pixel>::alignedAlloc(this->width(), this->height(), &pitch_));
+//    size_t stride = pitch / sizeof(pixel_storage_t);
+
+//    if (this->bytes() == pitch*height)
+//    {
+//      std::copy(data, data+stride*height, data_.get());
+//    }
+//    else
+//    {
+//      for (uint32_t y=0; y<height; ++y)
+//      {
+//        for (uint32_t x=0; x<width; ++x)
+//        {
+//          data_.get()[y*this->stride()+x] = data[y*stride + x];
+//        }
+//      }
+//    }
+//  }
+//}
+
+//-----------------------------------------------------------------------------
+template<typename Pixel>
+cv::Mat& ImageCv<Pixel>::cvMat()
+{
+  return mat_;
+}
+
+//-----------------------------------------------------------------------------
+template<typename Pixel>
+const cv::Mat& ImageCv<Pixel>::cvMat() const
+{
+  return mat_;
+}
+
+//-----------------------------------------------------------------------------
+template<typename Pixel>
+Pixel* ImageCv<Pixel>::data(uint32_t ox, uint32_t oy)
+{
+  CHECK_LT(ox, this->width());
+  CHECK_LT(oy, this->height());
+
+  Pixel* buffer = (Pixel*)mat_.data;
+  return &buffer[oy*this->stride() + ox];
+}
+
+//-----------------------------------------------------------------------------
+template<typename Pixel>
+const Pixel* ImageCv<Pixel>::data(uint32_t ox, uint32_t oy) const
+{
+  CHECK_LT(ox, this->width());
+  CHECK_LT(oy, this->height());
+
+  Pixel* buffer = (Pixel*)mat_.data;
+  return reinterpret_cast<const Pixel*>(&buffer[oy*this->stride() + ox]);
+}
+
+//-----------------------------------------------------------------------------
+template<typename Pixel>
+void ImageCv<Pixel>::setValue(const Pixel& value)
+{
+  mat_ = cv::Scalar::all(value);
+}
+
+
+//=============================================================================
+// Explicitely instantiate the desired classes
+// (sync with typedefs at the end of the hpp file)
+template class ImageCv<ze::Pixel8uC1>;
+template class ImageCv<ze::Pixel8uC2>;
+template class ImageCv<ze::Pixel8uC3>;
+template class ImageCv<ze::Pixel8uC4>;
+
+template class ImageCv<ze::Pixel16uC1>;
+template class ImageCv<ze::Pixel16uC2>;
+template class ImageCv<ze::Pixel16uC3>;
+template class ImageCv<ze::Pixel16uC4>;
+
+template class ImageCv<ze::Pixel32sC1>;
+template class ImageCv<ze::Pixel32sC2>;
+template class ImageCv<ze::Pixel32sC3>;
+template class ImageCv<ze::Pixel32sC4>;
+
+template class ImageCv<ze::Pixel32fC1>;
+template class ImageCv<ze::Pixel32fC2>;
+template class ImageCv<ze::Pixel32fC3>;
+template class ImageCv<ze::Pixel32fC4>;
+
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/imp_bridge_opencv/test/test_load_image.cpp b/RWR/src/ze_oss/imp_bridge_opencv/test/test_load_image.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..66301140222b99c02d474e3119dd5f40121dce69
--- /dev/null
+++ b/RWR/src/ze_oss/imp_bridge_opencv/test/test_load_image.cpp
@@ -0,0 +1,40 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#include <functional>
+
+#include <ze/common/test_entrypoint.hpp>
+#include <ze/common/test_utils.hpp>
+#include <imp/bridge/opencv/cv_bridge.hpp>
+
+TEST(ImpBridgeOpenCvTests, testLoad)
+{
+  using namespace ze;
+
+  std::string data_path = getTestDataDir("synthetic_room_pinhole");
+  ImageCv8uC1::Ptr img = cvBridgeLoad8uC1(joinPath(data_path, "img", "1.png"));
+  CHECK(img);
+}
+
+ZE_UNITTEST_ENTRYPOINT
diff --git a/RWR/src/ze_oss/imp_bridge_pangolin/CATKIN_IGNORE b/RWR/src/ze_oss/imp_bridge_pangolin/CATKIN_IGNORE
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/RWR/src/ze_oss/imp_bridge_pangolin/CMakeLists.txt b/RWR/src/ze_oss/imp_bridge_pangolin/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..7ef0295b31fd698c61d7e9082ddb72fff2eb0082
--- /dev/null
+++ b/RWR/src/ze_oss/imp_bridge_pangolin/CMakeLists.txt
@@ -0,0 +1,43 @@
+project(imp_bridge_pangolin)
+cmake_minimum_required(VERSION 2.8.0)
+
+if(${CMAKE_MAJOR_VERSION} VERSION_GREATER 3.0)
+  cmake_policy(SET CMP0054 OLD)
+endif(${CMAKE_MAJOR_VERSION} VERSION_GREATER 3.0)
+
+find_package(catkin_simple REQUIRED)
+catkin_simple(ALL_DEPS_REQUIRED)
+
+include(ze_setup)
+
+# SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -std=c++0x -D__STRICT_ANSI__")
+
+set(HEADERS
+  include/imp/bridge/pangolin/imread.hpp
+  include/imp/bridge/pangolin/pangolin_display.hpp
+  )
+
+set(SOURCES
+  src/empty.cpp
+  )
+
+set(IMPL_FILES
+  )
+
+cs_add_library(${PROJECT_NAME} ${SOURCES} ${HEADERS} ${IMPL_FILES})
+target_link_libraries(${PROJECT_NAME})
+
+###
+### GTESTS
+###
+
+# TODO (MWE)
+
+# catkin_add_gtest(${PROJECT_NAME}-test
+#    test/test_main.cpp
+#    test/todo.cpp
+# )
+# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME} pthread)
+
+cs_install()
+cs_export()
diff --git a/RWR/src/ze_oss/imp_bridge_pangolin/include/imp/bridge/pangolin/imread.hpp b/RWR/src/ze_oss/imp_bridge_pangolin/include/imp/bridge/pangolin/imread.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..4dfdf007bdf834fa2fa985ff0e06c36ffb814827
--- /dev/null
+++ b/RWR/src/ze_oss/imp_bridge_pangolin/include/imp/bridge/pangolin/imread.hpp
@@ -0,0 +1,64 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#pragma once
+
+#include <memory>
+#include <pangolin/image/image_io.h>
+#include <imp/core/image_raw.hpp>
+
+namespace ze
+{
+
+//------------------------------------------------------------------------------
+//template<typename Pixel, imp::PixelType pixel_type>
+void pangolinBridgeLoad(ze::ImageRaw8uC1::Ptr& out,
+                        const std::string& filename, ze::PixelOrder pixel_order)
+{
+  // try to load an image with pangolin first
+  pangolin::TypedImage im = pangolin::LoadImage(
+        filename, pangolin::ImageFileType::ImageFileTypePng);
+
+  //! @todo (MWE) FIX input output channel formatting, etc.
+  out.reset(new ze::ImageRaw8uC1(reinterpret_cast<ze::Pixel8uC1*>(im.ptr),
+                                  im.w, im.h, im.pitch));
+//  switch (im.fmt.channels)
+//  {
+//  case 1:
+//    out = std::make_shared<imp::ImageRaw<Pixel, pixel_type>>(im.w, im.h);
+//    break;
+//  case 3:
+//    out = std::make_shared<imp::ImageRaw<Pixel, pixel_type>>(im.w, im.h);
+//    break;
+//  case 4:
+//    out = std::make_shared<imp::ImageRaw<Pixel, pixel_type>>(im.w, im.h);
+//    break;
+//  default:
+//    throw imp::Exception("Conversion for reading given pixel_type not supported yet.", __FILE__, __FUNCTION__, __LINE__);
+
+//  }
+}
+
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/imp_bridge_pangolin/include/imp/bridge/pangolin/pangolin_bridge.hpp b/RWR/src/ze_oss/imp_bridge_pangolin/include/imp/bridge/pangolin/pangolin_bridge.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..f39643ef48d93f63122e747821cd658b4b012c34
--- /dev/null
+++ b/RWR/src/ze_oss/imp_bridge_pangolin/include/imp/bridge/pangolin/pangolin_bridge.hpp
@@ -0,0 +1,24 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
diff --git a/RWR/src/ze_oss/imp_bridge_pangolin/include/imp/bridge/pangolin/pangolin_display.cpp b/RWR/src/ze_oss/imp_bridge_pangolin/include/imp/bridge/pangolin/pangolin_display.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..1618322903a5e4c24c6d91e67857447aae6047f7
--- /dev/null
+++ b/RWR/src/ze_oss/imp_bridge_pangolin/include/imp/bridge/pangolin/pangolin_display.cpp
@@ -0,0 +1,41 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#include <imp/bridge/pangolin/pangolin_display.hpp>
+
+
+namespace imp
+{
+
+//PangolinDisplay::PangolinDisplay()
+//{
+
+//}
+
+//PangolinDisplay::~PangolinDisplay()
+//{
+
+//}
+
+} // namespace imp
diff --git a/RWR/src/ze_oss/imp_bridge_pangolin/include/imp/bridge/pangolin/pangolin_display.hpp b/RWR/src/ze_oss/imp_bridge_pangolin/include/imp/bridge/pangolin/pangolin_display.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..00af25d9061568202c73c4c607376ee9db2c2af7
--- /dev/null
+++ b/RWR/src/ze_oss/imp_bridge_pangolin/include/imp/bridge/pangolin/pangolin_display.hpp
@@ -0,0 +1,134 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#pragma once
+
+#include <imp/core/size.hpp>
+#include <imp/core/image.hpp>
+#include <pangolin/display/display.h>
+#include <pangolin/display/view.h>
+#include <pangolin/gl/gl.h>
+
+// #include <imp/cu_core/cu_image_gpu.cuh>
+
+namespace ze
+{
+
+////------------------------------------------------------------------------------
+//inline pangolin::View& setupPangolinCudaView(
+//    const imp::Size2u& sz,
+//    const std::string& title = "-")
+//{
+//  pangolin::View& container = pangolin::setupPangolinView(sz, title);
+//  .SetBounds(0, 1.0f, 0, 1.0f);
+
+//  // TODO
+
+//  return container;
+//}
+
+//------------------------------------------------------------------------------
+inline pangolin::View& setupPangolinView(
+    const ze::Size2u& sz,
+    const std::string& title = "-")
+{
+  // Create OpenGL window in single line
+  pangolin::CreateWindowAndBind(title, sz.width(), sz.height());
+
+  if (glewInit() != GLEW_OK )
+  {
+    LOG(ERROR) << "Unable to initialize GLEW." << std::endl;
+  }
+
+  glEnable(GL_DEPTH_TEST); // 3D Mouse handler requires depth testing to be enabled
+  //! @todo (MWE) set OpenGL parameters... which glHint / glEnable options do we need?
+
+  pangolin::View& container = pangolin::CreateDisplay()
+      .SetBounds(0, 1.0f, 0, 1.0f);
+
+  return container;
+}
+
+//------------------------------------------------------------------------------
+inline void setupPangolinViewLayout(
+    pangolin::View& container,
+    int num_tiles=1, std::vector<float> aspect={})
+{
+  container.SetLayout(pangolin::LayoutEqual);
+  for (int i=0; i<num_tiles; ++i)
+  {
+    pangolin::View& v = pangolin::CreateDisplay();
+    if(static_cast<int>(aspect.size())>i)
+    {
+      v.SetAspect(aspect.at(i));
+    }
+    container.AddDisplay(v);
+  }
+
+  //! @todo (MWE) register keypresses, etc.
+}
+
+//------------------------------------------------------------------------------
+inline void imshow(const ze::Image8uC1& im, const std::string& title="-")
+{
+  pangolin::View& container = ze::setupPangolinView(im.size(), title);
+  ze::setupPangolinViewLayout(container, 1, {(float)im.width()/im.height()});
+  pangolin::GlTexture tex8(im.width(), im.height(), GL_LUMINANCE8);
+
+  while(!pangolin::ShouldQuit())
+  {
+    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
+    glColor3f(1,1,1);
+
+    if (container[0].IsShown())
+    {
+      container[0].Activate();
+      tex8.Upload(im.data(), GL_LUMINANCE, GL_UNSIGNED_BYTE);
+      tex8.RenderToViewportFlipY();
+    }
+    pangolin::FinishFrame();
+  }
+}
+
+////------------------------------------------------------------------------------
+//inline void imshow(const imp::cu::ImageGpu8uC1& im, const std::string& title="-")
+//{
+//  pangolin::View& container = imp::setupPangolinView(im.size(), title);
+//  imp::setupPangolinViewLayout(container, 1, {(float)im.width()/im.height()});
+
+//  container[0].SetDrawFunction
+
+
+//}
+
+
+////==============================================================================
+//class PangolinDisplay
+//{
+//public:
+//  PangolinDisplay();
+//  ~PangolinDisplay();
+//};
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/imp_bridge_pangolin/package.xml b/RWR/src/ze_oss/imp_bridge_pangolin/package.xml
new file mode 100644
index 0000000000000000000000000000000000000000..2c4fb98c5c68b197d3860d10d99784b26d5f41ce
--- /dev/null
+++ b/RWR/src/ze_oss/imp_bridge_pangolin/package.xml
@@ -0,0 +1,21 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>imp_bridge_pangolin</name>
+  <version>0.1.4</version>
+  <description>The imp_bridge_pangolin package</description>
+  <maintainer email="code@werlberger.org">Manuel Werlberger</maintainer>
+  <license>TODO</license>
+  <url type="repository">https://github.com/mwerlberger/imp/</url>
+
+
+  <author email="code@werlberger.org">Manuel Werlberger</author>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <buildtool_depend>catkin_simple</buildtool_depend>
+
+  <depend>ze_cmake</depend>
+  <depend>imp_core</depend>
+  <depend>pangolin_catkin</depend>
+
+  <test_depend>gtest</test_depend>
+</package>
diff --git a/RWR/src/ze_oss/imp_bridge_pangolin/src/empty.cpp b/RWR/src/ze_oss/imp_bridge_pangolin/src/empty.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f39643ef48d93f63122e747821cd658b4b012c34
--- /dev/null
+++ b/RWR/src/ze_oss/imp_bridge_pangolin/src/empty.cpp
@@ -0,0 +1,24 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
diff --git a/RWR/src/ze_oss/imp_bridge_ros/CMakeLists.txt b/RWR/src/ze_oss/imp_bridge_ros/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..4843aa5110e67a29b3ca482bd59ca62c40eda013
--- /dev/null
+++ b/RWR/src/ze_oss/imp_bridge_ros/CMakeLists.txt
@@ -0,0 +1,22 @@
+project(imp_bridge_ros)
+cmake_minimum_required(VERSION 2.8.0)
+
+find_package(catkin_simple REQUIRED)
+catkin_simple(ALL_DEPS_REQUIRED)
+
+include(ze_setup)
+
+set(HEADERS
+  include/imp/bridge/ros/cu_ros_bridge.hpp
+  include/imp/bridge/ros/ros_bridge.hpp
+  )
+
+set(SOURCES
+  src/ros_bridge.cpp
+  )
+
+cs_add_library(${PROJECT_NAME} ${SOURCES} ${HEADERS})
+target_link_libraries(${PROJECT_NAME})
+
+cs_install()
+cs_export()
diff --git a/RWR/src/ze_oss/imp_bridge_ros/include/imp/bridge/ros/cu_ros_bridge.hpp b/RWR/src/ze_oss/imp_bridge_ros/include/imp/bridge/ros/cu_ros_bridge.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..b507693d1f6330cc5946419f8f09347752aad67b
--- /dev/null
+++ b/RWR/src/ze_oss/imp_bridge_ros/include/imp/bridge/ros/cu_ros_bridge.hpp
@@ -0,0 +1,116 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#pragma once
+
+#include <sensor_msgs/Image.h>
+#include <sensor_msgs/image_encodings.h>
+
+#include <imp/core/image.hpp>
+#include <imp/cu_core/cu_image_gpu.cuh>
+#include <imp/bridge/ros/ros_bridge.hpp>
+
+namespace ze {
+
+namespace imgenc = sensor_msgs::image_encodings;
+
+//------------------------------------------------------------------------------
+template<typename Pixel>
+cu::ImageGpu<Pixel>::Ptr toImageGpu(
+    const sensor_msgs::Image& src /*, PixelOrder pixel_order*/)
+{
+  PixelType src_pixel_type;
+  PixelOrder src_pixel_order;
+  std::tie(src_pixel_type, src_pixel_order) =
+      getPixelTypeFromRosImageEncoding(src.encoding);
+
+  int bit_depth = imgenc::bitDepth(src.encoding);
+  int num_channels = imgenc::numChannels(src.encoding);
+  uint32_t width = src.width;
+  uint32_t height = src.height;
+  uint32_t pitch = src.step;
+
+  // sanity check
+  CHECK_GE(pitch, width * num_channels * bit_depth/8) << "Input image seem to wrongly formatted";
+
+  switch (src_pixel_type)
+  {
+    case PixelType::i8uC1:
+    {
+      ImageRaw8uC1 src_wrapped(
+          reinterpret_cast<Pixel8uC1*>(const_cast<uint8_t*>(&src.data[0])),
+          width, height, pitch, true);
+      cu::ImageGpu8uC1::Ptr dst =
+          std::make_shared<cu::ImageGpu<Pixel8uC1>>(width, height);
+      dst->copyFrom(src_wrapped);
+      return dst;
+    }
+//  case imp::PixelType::i8uC2:
+//  { } break;
+//  case imp::PixelType::i8uC3:
+//  { } break;
+//  case imp::PixelType::i8uC4:
+//  { } break;
+//  case imp::PixelType::i16uC1:
+//  { } break;
+//  case imp::PixelType::i16uC2:
+//  { } break;
+//  case imp::PixelType::i16uC3:
+//  { } break;
+//  case imp::PixelType::i16uC4:
+//  { } break;
+//  case imp::PixelType::i32uC1:
+//  { } break;
+//  case imp::PixelType::i32uC2:
+//  { } break;
+//  case imp::PixelType::i32uC3:
+//  { } break;
+//  case imp::PixelType::i32uC4:
+//  { } break;
+//  case imp::PixelType::i32sC1:
+//  { } break;
+//  case imp::PixelType::i32sC2:
+//  { } break;
+//  case imp::PixelType::i32sC3:
+//  { } break;
+//  case imp::PixelType::i32sC4:
+//  { } break;
+//  case imp::PixelType::i32fC1:
+//  { } break;
+//  case imp::PixelType::i32fC2:
+//  { } break;
+//  case imp::PixelType::i32fC3:
+//  { } break;
+//  case imp::PixelType::i32fC4:
+//  { } break;
+    default:
+    {
+      LOG(FATAL) << "Unsupported pixel type" + src.encoding + ".";
+      break;
+    }
+  }
+  return nullptr;
+}
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/imp_bridge_ros/include/imp/bridge/ros/ros_bridge.hpp b/RWR/src/ze_oss/imp_bridge_ros/include/imp/bridge/ros/ros_bridge.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..3189954baf2c31e5a346dbd27dc4e20eac5e0f85
--- /dev/null
+++ b/RWR/src/ze_oss/imp_bridge_ros/include/imp/bridge/ros/ros_bridge.hpp
@@ -0,0 +1,43 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#pragma once
+
+#include <sensor_msgs/Image.h>
+#include <imp/core/image.hpp>
+
+namespace ze {
+
+std::pair<PixelType, PixelOrder> getPixelTypeFromRosImageEncoding(
+    const std::string& encoding);
+
+ImageBase::Ptr toImageCpu(
+    const sensor_msgs::Image& src,
+    PixelOrder pixel_order = PixelOrder::undefined);
+
+ImageBase::Ptr toImageGpu(
+    const sensor_msgs::Image& src,
+    PixelOrder pixel_order = PixelOrder::undefined);
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/imp_bridge_ros/package.xml b/RWR/src/ze_oss/imp_bridge_ros/package.xml
new file mode 100644
index 0000000000000000000000000000000000000000..a458b171ab2ad4fcb248f6b28755fc4309769885
--- /dev/null
+++ b/RWR/src/ze_oss/imp_bridge_ros/package.xml
@@ -0,0 +1,22 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>imp_bridge_ros</name>
+  <description>
+    IMP bridge to Ros adds the ability to interconnect with the Ros framework
+  </description>
+  <version>0.1.4</version>
+  <license>ZE</license>
+
+  <maintainer email="code@werlberger.org">Manuel Werlberger</maintainer>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <buildtool_depend>catkin_simple</buildtool_depend>
+
+  <depend>image_transport</depend>
+    
+  <depend>ze_cmake</depend>
+  <depend>ze_common</depend>
+  <depend>imp_core</depend>
+
+  <test_depend>gtest</test_depend>
+</package>
diff --git a/RWR/src/ze_oss/imp_bridge_ros/src/ros_bridge.cpp b/RWR/src/ze_oss/imp_bridge_ros/src/ros_bridge.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..7e6913bf00ad1968b5095d0623ba0f228991ba57
--- /dev/null
+++ b/RWR/src/ze_oss/imp_bridge_ros/src/ros_bridge.cpp
@@ -0,0 +1,163 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#include <imp/bridge/ros/ros_bridge.hpp>
+
+#include <sensor_msgs/image_encodings.h>
+
+#include <imp/core/image_raw.hpp>
+#include <ze/common/logging.hpp>
+#include <ze/common/types.hpp>
+
+
+namespace ze {
+
+namespace imgenc = sensor_msgs::image_encodings;
+
+//------------------------------------------------------------------------------
+std::pair<PixelType, PixelOrder> getPixelTypeFromRosImageEncoding(
+    const std::string& encoding)
+{
+  //! @todo (MWE) we do not support bayer or YUV images yet.
+  if (encoding == imgenc::BGR8)
+  {
+    return std::make_pair(PixelType::i8uC3, PixelOrder::bgr);
+  }
+  else if (encoding == imgenc::MONO8)
+  {
+    return std::make_pair(PixelType::i8uC1, PixelOrder::gray);
+  }
+  else if (encoding == imgenc::RGB8)
+  {
+    return std::make_pair(PixelType::i8uC3, PixelOrder::rgb);
+  }
+  else if (encoding == imgenc::MONO16)
+  {
+    return std::make_pair(PixelType::i16uC1, PixelOrder::gray);
+  }
+  else if (encoding == imgenc::BGR16)
+  {
+    return std::make_pair(PixelType::i16uC3, PixelOrder::bgr);
+  }
+  else if (encoding == imgenc::RGB16)
+  {
+    return std::make_pair(PixelType::i16uC3, PixelOrder::rgb);
+  }
+  else if (encoding == imgenc::BGRA8)
+  {
+    return std::make_pair(PixelType::i8uC4, PixelOrder::bgra);
+  }
+  else if (encoding == imgenc::RGBA8)
+  {
+    return std::make_pair(PixelType::i8uC4, PixelOrder::rgba);
+  }
+  else if (encoding == imgenc::BGRA16)
+  {
+    return std::make_pair(PixelType::i16uC4, PixelOrder::bgra);
+  }
+  else if (encoding == imgenc::RGBA16)
+  {
+    return std::make_pair(PixelType::i16uC4, PixelOrder::rgba);
+  }
+  LOG(FATAL) << "Unsupported image encoding " + encoding + ".";
+  return std::make_pair(PixelType::undefined, PixelOrder::undefined);
+}
+
+//------------------------------------------------------------------------------
+ImageBase::Ptr toImageCpu(
+    const sensor_msgs::Image& src, PixelOrder /*pixel_order*/)
+{
+  PixelType src_pixel_type;
+  PixelOrder src_pixel_order;
+  std::tie(src_pixel_type, src_pixel_order) =
+      getPixelTypeFromRosImageEncoding(src.encoding);
+
+  int bit_depth = imgenc::bitDepth(src.encoding);
+  int num_channels = imgenc::numChannels(src.encoding);
+  uint32_t width = src.width;
+  uint32_t height = src.height;
+  uint32_t pitch = src.step;
+
+  // sanity check
+  CHECK_GE(pitch, width * num_channels * bit_depth/8) << "Input image seem to wrongly formatted";
+
+  switch (src_pixel_type)
+  {
+    case PixelType::i8uC1:
+    {
+      ImageRaw8uC1 src_wrapped(
+          reinterpret_cast<Pixel8uC1*>(const_cast<uint8_t*>(&src.data[0])),
+          width, height, pitch, true, PixelOrder::gray);
+      ImageRaw8uC1::Ptr dst =
+          std::make_shared<ImageRaw8uC1>(src_wrapped); // Deep copy of the image data.
+      return dst;
+    }
+//  case imp::PixelType::i8uC2:
+//  { } break;
+//  case imp::PixelType::i8uC3:
+//  { } break;
+//  case imp::PixelType::i8uC4:
+//  { } break;
+//  case imp::PixelType::i16uC1:
+//  { } break;
+//  case imp::PixelType::i16uC2:
+//  { } break;
+//  case imp::PixelType::i16uC3:
+//  { } break;
+//  case imp::PixelType::i16uC4:
+//  { } break;
+//  case imp::PixelType::i32uC1:
+//  { } break;
+//  case imp::PixelType::i32uC2:
+//  { } break;
+//  case imp::PixelType::i32uC3:
+//  { } break;
+//  case imp::PixelType::i32uC4:
+//  { } break;
+//  case imp::PixelType::i32sC1:
+//  { } break;
+//  case imp::PixelType::i32sC2:
+//  { } break;
+//  case imp::PixelType::i32sC3:
+//  { } break;
+//  case imp::PixelType::i32sC4:
+//  { } break;
+//  case imp::PixelType::i32fC1:
+//  { } break;
+//  case imp::PixelType::i32fC2:
+//  { } break;
+//  case imp::PixelType::i32fC3:
+//  { } break;
+//  case imp::PixelType::i32fC4:
+//  { } break;
+    default:
+    {
+      LOG(FATAL) << "Unsupported pixel type" + src.encoding + ".";
+      break;
+    }
+  }
+  return nullptr;
+}
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/imp_core/CMakeLists.txt b/RWR/src/ze_oss/imp_core/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..c9315ff0a7e51364e97ccd64097211ceec700c61
--- /dev/null
+++ b/RWR/src/ze_oss/imp_core/CMakeLists.txt
@@ -0,0 +1,58 @@
+project(imp_core)
+cmake_minimum_required(VERSION 2.8.0)
+
+if(${CMAKE_MAJOR_VERSION} VERSION_GREATER 3.0)
+  cmake_policy(SET CMP0054 OLD)
+endif(${CMAKE_MAJOR_VERSION} VERSION_GREATER 3.0)
+
+find_package(catkin_simple REQUIRED)
+catkin_simple(ALL_DEPS_REQUIRED)
+
+include(ze_setup)
+
+set(HEADERS
+  include/imp/core/macros.hpp
+  include/imp/core/types.hpp
+
+  include/imp/core/size.hpp
+  include/imp/core/roi.hpp
+
+  include/imp/core/pixel.hpp
+  include/imp/core/pixel_enums.hpp
+  include/imp/core/memory_storage.hpp
+  include/imp/core/linearmemory_base.hpp
+  include/imp/core/linearmemory.hpp
+  include/imp/core/image_header.hpp
+  include/imp/core/image_base.hpp
+  include/imp/core/image.hpp
+  include/imp/core/image_raw.hpp
+  include/imp/core/image_defs.hpp
+)
+
+set(SOURCES
+  src/linearmemory.cpp
+  src/image_raw.cpp
+  )
+
+cs_add_library(${PROJECT_NAME} ${SOURCES} ${HEADERS})
+
+###
+### GTEST
+###
+
+catkin_add_gtest(${PROJECT_NAME}-test
+  test/test_main.cpp
+  test/size_test.cpp
+  test/roi_test.cpp
+  test/vec_test.cpp
+  test/linearmemory_test.cpp
+  test/test_copy.cpp
+)
+target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME} pthread)
+
+catkin_add_gtest(test_image test/test_image.cpp)
+target_link_libraries(test_image ${PROJECT_NAME})
+
+cs_install()
+cs_export()
+
diff --git a/RWR/src/ze_oss/imp_core/include/imp/core/image.hpp b/RWR/src/ze_oss/imp_core/include/imp/core/image.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..64b50874833eb0502935a51f6684c08cce483931
--- /dev/null
+++ b/RWR/src/ze_oss/imp_core/include/imp/core/image.hpp
@@ -0,0 +1,233 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#pragma once
+
+#include <cstdint>
+
+#include <ze/common/types.hpp>
+#include <ze/common/macros.hpp>
+#include <ze/common/logging.hpp>
+#include <imp/core/image_base.hpp>
+#include <imp/core/pixel.hpp>
+
+namespace ze {
+
+template<typename Pixel>
+class Image : public ImageBase
+{
+public:
+  ZE_POINTER_TYPEDEFS(Image);
+  using pixel_t = Pixel;
+
+public:
+  Image() = delete;
+  virtual ~Image() = default;
+
+  /** Returns a pointer to the pixel data.
+   * The pointer can be offset to position \a (ox/oy).
+   * @param[in] ox Horizontal offset of the pointer array.
+   * @param[in] oy Vertical offset of the pointer array.
+   * @return Pointer to the pixel array.
+   */
+  virtual Pixel* data(uint32_t ox = 0, uint32_t oy = 0) = 0;
+  virtual const Pixel* data(uint32_t ox = 0, uint32_t oy = 0) const = 0;
+
+  /** Get Pixel value at position x,y. */
+  Pixel pixel(uint32_t x, uint32_t y) const
+  {
+    return *data(x, y);
+  }
+
+  /** Get Pixel value at position x,y. */
+  Pixel& pixel(uint32_t x, uint32_t y)
+  {
+    return *data(x, y);
+  }
+
+  /** Get Pixel value at position x,y. */
+  Pixel operator()(uint32_t x, uint32_t y) const
+  {
+    return *data(x, y);
+  }
+
+  /** Get reference to pixel value at position x,y. */
+  Pixel& operator()(uint32_t x, uint32_t y)
+  {
+    return *data(x, y);
+  }
+
+  /** Get Pixel value at position x,y. */
+  Pixel operator()(const Vector2i& px) const
+  {
+    return *data(px(0), px(1));
+  }
+
+  /** Get Pointer to beginning of row \a row (y index).
+   * This enables the usage of [y][x] operator.
+   */
+  Pixel* operator[] (uint32_t row)
+  {
+    return data(0,row);
+  }
+  const Pixel* operator[] (uint32_t row) const
+  {
+    return data(0,row);
+  }
+
+
+  /**
+   * @brief setValue Sets image data to the specified \a value.
+   * @param value Value to be set to the whole image data.
+   */
+  virtual void setValue(const Pixel& value)
+  {
+    CHECK(roi() != Roi2u(0,0,0,0)) << "ROI not set. Should not happen when initializing the image header properly.";
+    // safety check if memory is contiguous and roi is not set.
+    if ((this->bytes() == this->pitch()*this->height()) &&
+        (this->roi() == ze::Roi2u(this->size())))
+    {
+      std::fill(this->data(), this->data()+this->stride()*this->height(), value);
+    }
+    else
+    {
+      for (uint32_t y=this->roi().y(); y<this->roi().y()+this->roi().height(); ++y)
+      {
+        std::fill_n(this->data(this->roi().x(),y), this->roi().width(), value);
+      }
+    }
+  }
+
+  /**
+   * @brief copyTo copies the internal image data to another class instance
+   * @param dst Image class that will receive this image's data.
+   * @todo (MWE) handle roi
+   */
+  virtual void copyTo(Image& dst) const
+  {
+    CHECK_EQ(this->width(), dst.width());
+    CHECK_EQ(this->height(), dst.height());
+
+    // check if dst image is on the gpu and the src image is not so we can
+    // use the copyFrom functionality from the dst image as the Image class
+    // doesn't know anything about gpu memory (poor thing)
+    if (dst.isGpuMemory())
+    {
+      dst.copyFrom(*this);
+    }
+    else if (this->bytes() == dst.bytes())
+    {
+      std::copy(this->data(), this->data()+this->stride()*this->height(), dst.data());
+    }
+    else
+    {
+      for (uint32_t y=0; y<this->height(); ++y)
+      {
+        for (uint32_t x=0; x<this->width(); ++x)
+        {
+          dst[y][x] = this->pixel(x,y);
+        }
+      }
+    }
+  }
+
+  /**
+   * @brief copyFrom copies the image data from another class instance to this image
+   * @param from Image class providing the image data.
+   */
+  virtual void copyFrom(const Image& from)
+  {
+    CHECK_EQ(this->size(), from.size());
+
+    if (from.isGpuMemory())
+    {
+      from.copyTo(*this);
+    }
+    else if (this->bytes() == from.bytes())
+    {
+      std::copy(from.data(), from.data()+from.stride()*from.height(), this->data());
+    }
+    else
+    {
+      for (uint32_t y=0; y<this->height(); ++y)
+      {
+        for (uint32_t x=0; x<this->width(); ++x)
+        {
+          (*this)[y][x] = from.pixel(x, y);
+        }
+      }
+    }
+  }
+
+
+protected:
+  Image(ze::PixelOrder pixel_order = ze::PixelOrder::undefined)
+    : ImageBase(pixel_type<Pixel>::type, sizeof(Pixel), pixel_order)
+  {
+  }
+
+  Image(
+      const Size2u &size,
+      PixelOrder pixel_order = ze::PixelOrder::undefined)
+    : ImageBase(pixel_type<Pixel>::type, sizeof(Pixel), pixel_order, size)
+  {
+  }
+
+  Image(
+      uint32_t width, uint32_t height,
+      PixelOrder pixel_order = ze::PixelOrder::undefined)
+    : Image({width, height}, pixel_order)
+  {
+  }
+
+//  Image(const Image& from) = default;
+};
+
+//-----------------------------------------------------------------------------
+// convenience typedefs
+typedef Image<ze::Pixel8uC1> Image8uC1;
+typedef Image<ze::Pixel8uC2> Image8uC2;
+typedef Image<ze::Pixel8uC3> Image8uC3;
+typedef Image<ze::Pixel8uC4> Image8uC4;
+
+typedef Image<ze::Pixel16uC1> Image16uC1;
+typedef Image<ze::Pixel16uC2> Image16uC2;
+typedef Image<ze::Pixel16uC3> Image16uC3;
+typedef Image<ze::Pixel16uC4> Image16uC4;
+
+typedef Image<ze::Pixel32sC1> Image32sC1;
+typedef Image<ze::Pixel32sC2> Image32sC2;
+typedef Image<ze::Pixel32sC3> Image32sC3;
+typedef Image<ze::Pixel32sC4> Image32sC4;
+
+typedef Image<ze::Pixel32fC1> Image32fC1;
+typedef Image<ze::Pixel32fC2> Image32fC2;
+typedef Image<ze::Pixel32fC3> Image32fC3;
+typedef Image<ze::Pixel32fC4> Image32fC4;
+
+// convenience typedefs
+template<typename Pixel>
+using ImagePtr = typename std::shared_ptr<Image<Pixel>>;
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/imp_core/include/imp/core/image_base.hpp b/RWR/src/ze_oss/imp_core/include/imp/core/image_base.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..f411d1472fdc4d615dfa066e56392c9bda675f40
--- /dev/null
+++ b/RWR/src/ze_oss/imp_core/include/imp/core/image_base.hpp
@@ -0,0 +1,205 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#pragma once
+
+#include <memory>
+#include <vector>
+
+#include <ze/common/types.hpp>
+#include <ze/common/macros.hpp>
+#include <imp/core/image_header.hpp>
+#include <imp/core/pixel_enums.hpp>
+#include <imp/core/roi.hpp>
+#include <imp/core/size.hpp>
+#include <imp/core/types.hpp>
+
+namespace ze {
+
+/**
+ * @brief The ImageBase class is the base class of all our image representations.
+ *
+ * It defines the common interface that must be implemented for IMP images
+ *
+ */
+class ImageBase: public std::enable_shared_from_this<ImageBase>
+{
+public:
+  ZE_POINTER_TYPEDEFS(ImageBase);
+
+public:
+  virtual ~ImageBase() = default;
+
+  ImageBase& operator= (const ImageBase &from)
+  {
+    // TODO operator==
+    this->header_ = from.header_;
+    return *this;
+  }
+
+  inline void setRoi(const ze::Roi2u& roi) {header_.roi = roi;}
+
+  /** Returns the element types. */
+  PixelType pixelType() const
+  {
+    return header_.pixel_type;
+  }
+
+  /** Returns the pixel's channel order. */
+  inline PixelOrder pixelOrder() const {return header_.pixel_order;}
+
+  inline Size2u size() const {return header_.size;}
+
+  inline Roi2u roi() const {return header_.roi;}
+
+  /** Returns the distance in bytes between starts of consecutive rows. */
+  inline uint32_t pitch() const {return header_.pitch;}
+
+  inline uint32_t width() const {return header_.size[0];}
+
+  inline uint32_t height() const {return header_.size[1];}
+
+  inline std::uint8_t nChannels() const
+  {
+    switch (header_.pixel_type)
+    {
+    case PixelType::i8uC1:
+    case PixelType::i16uC1:
+    case PixelType::i32uC1:
+    case PixelType::i32sC1:
+    case PixelType::i32fC1:
+      return 1;
+    case PixelType::i8uC2:
+    case PixelType::i16uC2:
+    case PixelType::i32uC2:
+    case PixelType::i32sC2:
+    case PixelType::i32fC2:
+      return 2;
+    case PixelType::i8uC3:
+    case PixelType::i16uC3:
+    case PixelType::i32uC3:
+    case PixelType::i32sC3:
+    case PixelType::i32fC3:
+      return 3;
+    case PixelType::i8uC4:
+    case PixelType::i16uC4:
+    case PixelType::i32uC4:
+    case PixelType::i32sC4:
+    case PixelType::i32fC4:
+      return 4;
+    default:
+      return 0;
+    }
+  }
+
+  /** Returns the number of pixels in the image. */
+  inline size_t numel() const {return (header_.size[0]*header_.size[1]);}
+
+  /** Returns the total amount of bytes saved in the data buffer. */
+  inline size_t bytes() const {return header_.size[1]*header_.pitch;}
+
+  /** Returns the length of a row (not including the padding!) in bytes. */
+  inline size_t rowBytes() const {return header_.size[0]*header_.pixel_size;}
+
+  /** Returns the distance in pixels between starts of consecutive rows. */
+  inline size_t stride() const {return header_.pitch/header_.pixel_size;}
+
+  /** Returns the bit depth of the data pointer. */
+  inline std::uint8_t bitDepth() const {return 8*header_.pixel_size;}
+
+  /** Returns flag if the image data resides on the device/GPU (TRUE) or host/GPU (FALSE) */
+  inline bool isGpuMemory() const {return header_.isGpuMemory();}
+
+
+  /** Cast between image types */
+  template<class DERIVED>
+  inline typename DERIVED::Ptr as()
+  {
+    return std::dynamic_pointer_cast<DERIVED>(shared_from_this());
+  }
+
+  friend std::ostream& operator<<(std::ostream &os, const ImageBase& image);
+
+protected:
+  ImageBase() = delete;
+
+  ImageBase(
+      PixelType pixel_type,
+      uint8_t pixel_size,
+      PixelOrder pixel_order = ze::PixelOrder::undefined)
+    : header_(pixel_type, pixel_size, pixel_order)
+  {
+  }
+
+  ImageBase(
+      PixelType pixel_type,
+      uint8_t pixel_size,
+      PixelOrder pixel_order,
+      const Size2u &size)
+    : header_(pixel_type, pixel_size, pixel_order, size)
+  {
+  }
+
+  ImageBase(
+      PixelType pixel_type,
+      uint8_t pixel_size,
+      PixelOrder pixel_order,
+      uint32_t width,
+      uint32_t height)
+    : ImageBase(pixel_type, pixel_size, pixel_order, {width, height})
+  {
+  }
+
+  ImageBase(const ImageBase &from)
+    : std::enable_shared_from_this<ImageBase>()
+    , header_(from.header_)
+  {
+  }
+
+protected:
+  ImageHeader header_;
+};
+
+inline std::ostream& operator<<(std::ostream &os, const ImageBase& image)
+{
+  os << "size: " << image.width() << "x" << image.height()
+     << "; roi=(" << image.roi().x() << "," << image.roi().y()
+     << "+" << image.roi().width() << "+" << image.roi().height() << ")"
+     << "; stride: " << image.stride() << "; pitch: " << image.pitch()
+     << "; bitDepth: " << (int)image.bitDepth();
+  if (image.isGpuMemory())
+    os << "; (gpu)";
+  else
+    os << "; (cpu)";
+
+  return os;
+}
+
+// convenience typedefs
+using ImageBasePtr = std::shared_ptr<ImageBase>;
+using StampedImage = std::pair<int64_t, ImageBase::Ptr>;
+using StampedImages = std::vector<StampedImage>;
+
+} // namespace ze
+
diff --git a/RWR/src/ze_oss/imp_core/include/imp/core/image_casts.hpp b/RWR/src/ze_oss/imp_core/include/imp/core/image_casts.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..6cb288c73dd86539ce004a8083214501a5c8ba3d
--- /dev/null
+++ b/RWR/src/ze_oss/imp_core/include/imp/core/image_casts.hpp
@@ -0,0 +1,36 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#pragma once
+
+#include <imp/core/image.hpp>
+
+namespace imp {
+
+template<typename Pixel, imp::PixelType pixel_type>
+
+
+
+} // namespace imp
+
diff --git a/RWR/src/ze_oss/imp_core/include/imp/core/image_defs.hpp b/RWR/src/ze_oss/imp_core/include/imp/core/image_defs.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..acded0e6fe1237077644e7a92e420c66e7dd30f7
--- /dev/null
+++ b/RWR/src/ze_oss/imp_core/include/imp/core/image_defs.hpp
@@ -0,0 +1,44 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#pragma once
+
+#include <memory>
+#include <imp/core/pixel_enums.hpp>
+
+namespace ze {
+
+template<typename Pixel, ze::PixelType pixel_type>
+class ImageCv;
+
+template<typename Pixel, ze::PixelType pixel_type>
+using ImageCvPtr = typename std::shared_ptr<ImageCv<Pixel,pixel_type>>;
+
+template<typename Pixel, ze::PixelType pixel_type>
+using ConstImageCvPtrRef = const std::shared_ptr<ImageCv<Pixel,pixel_type>>&;
+
+
+
+} // namespace ze
+
diff --git a/RWR/src/ze_oss/imp_core/include/imp/core/image_header.hpp b/RWR/src/ze_oss/imp_core/include/imp/core/image_header.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..9447c9232dbe15cfd3a808a019ea71e13622277f
--- /dev/null
+++ b/RWR/src/ze_oss/imp_core/include/imp/core/image_header.hpp
@@ -0,0 +1,94 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#pragma once
+
+#include <ze/common/logging.hpp>
+#include <ze/common/types.hpp>
+#include <imp/core/pixel_enums.hpp>
+#include <imp/core/roi.hpp>
+#include <imp/core/size.hpp>
+#include <imp/core/types.hpp>
+
+namespace ze {
+
+//! Defines an image header including all size information. No data included.
+struct ImageHeader
+{
+  PixelType pixel_type{PixelType::undefined};
+  uint8_t pixel_size{0}; //!< Pixel size in bytes.
+  PixelOrder pixel_order{PixelOrder::undefined};
+  Size2u size{0, 0};
+  Roi2u roi{0, 0, 0, 0}; //!< Region of interest. x,y offset and width, height.
+  uint32_t pitch{0}; //!< Row alignment in bytes.
+  MemoryType memory_type{MemoryType::Undefined}; //!< Memory Type.
+
+  ImageHeader() = default;
+  ~ImageHeader() = default;
+
+  ImageHeader(
+      PixelType _pixel_type,
+      uint8_t _pixel_size = 0,
+      PixelOrder _pixel_order = PixelOrder::undefined,
+      Size2u _size = Size2u{0,0},
+      Roi2u _roi = Roi2u{0,0,0,0},
+      uint32_t _pitch = 0,
+      MemoryType _memory_type = MemoryType::Undefined)
+    : pixel_type(_pixel_type)
+    , pixel_size(_pixel_size)
+    , pixel_order(_pixel_order)
+    , size(_size)
+    , roi((_roi != Roi2u{0,0,0,0})? _roi : Roi2u(size))
+    , pitch(_pitch)
+    , memory_type(_memory_type)
+  { ; }
+
+  bool isGpuMemory() const
+  {
+    switch (memory_type)
+    {
+    case MemoryType::Cpu:
+    case MemoryType::CpuAligned:
+      return false;
+    case MemoryType::Gpu:
+    case MemoryType::GpuAligned:
+    case MemoryType::Managed:
+    case MemoryType::ManagedAligned:
+    case MemoryType::Unified:
+    case MemoryType::UnifiedAligned:
+      return true;
+    default:
+      CHECK(false) << "Undefined or unitialized memory";
+      return false;
+    }
+  }
+
+  bool hasRoi() const
+  {
+    return ((roi != Roi2u{0,0,0,0}) && (roi != Roi2u(size)));
+  }
+};
+
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/imp_core/include/imp/core/image_raw.hpp b/RWR/src/ze_oss/imp_core/include/imp/core/image_raw.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..3d98b645defcd349069f1339362832f57c0e0a93
--- /dev/null
+++ b/RWR/src/ze_oss/imp_core/include/imp/core/image_raw.hpp
@@ -0,0 +1,167 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#pragma once
+
+#include <memory>
+#include <algorithm>
+
+#include <ze/common/types.hpp>
+#include <ze/common/macros.hpp>
+#include <imp/core/image.hpp>
+#include <imp/core/memory_storage.hpp>
+
+namespace ze {
+
+/**
+ * @brief The ImageRaw class is an image (surprise) holding raw memory
+ *
+ * The ImageRaw memory can be used to allocate raw memory of a given size, or
+ * take external memory and hold a reference to that. Note that when external
+ * memory is given to the class, the memory won't be managed! You have to take
+ * care about the memory deletion. Instead when you let the class itself allocate
+ * the memory it will take care of freeing the memory again. In addition the allocation
+ * takes care about memory address alignment (default: 32-byte) for the beginning of
+ * every row.
+ *
+ * The template parameters are as follows:
+ *   - Pixel: The pixel's memory representation (e.g. imp::Pixel8uC1 for single-channel unsigned 8-bit images)
+ *   - pixel_type: The internal enum for specifying the pixel's type more specificly
+ */
+template<typename Pixel>
+class ImageRaw : public ze::Image<Pixel>
+{
+public:
+  ZE_POINTER_TYPEDEFS(ImageRaw);
+
+  using Base = Image<Pixel>;
+  using Memory = ze::MemoryStorage<Pixel>;
+  using Deallocator = ze::MemoryDeallocator<Pixel>;
+
+public:
+  ImageRaw() = default;
+  virtual ~ImageRaw() = default;
+
+  /**
+   * @brief ImageRaw construcs an image of given \a size
+   */
+  ImageRaw(const ze::Size2u& size,
+           PixelOrder pixel_order = ze::PixelOrder::undefined);
+
+  /**
+   * @brief ImageRaw construcs an image of given size \a width x \a height
+   */
+  ImageRaw(uint32_t width, uint32_t height,
+           PixelOrder pixel_order = ze::PixelOrder::undefined)
+    : ImageRaw({width, height}, pixel_order)
+  {
+  }
+
+  /**
+   * @brief ImageRaw copy constructs an image from the given image \a from
+   */
+  ImageRaw(const ImageRaw& from);
+
+  /**
+   * @brief ImageRaw copy constructs an arbitrary base image \a from (not necessarily am \a ImageRaw)
+   */
+  ImageRaw(const Base& from);
+
+  /**
+   * @brief ImageRaw constructs an image with the given data (copied or refererenced!)
+   * @param data Pointer to the image data.
+   * @param width Image width.
+   * @param height Image height.
+   * @param pitch Length of a row in bytes (including padding).
+   * @param use_ext_data_pointer Flag controlling whether the image should be copied (false) or if the data is just referenced (true)
+   */
+  ImageRaw(Pixel* data, uint32_t width, uint32_t height,
+           uint32_t pitch, bool use_ext_data_pointer = false,
+           PixelOrder pixel_order = ze::PixelOrder::undefined);
+
+  /**
+   * @brief ImageRaw constructs an image with the given data shared with the given tracked object
+   * @param data Pointer to the image data.
+   * @param width Image width.
+   * @param height Image height.
+   * @param pitch Length of a row in bytes (including padding).
+   * @param tracked Tracked object that shares the given image data
+   * @note we assume that the tracked object takes care about memory deallocations
+   */
+  ImageRaw(Pixel* data, uint32_t width, uint32_t height,
+           uint32_t pitch, const std::shared_ptr<void const>& tracked,
+           PixelOrder pixel_order = ze::PixelOrder::undefined);
+
+
+  /** Returns a pointer to the pixel data.
+   * The pointer can be offset to position \a (ox/oy).
+   * @param[in] ox Horizontal/Column offset of the pointer array.
+   * @param[in] oy Vertical/Row offset of the pointer array.
+   * @return Pointer to the pixel array.
+   */
+  virtual Pixel* data(uint32_t ox = 0, uint32_t oy = 0) override;
+  virtual const Pixel* data(uint32_t ox = 0, uint32_t oy = 0) const override;
+
+protected:
+  std::unique_ptr<Pixel, Deallocator> data_; //!< the actual image data
+  std::shared_ptr<void const> tracked_ = nullptr; //!< tracked object to share memory
+};
+
+//-----------------------------------------------------------------------------
+// convenience typedefs
+// (sync with explicit template class instantiations at the end of the cpp file)
+typedef ImageRaw<ze::Pixel8uC1> ImageRaw8uC1;
+typedef ImageRaw<ze::Pixel8uC2> ImageRaw8uC2;
+typedef ImageRaw<ze::Pixel8uC3> ImageRaw8uC3;
+typedef ImageRaw<ze::Pixel8uC4> ImageRaw8uC4;
+
+typedef ImageRaw<ze::Pixel16sC1> ImageRaw16sC1;
+typedef ImageRaw<ze::Pixel16sC2> ImageRaw16sC2;
+typedef ImageRaw<ze::Pixel16sC3> ImageRaw16sC3;
+typedef ImageRaw<ze::Pixel16sC4> ImageRaw16sC4;
+
+typedef ImageRaw<ze::Pixel16uC1> ImageRaw16uC1;
+typedef ImageRaw<ze::Pixel16uC2> ImageRaw16uC2;
+typedef ImageRaw<ze::Pixel16uC3> ImageRaw16uC3;
+typedef ImageRaw<ze::Pixel16uC4> ImageRaw16uC4;
+
+typedef ImageRaw<ze::Pixel32sC1> ImageRaw32sC1;
+typedef ImageRaw<ze::Pixel32sC2> ImageRaw32sC2;
+typedef ImageRaw<ze::Pixel32sC3> ImageRaw32sC3;
+typedef ImageRaw<ze::Pixel32sC4> ImageRaw32sC4;
+
+typedef ImageRaw<ze::Pixel32fC1> ImageRaw32fC1;
+typedef ImageRaw<ze::Pixel32fC2> ImageRaw32fC2;
+typedef ImageRaw<ze::Pixel32fC3> ImageRaw32fC3;
+typedef ImageRaw<ze::Pixel32fC4> ImageRaw32fC4;
+
+// shared pointers
+template <typename Pixel>
+using ImageRawPtr = typename ImageRaw<Pixel>::Ptr;
+
+template <typename Pixel>
+using ImageRawConstPtr = typename ImageRaw<Pixel>::ConstPtr;
+
+} // namespace ze
+
diff --git a/RWR/src/ze_oss/imp_core/include/imp/core/linearmemory.hpp b/RWR/src/ze_oss/imp_core/include/imp/core/linearmemory.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..b25286d482e91ecdead6e51b1f9f736bfd3e6def
--- /dev/null
+++ b/RWR/src/ze_oss/imp_core/include/imp/core/linearmemory.hpp
@@ -0,0 +1,133 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#pragma once
+
+#include <stdio.h>
+#include <assert.h>
+#include <cstdlib>
+#include <memory>
+
+#include <imp/core/linearmemory_base.hpp>
+#include <imp/core/memory_storage.hpp>
+#include <imp/core/pixel.hpp>
+
+namespace ze {
+
+template<typename Pixel>
+class LinearMemory : public LinearMemoryBase
+{
+public:
+  using Memory = ze::MemoryStorage<Pixel>;
+  using Deallocator = ze::MemoryDeallocator<Pixel>;
+
+public:
+  LinearMemory() = delete;
+  virtual ~LinearMemory() { }
+  LinearMemory(const uint32_t& length);
+  LinearMemory(const LinearMemory<Pixel>& from);
+  LinearMemory(Pixel* host_data, const uint32_t& length,
+               bool use_ext_data_pointer = false);
+
+  /**
+   * @brief Returns a pointer to the device buffer.
+   * @param[in] offset Offset of the pointer array.
+   * @return Pointer to the device buffer.
+   *
+   * @note The pointer can be offset to position \a offset.
+   *
+   */
+  Pixel* data(uint32_t offset = 0);
+
+  /** Returns a const pointer to the device buffer.
+   * @param[in] offset Desired offset within the array.
+   * @return Const pointer to the device buffer.
+   */
+  const Pixel* data(uint32_t offset = 0) const;
+
+  /** Sets a certain value to all pixels in the data vector.
+   */
+  void setValue(const Pixel& value);
+
+  /** Copy data to another class instance.
+   */
+  void copyTo(LinearMemory<Pixel>& dst);
+
+  //! @todo (MWE) operator= for copyTo/copyFrom?
+  LinearMemory<Pixel>& operator=(Pixel rhs);
+
+  /** Returns the total amount of bytes saved in the data buffer. */
+  virtual size_t bytes() const override { return this->length()*sizeof(Pixel); }
+
+  /** Returns the total amount of bytes for the region-of-interest saved in the data buffer. */
+  virtual size_t roiBytes() const override { return this->roi().length()*sizeof(Pixel); }
+
+  /** Returns the bit depth of the data pointer. */
+  virtual std::uint8_t bitDepth() const override { return 8*sizeof(Pixel); }
+
+  /** Returns flag if the image data resides on the device/GPU (TRUE) or host/GPU (FALSE) */
+  virtual bool isGpuMemory() const  override { return false; }
+
+  /** Pixel access with (idx). */
+   inline Pixel& operator()(uint32_t idx) {return *this->data(idx);}
+   /** Pixel access with [idx]. */
+   inline Pixel& operator[](uint32_t idx) {return *this->data(idx);}
+   inline const Pixel& operator[](uint32_t idx) const {return *this->data(idx);}
+
+private:
+  std::unique_ptr<Pixel, Deallocator> data_;
+
+};
+
+// convenience typedefs
+// (sync with explicit template class instantiations at the end of the cpp file)
+typedef LinearMemory<ze::Pixel8uC1> LinearMemory8uC1;
+typedef LinearMemory<ze::Pixel8uC2> LinearMemory8uC2;
+typedef LinearMemory<ze::Pixel8uC3> LinearMemory8uC3;
+typedef LinearMemory<ze::Pixel8uC4> LinearMemory8uC4;
+
+typedef LinearMemory<ze::Pixel16uC1> LinearMemory16uC1;
+typedef LinearMemory<ze::Pixel16uC2> LinearMemory16uC2;
+typedef LinearMemory<ze::Pixel16uC3> LinearMemory16uC3;
+typedef LinearMemory<ze::Pixel16uC4> LinearMemory16uC4;
+
+typedef LinearMemory<ze::Pixel32uC1> LinearMemory32uC1;
+typedef LinearMemory<ze::Pixel32uC2> LinearMemory32uC2;
+typedef LinearMemory<ze::Pixel32uC3> LinearMemory32uC3;
+typedef LinearMemory<ze::Pixel32uC4> LinearMemory32uC4;
+
+typedef LinearMemory<ze::Pixel32sC1> LinearMemory32sC1;
+typedef LinearMemory<ze::Pixel32sC2> LinearMemory32sC2;
+typedef LinearMemory<ze::Pixel32sC3> LinearMemory32sC3;
+typedef LinearMemory<ze::Pixel32sC4> LinearMemory32sC4;
+
+typedef LinearMemory<ze::Pixel32fC1> LinearMemory32fC1;
+typedef LinearMemory<ze::Pixel32fC2> LinearMemory32fC2;
+typedef LinearMemory<ze::Pixel32fC3> LinearMemory32fC3;
+typedef LinearMemory<ze::Pixel32fC4> LinearMemory32fC4;
+
+
+
+} // namespace ze
+
diff --git a/RWR/src/ze_oss/imp_core/include/imp/core/linearmemory_base.hpp b/RWR/src/ze_oss/imp_core/include/imp/core/linearmemory_base.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..91fdb361e6d5e7162e9dd477008ba8e7c2d14bf0
--- /dev/null
+++ b/RWR/src/ze_oss/imp_core/include/imp/core/linearmemory_base.hpp
@@ -0,0 +1,96 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#pragma once
+
+#include <cstdint>
+#include <imp/core/size.hpp>
+#include <imp/core/roi.hpp>
+
+namespace ze {
+
+/** \brief LinearMemory Base class for linear memory classes.
+  */
+class LinearMemoryBase
+{
+protected:
+  LinearMemoryBase() = delete;
+
+  LinearMemoryBase(const LinearMemoryBase& from)
+    : size_(from.size_)
+    , roi_(from.roi_)
+  { }
+
+  LinearMemoryBase(const uint32_t& length)
+    : size_(length)
+    , roi_(0, length)
+  { }
+
+
+public:
+  virtual ~LinearMemoryBase()
+  { }
+
+  /** Returns the number of elements available in the internal buffer. */
+  uint32_t length() const
+  {
+    return size_.length();
+  }
+
+  inline ze::Size1u size() const { return size_; }
+  inline ze::Roi1u roi() const { return roi_; }
+
+  /** Sets a region-of-interest (clamps towards boundaries [0, length[) */
+  void setRoi(const ze::Roi1u& roi)
+  {
+    roi_.x() = std::max(0u, std::min(this->length()-1, roi.x()));
+    uint32_t remaining_elements = this->length()-roi_.x();
+    roi_.length() = std::max(1u, std::min(remaining_elements, roi.length()));
+  }
+
+  void resetRoi()
+  {
+    roi_ = ze::Roi1u(size_);
+  }
+
+  /** Returns the total amount of bytes saved in the data buffer. */
+  virtual size_t bytes() const = 0;
+
+  /** Returns the total amount of bytes for the region-of-interest saved in the data buffer. */
+  virtual size_t roiBytes() const = 0;
+
+  /** Returns the bit depth of the data pointer. */
+  virtual std::uint8_t bitDepth() const = 0;
+
+  /** Returns flag if the image data resides on the GPU (TRUE) or CPU (FALSE) */
+  virtual bool isGpuMemory() const = 0;
+
+protected:
+  ze::Size1u size_;
+  ze::Roi1u roi_;
+
+};
+
+} // namespace ze
+
diff --git a/RWR/src/ze_oss/imp_core/include/imp/core/macros.hpp b/RWR/src/ze_oss/imp_core/include/imp/core/macros.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..b58c88a4c4d0593f910803f60ebfba4650efe7cf
--- /dev/null
+++ b/RWR/src/ze_oss/imp_core/include/imp/core/macros.hpp
@@ -0,0 +1,40 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#pragma once
+
+// from Simon's multi-agent mapping framework
+#define IMP_USING_INTERNAL_SMART_POINTERS(TypeName) \
+  using Ptr = std::shared_ptr<TypeName>;              \
+  using ConstPtr = std::shared_ptr<const TypeName>;   \
+  using UniquePtr = std::unique_ptr<TypeName>;        \
+  void defineUsingInternalSharedPointers##__FILE__##__LINE__(void)
+
+#define IMP_USING_EXTERNAL_SMART_POINTERS(TypeName) \
+  using Ptr = std::shared_ptr<TypeName>;              \
+  using ConstPtr = std::shared_ptr<const TypeName>;   \
+  using UniquePtr = std::unique_ptr<TypeName>;        \
+  void defineUsingInternalSharedPointers##__FILE__##__LINE__(void)
+
+
diff --git a/RWR/src/ze_oss/imp_core/include/imp/core/memory_storage.hpp b/RWR/src/ze_oss/imp_core/include/imp/core/memory_storage.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..2ffafd08f28669562dabaf80eccc5c36e2c876eb
--- /dev/null
+++ b/RWR/src/ze_oss/imp_core/include/imp/core/memory_storage.hpp
@@ -0,0 +1,172 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#pragma once
+
+#include <stdlib.h>
+#include <assert.h>
+#include <math.h>
+#include <functional>
+#include <algorithm>
+
+#include <ze/common/logging.hpp>
+#include <ze/common/types.hpp>
+#include <imp/core/size.hpp>
+#include <imp/core/types.hpp>
+
+namespace ze {
+
+//--------------------------------------------------------------------------
+template <typename Pixel, int memaddr_align=32, bool align_rows=true>
+struct MemoryStorage
+{
+public:
+  MemoryStorage() = delete;
+  virtual ~MemoryStorage() = delete;
+
+  /**
+   * @brief isAligned checks if the given data pointer \a p is aligned according to \a memaddr_align
+   */
+  static bool isAligned(void* p)
+  {
+    return reinterpret_cast<intptr_t>(p) % memaddr_align == 0;
+  }
+
+  /**
+   * @brief alignedAlloc allocates an aligned block of memory
+   * @param num_elements Number of (minimum) allocated elements
+   * @param init_with_zeros Flag if the memory elements should be zeroed out (default=false).
+   *
+   * @note Internally we use the C11 function aligned_alloc although there
+   *       are also alignment functions in C++11 but aligned_alloc is the only
+   *       one where we don't have to mess around with allocated bigger chuncks of
+   *       memory and shifting the start address accordingly. If you know a
+   *       better approach using e.g. std::align(), let me know.
+   */
+  static Pixel* alignedAlloc(const uint32_t num_elements,
+                             bool init_with_zeros=false)
+  {
+    CHECK_GT(num_elements, 0u) << "Failed to allocate memory: num_elements=0";
+
+    // restrict the memory address alignment to be in the interval ]0,128] and
+    // of power-of-two using the 'complement and compare' method
+    assert((memaddr_align != 0) && memaddr_align <= 128 &&
+           ((memaddr_align & (~memaddr_align + 1)) == memaddr_align));
+
+    const uint32_t memory_size = sizeof(Pixel) * num_elements;
+
+    Pixel* p_data_aligned;
+    int ret = posix_memalign((void**)&p_data_aligned, memaddr_align, memory_size);
+    // alternatively one could use -- benchmark before you change this
+    // Pixel* p_data_aligned =
+    //     (Pixel*)aligned_alloc(memaddr_align, memory_size);
+
+    if (p_data_aligned == nullptr || ret != 0)
+    {
+      throw std::bad_alloc();
+    }
+
+    if (init_with_zeros)
+    {
+      std::fill(p_data_aligned, p_data_aligned+num_elements, Pixel(0));
+    }
+
+    return (Pixel*)p_data_aligned;
+  }
+
+  /**
+   * @brief alignedAlloc allocates an aligned block of memory that guarantees to host the image of size \a size
+   * @param size Image size
+   * @param pitch Row alignment [bytes] if padding is needed.
+   * @param init_with_zeros Flag if the memory elements should be zeroed out (default=false).
+   * @return
+   */
+  static Pixel* alignedAlloc(
+      ze::Size2u size, uint32_t* pitch, bool init_with_zeros=false)
+  {
+    CHECK_GT(size.width(), 0u);
+    CHECK_GT(size.height(), 0u);
+
+    // restrict the memory address alignment to be in the interval ]0,128] and
+    // of power-of-two using the 'complement and compare' method
+    assert((memaddr_align != 0) && memaddr_align <= 128 &&
+           ((memaddr_align & (~memaddr_align + 1)) == memaddr_align));
+
+    // check if the width allows a correct alignment of every row, otherwise add padding
+    const uint32_t width_bytes = size.width() * sizeof(Pixel);
+    // bytes % memaddr_align = 0 for bytes=n*memaddr_align is the reason for
+    // the decrement in the following compution:
+    const uint32_t bytes_to_add = (memaddr_align-1) - ((width_bytes-1) % memaddr_align);
+    const uint32_t pitched_width = size.width() + bytes_to_add/sizeof(Pixel);
+    *pitch = width_bytes + bytes_to_add;
+    return alignedAlloc(pitched_width*size.height(), init_with_zeros);
+  }
+
+
+  /**
+   * @brief free releases the pixel \a buffer
+   * @param buffer
+   */
+  static void free(Pixel* buffer)
+  {
+    free(buffer);
+  }
+}; // struct MemoryStorage
+
+/**
+ * @brief The MemoryDeallocator struct offers the ability to have custom deallocation methods.
+ *
+ * The Deallocator struct can be used as e.g. having custom deallocations with
+ * shared pointers. Furthermore it enables the usage of external memory buffers
+ * using shared pointers but not taking ownership of the memory. Be careful when
+ * doing so as an application would behave badly if the memory got deleted although
+ * we are still using it.
+ *
+ */
+template<typename Pixel>
+class MemoryDeallocator
+{
+public:
+  // Default custom deleter assuming we use arrays (new PixelType[length])
+  MemoryDeallocator()
+  {
+    f_ = [](Pixel* p) {free(p);};
+  }
+
+  // allow us to define a custom deallocator
+  explicit MemoryDeallocator(std::function<void(Pixel*)> const &f)
+    : f_(f)
+  { }
+
+  void operator()(Pixel* p) const
+  {
+    f_(p);
+  }
+
+private:
+  std::function< void(Pixel* )> f_;
+}; // MemoryDeallocator
+
+} // imp
+
diff --git a/RWR/src/ze_oss/imp_core/include/imp/core/pixel.hpp b/RWR/src/ze_oss/imp_core/include/imp/core/pixel.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..0765dbb74375b14fd0d8fa9c95bdf30bfeeba555
--- /dev/null
+++ b/RWR/src/ze_oss/imp_core/include/imp/core/pixel.hpp
@@ -0,0 +1,446 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#pragma once
+
+#include <cstdint>
+#include <cmath>
+#include <imp/core/pixel_enums.hpp>
+#include <ze/common/types.hpp>
+
+#ifdef WITH_CUDA
+#  include<cuda_runtime_api.h>
+#  define CUDA_HOST __host__
+#  define CUDA_DEVICE  __device__
+#else
+#  define CUDA_HOST
+#  define CUDA_DEVICE
+#endif
+
+namespace ze {
+
+//------------------------------------------------------------------------------
+template<typename _T>
+union Pixel1
+{
+  using T = _T;
+
+  struct
+  {
+    T x;
+  };
+  struct
+  {
+    T r;
+  };
+  T c[1];
+
+  CUDA_HOST CUDA_DEVICE Pixel1() : x(0) { }
+  CUDA_HOST CUDA_DEVICE Pixel1(T _x) : x(_x) { }
+  CUDA_HOST CUDA_DEVICE ~Pixel1() = default;
+
+  CUDA_HOST CUDA_DEVICE constexpr std::uint8_t numDims() const {return 1;}
+
+  CUDA_HOST CUDA_DEVICE operator T() const { return c[0]; }
+  CUDA_HOST CUDA_DEVICE T& operator[](size_t i) { return c[i]; }
+  CUDA_HOST CUDA_DEVICE const T& operator[](size_t i) const { return c[i]; }
+  CUDA_HOST CUDA_DEVICE Pixel1<T>& operator+=(const Pixel1<T>& rhs)
+  {
+    c[0] += rhs[0];
+    return *this;
+  }
+  CUDA_HOST CUDA_DEVICE Pixel1<T>& operator*=(const Pixel1<T>& rhs)
+  {
+    c[0] *= rhs[0];
+    return *this;
+  }
+  template<typename TRHS>
+  CUDA_HOST CUDA_DEVICE Pixel1<T>& operator*=(const TRHS& rhs)
+  {
+    c[0] *= rhs;
+    return *this;
+  }
+  template<typename TRHS>
+  CUDA_HOST CUDA_DEVICE Pixel1<T>& operator/(const TRHS& rhs)
+  {
+    c[0] /= rhs;
+    return *this;
+  }
+};
+
+//------------------------------------------------------------------------------
+template<typename _T>
+union Pixel2
+{
+  using T = _T;
+
+  struct
+  {
+    T x,y;
+  };
+  struct
+  {
+    T r,g;
+  };
+  T c[2];
+
+  CUDA_HOST CUDA_DEVICE Pixel2() : x(0), y(0) { }
+  CUDA_HOST CUDA_DEVICE Pixel2(T _a) : x(_a), y(_a) { }
+  CUDA_HOST CUDA_DEVICE Pixel2(T _x, T _y) : x(_x), y(_y) { }
+  CUDA_HOST CUDA_DEVICE ~Pixel2() = default;
+
+  CUDA_HOST CUDA_DEVICE constexpr std::uint8_t numDims() const {return 2;}
+
+  CUDA_HOST CUDA_DEVICE operator T() const { return c[0]; }
+  CUDA_HOST CUDA_DEVICE T& operator[](size_t i) { return c[i]; }
+  CUDA_HOST CUDA_DEVICE const T& operator[](size_t i) const { return c[i]; }
+  CUDA_HOST CUDA_DEVICE Pixel2<T>& operator+=(const Pixel2<T>& rhs)
+  {
+    c[0] += rhs[0];
+    c[1] += rhs[1];
+    return *this;
+  }
+  CUDA_HOST CUDA_DEVICE Pixel2<T>& operator*=(const Pixel1<T>& rhs)
+  {
+    c[0] *= rhs[0];
+    c[1] *= rhs[0];
+    return *this;
+  }
+  CUDA_HOST CUDA_DEVICE Pixel2<T>& operator*=(const Pixel2<T>& rhs)
+  {
+    c[0] *= rhs[0];
+    c[1] *= rhs[1];
+    return *this;
+  }
+  template<typename TRHS>
+  CUDA_HOST CUDA_DEVICE Pixel2<T>& operator*=(const TRHS& rhs)
+  {
+    c[0] *= rhs;
+    c[1] *= rhs;
+    return *this;
+  }
+  template<typename TRHS>
+  CUDA_HOST CUDA_DEVICE Pixel2<T>& operator/(const TRHS& rhs)
+  {
+    c[0] /= rhs;
+    c[1] /= rhs;
+    return *this;
+  }
+};
+
+//------------------------------------------------------------------------------
+template<typename _T>
+union Pixel3
+{
+  using T = _T;
+
+  struct
+  {
+    T x,y,z;
+  };
+  struct
+  {
+    T r,g,b;
+  };
+  T c[3];
+
+  CUDA_HOST CUDA_DEVICE Pixel3() : x(0), y(0), z(0) { }
+  CUDA_HOST CUDA_DEVICE Pixel3(T _a) : x(_a), y(_a), z(_a) { }
+  CUDA_HOST CUDA_DEVICE Pixel3(T _x, T _y, T _z) : x(_x), y(_y), z(_z) { }
+  CUDA_HOST CUDA_DEVICE ~Pixel3() = default;
+
+  CUDA_HOST CUDA_DEVICE constexpr std::uint8_t numDims() const {return 3;}
+
+  CUDA_HOST CUDA_DEVICE operator T() const { return c[0]; }
+  CUDA_HOST CUDA_DEVICE T& operator[](size_t i) { return c[i]; }
+  CUDA_HOST CUDA_DEVICE const T& operator[](size_t i) const { return c[i]; }
+  CUDA_HOST CUDA_DEVICE Pixel3<T>& operator+=(const Pixel3<T>& rhs)
+  {
+    c[0] += rhs[0];
+    c[1] += rhs[1];
+    c[2] += rhs[2];
+    return *this;
+  }
+  CUDA_HOST CUDA_DEVICE Pixel3<T>& operator*=(const Pixel1<T>& rhs)
+  {
+    c[0] *= rhs[0];
+    c[1] *= rhs[0];
+    c[2] *= rhs[0];
+    return *this;
+  }
+  CUDA_HOST CUDA_DEVICE Pixel3<T>& operator*=(const Pixel3<T>& rhs)
+  {
+    c[0] *= rhs[0];
+    c[1] *= rhs[1];
+    c[2] *= rhs[2];
+    return *this;
+  }
+  template<typename TRHS>
+  CUDA_HOST CUDA_DEVICE Pixel3<T>& operator*=(const TRHS& rhs)
+  {
+    c[0] *= rhs;
+    c[1] *= rhs;
+    c[2] *= rhs;
+    return *this;
+  }
+  template<typename TRHS>
+  CUDA_HOST CUDA_DEVICE Pixel3<T>& operator/(const TRHS& rhs)
+  {
+    c[0] /= rhs;
+    c[1] /= rhs;
+    c[2] /= rhs;
+    return *this;
+  }
+};
+
+//------------------------------------------------------------------------------
+template<typename _T>
+union Pixel4
+{
+  using T = _T;
+
+  struct
+  {
+    T x,y,z,w;
+  };
+  struct
+  {
+    T r,g,b,a;
+  };
+  T c[4];
+
+  CUDA_HOST CUDA_DEVICE Pixel4() : x(0), y(0), z(0), w(0) { }
+  CUDA_HOST CUDA_DEVICE Pixel4(T _a) : x(_a), y(_a), z(_a), w(_a) { }
+  CUDA_HOST CUDA_DEVICE Pixel4(T _x, T _y, T _z, T _w) : x(_x), y(_y), z(_z), w(_w) { }
+  CUDA_HOST CUDA_DEVICE ~Pixel4() = default;
+
+  CUDA_HOST CUDA_DEVICE constexpr std::uint8_t numDims() const {return 4;}
+
+  CUDA_HOST CUDA_DEVICE operator T() const { return c[0]; }
+  CUDA_HOST CUDA_DEVICE T& operator[](size_t i) { return c[i]; }
+  CUDA_HOST CUDA_DEVICE const T& operator[](size_t i) const { return c[i]; }
+  CUDA_HOST CUDA_DEVICE Pixel4<T>& operator+=(const Pixel4<T>& rhs)
+  {
+    c[0] += rhs[0];
+    c[1] += rhs[1];
+    c[2] += rhs[2];
+    c[3] += rhs[3];
+    return *this;
+  }
+  CUDA_HOST CUDA_DEVICE Pixel4<T>& operator*=(const Pixel1<T>& rhs)
+  {
+    c[0] *= rhs[0];
+    c[1] *= rhs[0];
+    c[2] *= rhs[0];
+    c[3] *= rhs[0];
+    return *this;
+  }
+  CUDA_HOST CUDA_DEVICE Pixel4<T>& operator*=(const Pixel4<T>& rhs)
+  {
+    c[0] *= rhs[0];
+    c[1] *= rhs[1];
+    c[2] *= rhs[2];
+    c[3] *= rhs[3];
+    return *this;
+  }
+  template<typename TRHS>
+  CUDA_HOST CUDA_DEVICE Pixel4<T>& operator*=(const TRHS& rhs)
+  {
+    c[0] *= rhs;
+    c[1] *= rhs;
+    c[2] *= rhs;
+    c[3] *= rhs;
+    return *this;
+  }
+  template<typename TRHS>
+  CUDA_HOST CUDA_DEVICE Pixel4<T>& operator/(const TRHS& rhs)
+  {
+    c[0] /= rhs;
+    c[1] /= rhs;
+    c[2] /= rhs;
+    c[3] /= rhs;
+    return *this;
+  }
+};
+
+//------------------------------------------------------------------------------
+// convenience typedefs
+typedef Pixel1<uint8_t> Pixel8uC1;
+typedef Pixel2<uint8_t> Pixel8uC2;
+typedef Pixel3<uint8_t> Pixel8uC3;
+typedef Pixel4<uint8_t> Pixel8uC4;
+
+typedef Pixel1<int16_t> Pixel16sC1;
+typedef Pixel2<int16_t> Pixel16sC2;
+typedef Pixel3<int16_t> Pixel16sC3;
+typedef Pixel4<int16_t> Pixel16sC4;
+
+typedef Pixel1<uint16_t> Pixel16uC1;
+typedef Pixel2<uint16_t> Pixel16uC2;
+typedef Pixel3<uint16_t> Pixel16uC3;
+typedef Pixel4<uint16_t> Pixel16uC4;
+
+typedef Pixel1<int32_t> Pixel32sC1;
+typedef Pixel2<int32_t> Pixel32sC2;
+typedef Pixel3<int32_t> Pixel32sC3;
+typedef Pixel4<int32_t> Pixel32sC4;
+
+typedef Pixel1<uint32_t> Pixel32uC1;
+typedef Pixel2<uint32_t> Pixel32uC2;
+typedef Pixel3<uint32_t> Pixel32uC3;
+typedef Pixel4<uint32_t> Pixel32uC4;
+
+typedef Pixel1<float> Pixel32fC1;
+typedef Pixel2<float> Pixel32fC2;
+typedef Pixel3<float> Pixel32fC3;
+typedef Pixel4<float> Pixel32fC4;
+
+
+// vector types (same as pixel)
+template<typename T> using Vec1 = Pixel1<T>;
+template<typename T> using Vec2 = Pixel2<T>;
+template<typename T> using Vec3 = Pixel3<T>;
+template<typename T> using Vec4 = Pixel4<T>;
+
+using Vec8uC1 = Pixel8uC1;
+using Vec8uC2 = Pixel8uC2;
+using Vec8uC3 = Pixel8uC3;
+using Vec8uC4 = Pixel8uC4;
+
+using Vec16uC1 = Pixel16sC1;
+using Vec16uC2 = Pixel16sC2;
+using Vec16uC3 = Pixel16sC3;
+using Vec16uC4 = Pixel16sC4;
+
+using Vec32sC1 = Pixel32sC1;
+using Vec32sC2 = Pixel32sC2;
+using Vec32sC3 = Pixel32sC3;
+using Vec32sC4 = Pixel32sC4;
+
+using Vec32uC1 = Pixel32uC1;
+using Vec32uC2 = Pixel32uC2;
+using Vec32uC3 = Pixel32uC3;
+using Vec32uC4 = Pixel32uC4;
+
+using Vec32fC1 = Pixel32fC1;
+using Vec32fC2 = Pixel32fC2;
+using Vec32fC3 = Pixel32fC3;
+using Vec32fC4 = Pixel32fC4;
+
+//------------------------------------------------------------------------------
+// Pixel traits.
+template<typename> struct pixel_type { static constexpr PixelType type = PixelType::undefined; };
+template<> struct pixel_type <Pixel8uC1> { static constexpr PixelType type = PixelType::i8uC1; };
+template<> struct pixel_type <Pixel8uC2> { static constexpr PixelType type = PixelType::i8uC2; };
+template<> struct pixel_type <Pixel8uC3> { static constexpr PixelType type = PixelType::i8uC3; };
+template<> struct pixel_type <Pixel8uC4> { static constexpr PixelType type = PixelType::i8uC4; };
+
+template<> struct pixel_type <Pixel16sC1> { static constexpr PixelType type = PixelType::i16sC1; };
+template<> struct pixel_type <Pixel16sC2> { static constexpr PixelType type = PixelType::i16sC2; };
+template<> struct pixel_type <Pixel16sC3> { static constexpr PixelType type = PixelType::i16sC3; };
+template<> struct pixel_type <Pixel16sC4> { static constexpr PixelType type = PixelType::i16sC4; };
+
+template<> struct pixel_type <Pixel16uC1> { static constexpr PixelType type = PixelType::i16uC1; };
+template<> struct pixel_type <Pixel16uC2> { static constexpr PixelType type = PixelType::i16uC2; };
+template<> struct pixel_type <Pixel16uC3> { static constexpr PixelType type = PixelType::i16uC3; };
+template<> struct pixel_type <Pixel16uC4> { static constexpr PixelType type = PixelType::i16uC4; };
+
+template<> struct pixel_type <Pixel32uC1> { static constexpr PixelType type = PixelType::i32uC1; };
+template<> struct pixel_type <Pixel32uC2> { static constexpr PixelType type = PixelType::i32uC2; };
+template<> struct pixel_type <Pixel32uC3> { static constexpr PixelType type = PixelType::i32uC3; };
+template<> struct pixel_type <Pixel32uC4> { static constexpr PixelType type = PixelType::i32uC4; };
+
+template<> struct pixel_type <Pixel32sC1> { static constexpr PixelType type = PixelType::i32sC1; };
+template<> struct pixel_type <Pixel32sC2> { static constexpr PixelType type = PixelType::i32sC2; };
+template<> struct pixel_type <Pixel32sC3> { static constexpr PixelType type = PixelType::i32sC3; };
+template<> struct pixel_type <Pixel32sC4> { static constexpr PixelType type = PixelType::i32sC4; };
+
+template<> struct pixel_type <Pixel32fC1> { static constexpr PixelType type = PixelType::i32fC1; };
+template<> struct pixel_type <Pixel32fC2> { static constexpr PixelType type = PixelType::i32fC2; };
+template<> struct pixel_type <Pixel32fC3> { static constexpr PixelType type = PixelType::i32fC3; };
+template<> struct pixel_type <Pixel32fC4> { static constexpr PixelType type = PixelType::i32fC4; };
+
+//------------------------------------------------------------------------------
+// comparison operators
+template<typename T>
+inline bool operator==(const Pixel1<T>& lhs, const Pixel1<T>& rhs)
+{
+  return (lhs.x == rhs.x);
+}
+
+//------------------------------------------------------------------------------
+// dot product
+
+template<typename T>
+inline CUDA_HOST CUDA_DEVICE T dot(Vec2<T> a, Vec2<T> b)
+{
+  return a.x * b.x + a.y * b.y;
+}
+template<typename T>
+inline CUDA_HOST CUDA_DEVICE T dot(Vec3<T> a, Vec3<T> b)
+{
+  return a.x * b.x + a.y * b.y + a.z * b.z;
+}
+template<typename T>
+inline CUDA_HOST CUDA_DEVICE T dot(Vec4<T> a, Vec4<T> b)
+{
+  return a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w;
+}
+
+//------------------------------------------------------------------------------
+// length
+template<typename T>
+inline CUDA_HOST CUDA_DEVICE float length(T v)
+{
+  return std::sqrt((float)dot(v,v));
+}
+
+//------------------------------------------------------------------------------
+//normalize
+
+template<typename T>
+inline CUDA_HOST CUDA_DEVICE Vec32fC2 normalize(Vec2<T> v)
+{
+  float inv_len = 1.0f/length(v);
+  return Vec32fC2(v.x*inv_len, v.y*inv_len);
+}
+template<typename T>
+inline CUDA_HOST CUDA_DEVICE Vec32fC3 normalize(Vec3<T> v)
+{
+  float inv_len = 1.0f/length(v);
+  return Vec32fC3(v.x*inv_len, v.y*inv_len, v.z*inv_len);
+}
+template<typename T>
+inline CUDA_HOST CUDA_DEVICE Vec32fC4 normalize(Vec4<T> v)
+{
+  float inv_len = 1.0f/length(v);
+  return Vec32fC4(v.x*inv_len, v.y*inv_len, v.z*inv_len, v.w*inv_len);
+}
+
+
+} // namespace ze
+
+#undef CUDA_HOST
+#undef CUDA_DEVICE
+
diff --git a/RWR/src/ze_oss/imp_core/include/imp/core/pixel_enums.hpp b/RWR/src/ze_oss/imp_core/include/imp/core/pixel_enums.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..b6ad0a8ff3b607db26a46bfddb36f38ae0819b54
--- /dev/null
+++ b/RWR/src/ze_oss/imp_core/include/imp/core/pixel_enums.hpp
@@ -0,0 +1,76 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#pragma once
+
+namespace ze {
+
+/**
+ * @brief The PixelType enum defines a pixel's bit depth and number of channels.
+ */
+enum class PixelType
+{
+  undefined = -1,
+  i8uC1,  //!< interleaved, 8-bit unsigned, 1 channel
+  i8uC2,  //!< interleaved, 8-bit unsigned, 2 channel
+  i8uC3,  //!< interleaved, 8-bit unsigned, 3 channel
+  i8uC4,  //!< interleaved, 8-bit unsigned, 4 channel
+  i16sC1, //!< interleaved, 16-bit signed, 1 channel
+  i16sC2, //!< interleaved, 16-bit signed, 2 channel
+  i16sC3, //!< interleaved, 16-bit signed, 3 channel
+  i16sC4, //!< interleaved, 16-bit signed, 4 channel
+  i16uC1, //!< interleaved, 16-bit unsigned, 1 channel
+  i16uC2, //!< interleaved, 16-bit unsigned, 2 channel
+  i16uC3, //!< interleaved, 16-bit unsigned, 3 channel
+  i16uC4, //!< interleaved, 16-bit unsigned, 4 channel
+  i32uC1, //!< interleaved, 32-bit unsigned, 1 channel
+  i32uC2, //!< interleaved, 32-bit unsigned, 2 channel
+  i32uC3, //!< interleaved, 32-bit unsigned, 3 channel
+  i32uC4, //!< interleaved, 32-bit unsigned, 4 channel
+  i32sC1, //!< interleaved, 32-bit signed, 1 channel
+  i32sC2, //!< interleaved, 32-bit signed, 2 channel
+  i32sC3, //!< interleaved, 32-bit signed, 3 channel
+  i32sC4, //!< interleaved, 32-bit signed, 4 channel
+  i32fC1, //!< interleaved, 32-bit float, 1 channel
+  i32fC2, //!< interleaved, 32-bit float, 2 channel
+  i32fC3, //!< interleaved, 32-bit float, 3 channel
+  i32fC4  //!< interleaved, 32-bit float, 4 channel
+};
+
+/**
+ * @brief The PixelOrder enum defines a pixel's channel ordering (for interleaved pixels).
+ */
+enum class PixelOrder
+{
+  undefined = -1,
+  gray,  //!< single-channel grayscale
+  rgb,   //!< 3-channel RGB
+  bgr,   //!< 3-channel BGR
+  rgba,  //!< 3-channel RGBA
+  bgra   //!< 3-channel BGRA
+};
+
+
+} // namespace ze
+
diff --git a/RWR/src/ze_oss/imp_core/include/imp/core/rect.hpp b/RWR/src/ze_oss/imp_core/include/imp/core/rect.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..6457f9f2ad77eecfde9b8450b12f00c2445d0815
--- /dev/null
+++ b/RWR/src/ze_oss/imp_core/include/imp/core/rect.hpp
@@ -0,0 +1,32 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#pragma once
+
+namespace imp {
+
+
+
+}
+
diff --git a/RWR/src/ze_oss/imp_core/include/imp/core/roi.hpp b/RWR/src/ze_oss/imp_core/include/imp/core/roi.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..7f1a9fac3cfaee5928b172df02d6a0dd49eced8d
--- /dev/null
+++ b/RWR/src/ze_oss/imp_core/include/imp/core/roi.hpp
@@ -0,0 +1,272 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#pragma once
+
+#include <cstdint>
+#include <array>
+#include <algorithm>
+
+#include <imp/core/size.hpp>
+
+namespace ze {
+
+//------------------------------------------------------------------------------
+/**
+ * @brief The class RoiBase defines the templated base class of an \a DIM-dimensional
+ *        region-of-interest utilizing the CRTP pattern.
+ */
+template<typename T, std::uint8_t DIM, typename Derived>
+struct RoiBase
+{
+  std::array<T, DIM> pt; //!< internal data storage for all dimensions' 'left-upper' corner
+  ze::Size<T, DIM> sz; //!< size of the given roi
+
+  RoiBase()
+  {
+    std::fill(pt.begin(), pt.end(), 0);
+  }
+
+  /**
+   * @brief RoiBase Constructor given the lef-upper corner and the ROI's size
+   * @param lu array of the left-upper corner of the format {a1, a2, a3, ...., aN}
+   * @param sz Size of the \a DIM-dimensional ROI
+   */
+  RoiBase(const std::array<T,DIM>& lu, const ze::Size<T,DIM>& sz)
+    : pt(lu)
+    , sz(sz)
+  {
+  }
+
+  /**
+   * @brief RoiBase initialized max size only. The top-left corner will be ZEROS
+   * @param sz The ROI's size in all dimensions.
+   */
+  RoiBase(const ze::Size<T,DIM>& sz)
+    : sz(sz)
+  {
+    std::fill(pt.begin(), pt.end(), 0);
+  }
+
+  virtual ~RoiBase() = default;
+
+  RoiBase(const RoiBase& from)
+    : pt(from.pt)
+    , sz(from.sz)
+  {
+  }
+
+  RoiBase& operator= (const RoiBase& from)
+  {
+    this->sz = from.sz;
+    this->pt = from.pt;
+    return *this;
+  }
+
+  /**
+   * @brief dim Returns the dimension of the Roi object.
+   * @return Dimension.
+   */
+  std::uint8_t dim() const {return DIM;}
+
+  /**
+   * @brief data gives access to the underlying (raw) data storage
+   * @return Pointer address to the buffer of the underlying data storage.
+   */
+  T* luRaw() {return pt.data();}
+
+  /**
+   * @brief data gives access to the underlying (raw) const data storage
+   * @return Pointer address to the buffer of the underlying data storage.
+   */
+  const T* luRaw() const {return reinterpret_cast<const T*>(pt.data());}
+
+  /**
+   * @brief array with the internal data storing the values for the left-upper corner
+   */
+  std::array<T, DIM>& lu() {return pt;}
+  /**
+   * @brief const array with the internal data storing the values for the left-upper corner
+   */
+  const std::array<T, DIM>& lu() const {return reinterpret_cast<const std::array<T, DIM>&>(pt);}
+
+  /**
+   * @brief size of the ROI
+   */
+  ze::Size<T, DIM>& size() {return sz;}
+  /**
+   * @brief size of the ROI (const)
+   */
+  const ze::Size<T, DIM>& size() const {return reinterpret_cast<const ze::Size<T, DIM>&>(sz);}
+
+};
+
+template<typename T, std::uint8_t DIM, typename Derived>
+inline std::ostream& operator<<(std::ostream &os, const RoiBase<T, DIM, Derived>& rhs)
+{
+  auto it = rhs.lu().begin();
+  os << "(" << *it;
+  ++it;
+  for (; it != rhs.lu().end(); ++it)
+  {
+    os << "," << *it;
+  }
+  os << ")";
+  os << rhs.size();
+  return os;
+}
+
+//------------------------------------------------------------------------------
+// relational operators
+
+template<typename T, std::uint8_t DIM, typename Derived>
+inline bool operator==(const RoiBase<T, DIM, Derived>& lhs,
+                       const RoiBase<T, DIM, Derived>& rhs)
+{
+  return ( (lhs.pt == rhs.pt) && (lhs.sz == rhs.sz) );
+}
+
+template<typename T, std::uint8_t DIM, typename Derived>
+inline bool operator!=(const RoiBase<T, DIM, Derived>& lhs,
+                       const RoiBase<T, DIM, Derived>& rhs)
+{
+  return ( (lhs.pt != rhs.pt) || (lhs.sz != rhs.sz) );
+}
+
+//------------------------------------------------------------------------------
+/**
+ * @brief The class Roi defines region of interest for \a DIM dimensions
+ */
+template<typename T, std::uint8_t DIM>
+struct Roi
+    : public RoiBase<T, DIM, Roi<T, DIM> >
+{
+  using Base = RoiBase<T, DIM, Roi<T, DIM> >;
+  using Base::Base;
+
+  Roi() = default;
+  virtual ~Roi() = default;
+};
+
+//------------------------------------------------------------------------------
+/**
+ * @brief The Roi<T, 1> is a special Roi for e.g. an 1D array defining its start element and length
+ */
+template<typename T>
+struct Roi<T, 1>
+    : public RoiBase<T, 1, Roi<T, 1> >
+{
+  using Base = RoiBase<T, 1, Roi<T, 1> >;
+  using Base::Base;
+
+  Roi() = default;
+  virtual ~Roi() = default;
+
+  Roi(const T& x, const T& length)
+    : Base({x}, {length})
+  {
+  }
+
+  /**
+   * @brief x returns the ROI's x coordinate of the left-upper corner
+   */
+  T& x() noexcept {return this->pt[0];}
+  constexpr const T& x() const noexcept {return this->pt[0];}
+
+  /**
+   * @brief width returns the width of the 2d Roi
+   */
+  T& length() noexcept {return this->sz[0];}
+  constexpr const T& length() const noexcept {return this->sz[0];}
+
+};
+
+
+//------------------------------------------------------------------------------
+/**
+ * @brief The Roi<T, 2> is a special Roi for a 2D shape defining its width and height
+ */
+template<typename T>
+struct Roi<T, 2>
+    : public RoiBase<T, 2, Roi<T, 2> >
+{
+  using Base = RoiBase<T, 2, Roi<T, 2> >;
+  using Base::Base;
+
+  Roi() = default;
+  virtual ~Roi() = default;
+
+  Roi(const T& x, const T& y, const T& width, const T& height)
+    : Base({x,y}, {width, height})
+  {
+  }
+
+  /**
+   * @brief x returns the ROI's x coordinate of the left-upper corner
+   */
+  T& x() noexcept {return this->pt[0];}
+  constexpr const T& x() const noexcept {return this->pt[0];}
+
+  /**
+   * @brief y returns the ROI's y coordinate of the left-upper corner
+   */
+  T& y() noexcept {return this->pt[1];}
+  constexpr const T& y() const noexcept {return this->pt[1];}
+
+  /**
+   * @brief width returns the width of the 2d Roi
+   */
+  T& width() noexcept {return this->sz[0];}
+  constexpr const T& width() const noexcept {return this->sz[0];}
+
+  /**
+   * @brief height returns the length of the second dimension of the 2d Roi
+   * @return
+   */
+  T& height() noexcept {return this->sz[1];}
+  constexpr const T& height() const noexcept {return this->sz[1];}
+};
+
+//------------------------------------------------------------------------------
+// some convencience typedefs
+
+// 1D
+using Roi1u = Roi<uint32_t, 1>;
+using Roi1i = Roi<std::int32_t, 1>;
+using Roi1f = Roi<float, 1>;
+using Roi1d = Roi<float, 1>;
+// 2D
+using Roi2u = Roi<uint32_t, 2>;
+using Roi2i = Roi<std::int32_t, 2>;
+using Roi2f = Roi<float, 2>;
+using Roi2d = Roi<float, 2>;
+//3D
+using Roi3u = Roi<uint32_t, 3>;
+using Roi3i = Roi<std::int32_t, 3>;
+using Roi3f = Roi<float, 3>;
+using Roi3d = Roi<float, 3>;
+
+
+} // namespace ze
+
diff --git a/RWR/src/ze_oss/imp_core/include/imp/core/size.hpp b/RWR/src/ze_oss/imp_core/include/imp/core/size.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..d33c1ec9e734d01bc2aff6a9e59e984f2c69a069
--- /dev/null
+++ b/RWR/src/ze_oss/imp_core/include/imp/core/size.hpp
@@ -0,0 +1,346 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#pragma once
+
+#include <cstdint>
+#include <array>
+#include <algorithm>
+#include <iostream>
+
+namespace ze {
+
+//------------------------------------------------------------------------------
+/**
+ * @brief The class SizeBase defines the templated base class of any size utilizing the CRTP pattern.
+ */
+template<typename T, std::uint8_t DIM, typename Derived>
+struct SizeBase
+{
+  using size_t = std::size_t;
+
+  std::array<T, DIM> sz; //!< internal data storage for all dimensions' sizes
+
+  SizeBase()
+  {
+    std::fill(sz.begin(), sz.end(), 0);
+  }
+
+  SizeBase(const std::array<T,DIM>& arr)
+    : sz(arr)
+  {
+  }
+
+  virtual ~SizeBase() = default;
+
+  SizeBase(const SizeBase& from)
+    : sz(from.sz)
+  {
+  }
+
+  SizeBase& operator= (const SizeBase& from)
+  {
+    this->sz = from.sz;
+    return *this;
+  }
+
+//  SizeBase& operator* (const double factor) const
+//  {
+//    std::array<T,Dim> arr;
+//    for(std::size_t i=0; i<sz.size(); ++i)
+//    {
+//      arr[i] = static_cast<T>(sz[i] * factor);
+//    }
+//  }
+
+//  IuSize operator/ (const double factor) const
+//  {
+//    IU_ASSERT(factor != 0);
+//    double invFactor = 1 / factor;
+//    return IuSize(this->width, this->height, this->depth) * invFactor;
+//  }
+
+
+  /**
+   * @brief operator [] returns the reference to the element storing the size
+   *        of the \a n-the dimension
+   * @param n dimension index
+   * @return Reference to size element of the n-th dimension
+   */
+  T& operator[] (std::uint8_t n) noexcept {return sz[n];}
+
+  /**
+   * @brief operator [] returns the reference to the element storing the size
+   *        of the \a n-the dimension
+   * @param n dimension index
+   * @return Reference to size element of the n-th dimension
+   */
+  constexpr const T& operator[] (std::uint8_t n) const noexcept { return sz[n]; }
+
+  /**
+   * @brief dim Returns the dimension of the size object.
+   * @return Dimension.
+   */
+  constexpr std::uint8_t dim() const noexcept {return DIM;}
+
+  /**
+   * @brief prod Computes the product of all elements
+   * @return Product of all elements.
+   */
+  T prod() const noexcept
+  {
+    T a=1;
+    for (auto elem : sz)
+    {
+      a *= elem;
+    }
+    return a;
+  }
+
+
+  /**
+   * @brief data gives access to the underlying (raw) data storage
+   * @return Pointer address to the buffer of the underlying data storage.
+   */
+  T* data() {return sz.data();}
+
+  /**
+   * @brief data gives access to the underlying (raw) const data storage
+   * @return Pointer address to the buffer of the underlying data storage.
+   */
+  const T* data() const {return reinterpret_cast<const T*>(sz.data());}
+
+  /**
+   * @brief array returns a reference to the internal array used for storing the data
+   */
+  std::array<T, DIM>& array() {return sz;}
+  /**
+   * @brief array returns a const reference to the internal array used for storing the data
+   */
+  const std::array<T, DIM>& array() const {return reinterpret_cast<const std::array<T, DIM>&>(sz);}
+};
+
+//------------------------------------------------------------------------------
+// relational operators
+
+template<typename T, std::uint8_t DIM, typename Derived>
+inline bool operator==(const SizeBase<T, DIM, Derived>& lhs,
+                       const SizeBase<T, DIM, Derived>& rhs)
+{
+  return (lhs.array() == rhs.array());
+}
+
+template<typename T, std::uint8_t DIM, typename Derived>
+inline bool operator!=(const SizeBase<T, DIM, Derived>& lhs,
+                       const SizeBase<T, DIM, Derived>& rhs)
+{
+  return (lhs.array() != rhs.array());
+}
+
+//! @todo (MWE) the following rational comparisons cannot be used directly
+//!             as they use lexicographical_compare operators
+
+//template<typename T, std::uint8_t DIM, typename Derived>
+//inline bool operator>(const SizeBase<T, DIM, Derived>& lhs,
+//                      const SizeBase<T, DIM, Derived>& rhs)
+//{
+//  return (lhs.array() > rhs.array());
+//}
+
+//template<typename T, std::uint8_t DIM, typename Derived>
+//inline bool operator>=(const SizeBase<T, DIM, Derived>& lhs,
+//                      const SizeBase<T, DIM, Derived>& rhs)
+//{
+//  return (lhs.array() >= rhs.array());
+//}
+
+//template<typename T, std::uint8_t DIM, typename Derived>
+//inline bool operator<(const SizeBase<T, DIM, Derived>& lhs,
+//                      const SizeBase<T, DIM, Derived>& rhs)
+//{
+//  return (lhs.array() < rhs.array());
+//}
+
+//template<typename T, std::uint8_t DIM, typename Derived>
+//inline bool operator<=(const SizeBase<T, DIM, Derived>& lhs,
+//                      const SizeBase<T, DIM, Derived>& rhs)
+//{
+//  return (lhs.array() <= rhs.array());
+//}
+
+
+template<typename T, std::uint8_t DIM, typename Derived>
+inline std::ostream& operator<<(std::ostream &os, const SizeBase<T, DIM, Derived>& rhs)
+{
+  auto it = rhs.array().begin();
+  os << "(" << *it;
+  ++it;
+  for (; it != rhs.array().end(); ++it)
+  {
+    os << "," << *it;
+  }
+  os << ")";
+  return os;
+}
+
+
+//------------------------------------------------------------------------------
+/**
+ * @brief The class Size defines a generic size implementation for \a DIM dimensions
+ */
+template<typename T, std::uint8_t DIM>
+struct Size
+    : public SizeBase<T, DIM, Size<T, DIM> >
+{
+  using Base = SizeBase<T, DIM, Size<T, DIM> >;
+  using Base::Base;
+  Size() = default;
+  virtual ~Size() = default;
+};
+
+//------------------------------------------------------------------------------
+/**
+ * @brief The Size<T, 1> is a special size for an 1D array shape defining its length
+ */
+template<typename T>
+struct Size<T, 1>
+    : public SizeBase<T, 1, Size<T, 1> >
+{
+  using Base = SizeBase<T, 1, Size<T, 1> >;
+  using Base::Base;
+  Size() = default;
+  virtual ~Size() = default;
+
+  Size(const T& length)
+    : Base({length})
+  {
+  }
+
+  /** @brief Operator that returns the length of the 1d array. */
+  constexpr operator T() const {return this->sz[0];}
+
+  /**
+   * @brief length returns the length of the 1D array
+   */
+  constexpr T length() const {return this->sz[0];}
+};
+
+//------------------------------------------------------------------------------
+/**
+ * @brief The Size<T, 2> is a special size for a 2D shape defining its width and height
+ */
+template<typename T>
+struct Size<T, 2>
+    : public SizeBase<T, 2, Size<T, 2> >
+{
+  using Base = SizeBase<T, 2, Size<T, 2> >;
+  using Base::Base;
+  Size() = default;
+  virtual ~Size() = default;
+
+  Size(const T& width, const T& height)
+    : Base({width, height})
+  {
+  }
+
+  /**
+   * @brief width returns the width of the 2d size
+   */
+  T width() const {return this->sz[0];}
+
+  /**
+   * @brief height returns the length of the second dimension of the 2d size
+   * @return
+   */
+  T height() const {return this->sz[1];}
+
+  /**
+   * @brief area Computes the area (width*height)
+   * @return width*height
+   */
+  T area() const noexcept {return (this->sz[0]*this->sz[1]);}
+};
+
+//------------------------------------------------------------------------------
+/**
+ * @brief The Size<T, 3> is a special size for a 3D shape defining its width, height and depth
+ */
+template<typename T>
+struct Size<T, 3>
+    : public SizeBase<T, 3, Size<T, 3> >
+{
+  using Base = SizeBase<T, 3, Size<T, 3> >;
+  using Base::Base;
+  Size() = default;
+  virtual ~Size() = default;
+
+  Size(const T& width, const T& height, const T& depth)
+    : Base({width, height, depth})
+  {
+  }
+
+  /**
+   * @brief width returns the width of the 3d size
+   */
+  T width() const {return this->sz[0];}
+
+  /**
+   * @brief height returns the length of the second dimension of the 3d size
+   */
+  T height() const {return this->sz[1];}
+
+  /**
+   * @brief depth returns the length of the third dimension of the 3d size
+   */
+  T depth() const {return this->sz[2];}
+
+  /**
+   * @brief volume Computes the volume (width*height*depth)
+   * @return width*height*depth
+   */
+  T volume() const noexcept {return (this->sz[0]*this->sz[1]*this->sz[2]);}
+
+};
+
+//------------------------------------------------------------------------------
+// some convencience typedefs
+
+// 1D
+typedef Size<uint32_t, 1> Size1u;
+typedef Size<std::int32_t, 1> Size1i;
+typedef Size<float, 1> Size1f;
+typedef Size<double, 1> Size1d;
+// 2D
+typedef Size<uint32_t, 2> Size2u;
+typedef Size<std::int32_t, 2> Size2i;
+typedef Size<float, 2> Size2f;
+typedef Size<double, 2> Size2d;
+//3D
+typedef Size<uint32_t, 3> Size3u;
+typedef Size<std::int32_t, 3> Size3i;
+typedef Size<float, 3> Size3f;
+typedef Size<double, 3> Size3d;
+
+} // namespace ze
+
diff --git a/RWR/src/ze_oss/imp_core/include/imp/core/types.hpp b/RWR/src/ze_oss/imp_core/include/imp/core/types.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..a206743d70da4cc40f1642ac9a8f4942fbf598a1
--- /dev/null
+++ b/RWR/src/ze_oss/imp_core/include/imp/core/types.hpp
@@ -0,0 +1,53 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#pragma once
+
+#include <cstdint>
+
+namespace ze {
+
+enum class InterpolationMode
+{
+  Point,
+  Linear,
+  Cubic,
+  CubicSpline
+};
+
+enum class MemoryType
+{
+  Undefined,
+  Cpu,
+  CpuAligned,
+  Gpu,
+  GpuAligned,
+  Unified,
+  UnifiedAligned,
+  Managed,
+  ManagedAligned
+};
+
+} // namespace ze
+
diff --git a/RWR/src/ze_oss/imp_core/package.xml b/RWR/src/ze_oss/imp_core/package.xml
new file mode 100644
index 0000000000000000000000000000000000000000..cd0a4325e2665a88181c42639461a08c9cba1d32
--- /dev/null
+++ b/RWR/src/ze_oss/imp_core/package.xml
@@ -0,0 +1,19 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>imp_core</name>
+  <description>
+    IMP core module
+  </description>
+  <version>0.1.4</version>
+  <license>ZE</license>
+
+  <maintainer email="code@werlberger.org">Manuel Werlberger</maintainer>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <buildtool_depend>catkin_simple</buildtool_depend>
+
+  <depend>ze_cmake</depend>
+  <depend>ze_common</depend>
+
+  <test_depend>gtest</test_depend>
+</package>
diff --git a/RWR/src/ze_oss/imp_core/src/image_raw.cpp b/RWR/src/ze_oss/imp_core/src/image_raw.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..3d72753a9e505080c268189424423036354cf566
--- /dev/null
+++ b/RWR/src/ze_oss/imp_core/src/image_raw.cpp
@@ -0,0 +1,167 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#include <imp/core/image_raw.hpp>
+
+#include <iostream>
+#include <ze/common/logging.hpp>
+
+namespace ze {
+
+//-----------------------------------------------------------------------------
+template<typename Pixel>
+ImageRaw<Pixel>::ImageRaw(const ze::Size2u& size, PixelOrder pixel_order)
+  : Base(size, pixel_order)
+{
+  data_.reset(Memory::alignedAlloc(size, &this->header_.pitch));
+  this->header_.memory_type = MemoryType::CpuAligned;
+}
+
+//-----------------------------------------------------------------------------
+template<typename Pixel>
+ImageRaw<Pixel>::ImageRaw(const ImageRaw& from)
+  : Base(from)
+{
+  data_.reset(Memory::alignedAlloc(this->size(), &this->header_.pitch));
+  this->header_.memory_type = MemoryType::CpuAligned;
+  from.copyTo(*this);
+}
+
+//-----------------------------------------------------------------------------
+template<typename Pixel>
+ImageRaw<Pixel>::ImageRaw(const Image<Pixel>& from)
+  : Base(from)
+{
+  data_.reset(Memory::alignedAlloc(this->size(), &this->header_.pitch));
+  this->header_.memory_type = MemoryType::CpuAligned;
+  from.copyTo(*this);
+}
+
+//-----------------------------------------------------------------------------
+template<typename Pixel>
+ImageRaw<Pixel>
+::ImageRaw(Pixel* data, uint32_t width, uint32_t height,
+           uint32_t pitch, bool use_ext_data_pointer, PixelOrder pixel_order)
+  : Base(width, height, pixel_order)
+{
+  CHECK(data);
+
+  if(use_ext_data_pointer)
+  {
+    // This uses the external data pointer as internal data pointer.
+    auto dealloc_nop = [](Pixel*) { ; };
+    data_ = std::unique_ptr<Pixel, Deallocator>(data, Deallocator(dealloc_nop));
+    this->header_.pitch = pitch;
+    this->header_.memory_type = (Memory::isAligned(data)) ?
+                                   MemoryType::CpuAligned : MemoryType::Cpu;
+  }
+  else
+  {
+    data_.reset(Memory::alignedAlloc(this->size(), &this->header_.pitch));
+    this->header_.memory_type = MemoryType::CpuAligned;
+
+    if (this->bytes() == pitch*height)
+    {
+      std::copy(data, data+this->stride()*height, data_.get());
+    }
+    else
+    {
+      for (uint32_t y=0; y<height; ++y)
+      {
+        for (uint32_t x=0; x<width; ++x)
+        {
+          data_.get()[y*this->stride()+x] = data[y*this->stride() + x];
+        }
+      }
+    }
+  }
+}
+
+//-----------------------------------------------------------------------------
+template<typename Pixel>
+ImageRaw<Pixel>::ImageRaw(Pixel* data,
+                          uint32_t width, uint32_t height,
+                          uint32_t pitch,
+                          const std::shared_ptr<void const>& tracked,
+                          PixelOrder pixel_order)
+  : Base(width, height, pixel_order)
+{
+  CHECK(data);
+  CHECK(tracked);
+
+  auto dealloc_nop = [](Pixel*) { ; };
+  data_ = std::unique_ptr<Pixel, Deallocator>(data, Deallocator(dealloc_nop));
+  this->header_.pitch = pitch;
+  tracked_ = tracked;
+  this->header_.memory_type = (Memory::isAligned(data)) ?
+                                 MemoryType::CpuAligned : MemoryType::Cpu;
+}
+
+//-----------------------------------------------------------------------------
+template<typename Pixel>
+Pixel* ImageRaw<Pixel>::data(uint32_t ox, uint32_t oy)
+{
+  CHECK_LT(ox, this->width());
+  CHECK_LT(oy, this->height());
+  return &data_.get()[oy*this->stride() + ox];
+}
+
+//-----------------------------------------------------------------------------
+template<typename Pixel>
+const Pixel* ImageRaw<Pixel>::data(uint32_t ox, uint32_t oy) const
+{
+  CHECK_LT(ox, this->width());
+  CHECK_LT(oy, this->height());
+  return reinterpret_cast<const Pixel*>(&data_.get()[oy*this->stride() + ox]);
+}
+
+//=============================================================================
+// Explicitely instantiate the desired classes
+// (sync with typedefs at the end of the hpp file)
+template class ImageRaw<ze::Pixel8uC1>;
+template class ImageRaw<ze::Pixel8uC2>;
+template class ImageRaw<ze::Pixel8uC3>;
+template class ImageRaw<ze::Pixel8uC4>;
+
+template class ImageRaw<ze::Pixel16sC1>;
+template class ImageRaw<ze::Pixel16sC2>;
+template class ImageRaw<ze::Pixel16sC3>;
+template class ImageRaw<ze::Pixel16sC4>;
+
+template class ImageRaw<ze::Pixel16uC1>;
+template class ImageRaw<ze::Pixel16uC2>;
+template class ImageRaw<ze::Pixel16uC3>;
+template class ImageRaw<ze::Pixel16uC4>;
+
+template class ImageRaw<ze::Pixel32sC1>;
+template class ImageRaw<ze::Pixel32sC2>;
+template class ImageRaw<ze::Pixel32sC3>;
+template class ImageRaw<ze::Pixel32sC4>;
+
+template class ImageRaw<ze::Pixel32fC1>;
+template class ImageRaw<ze::Pixel32fC2>;
+template class ImageRaw<ze::Pixel32fC3>;
+template class ImageRaw<ze::Pixel32fC4>;
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/imp_core/src/linearmemory.cpp b/RWR/src/ze_oss/imp_core/src/linearmemory.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b3c214e16cc1d12ae7eb1b7169c8970c281b65e2
--- /dev/null
+++ b/RWR/src/ze_oss/imp_core/src/linearmemory.cpp
@@ -0,0 +1,145 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#include <imp/core/linearmemory.hpp>
+
+#include <cstring>
+#include <algorithm>
+
+namespace ze {
+
+//-----------------------------------------------------------------------------
+template<typename Pixel>
+LinearMemory<Pixel>::LinearMemory(const uint32_t& length)
+  : LinearMemoryBase(length)
+  , data_(Memory::alignedAlloc(this->length()))
+{
+}
+
+//-----------------------------------------------------------------------------
+template<typename Pixel>
+LinearMemory<Pixel>::LinearMemory(const LinearMemory<Pixel>& from)
+  : LinearMemoryBase(from)
+{
+  CHECK(from.data_);
+  data_.reset(Memory::alignedAlloc(this->length()));
+  std::copy(from.data_.get(), from.data_.get()+from.length(), data_.get());
+}
+
+//-----------------------------------------------------------------------------
+template<typename Pixel>
+LinearMemory<Pixel>::LinearMemory(Pixel* host_data,
+                                  const uint32_t& length,
+                                  bool use_ext_data_pointer)
+  : LinearMemoryBase(length)
+{
+  CHECK(host_data);
+
+  if(use_ext_data_pointer)
+  {
+    // This uses the external data pointer and stores it as a 'reference':
+    // memory won't be managed by us!
+    auto dealloc_nop = [](Pixel*) { ; };
+    data_ = std::unique_ptr<Pixel, Deallocator>(
+          host_data, Deallocator(dealloc_nop));
+  }
+  else
+  {
+    // allocates an internal data pointer and copies the external data it.
+    data_.reset(Memory::alignedAlloc(this->length()));
+    std::copy(host_data, host_data+length, data_.get());
+  }
+}
+
+//-----------------------------------------------------------------------------
+template<typename Pixel>
+Pixel* LinearMemory<Pixel>::data(uint32_t offset)
+{
+  CHECK_LE(offset, this->length());
+  return &(data_.get()[offset]);
+}
+
+//-----------------------------------------------------------------------------
+template<typename Pixel>
+const Pixel* LinearMemory<Pixel>::data(uint32_t offset) const
+{
+  CHECK_LE(offset, this->length());
+  return reinterpret_cast<const Pixel*>(&(data_.get()[offset]));
+}
+
+//-----------------------------------------------------------------------------
+template<typename Pixel>
+void LinearMemory<Pixel>::setValue(const Pixel& value)
+{
+  std::fill(data_.get()+roi_.x(),
+            data_.get()+this->roi().x()+this->roi().length(),
+            value);
+}
+
+//-----------------------------------------------------------------------------
+template<typename Pixel>
+void LinearMemory<Pixel>::copyTo(LinearMemory<Pixel>& dst)
+{
+  CHECK_EQ(this->length(), dst.length());
+  std::copy(data_.get(), data_.get()+this->length(), dst.data_.get());
+}
+
+
+//-----------------------------------------------------------------------------
+template<typename Pixel>
+LinearMemory<Pixel>& LinearMemory<Pixel>::operator=(Pixel rhs)
+{
+  this->setValue(rhs);
+  return *this;
+}
+
+
+//=============================================================================
+// Explicitely instantiate the desired classes
+template class LinearMemory<ze::Pixel8uC1>;
+template class LinearMemory<ze::Pixel8uC2>;
+template class LinearMemory<ze::Pixel8uC3>;
+template class LinearMemory<ze::Pixel8uC4>;
+
+template class LinearMemory<ze::Pixel16uC1>;
+template class LinearMemory<ze::Pixel16uC2>;
+template class LinearMemory<ze::Pixel16uC3>;
+template class LinearMemory<ze::Pixel16uC4>;
+
+template class LinearMemory<ze::Pixel32uC1>;
+template class LinearMemory<ze::Pixel32uC2>;
+template class LinearMemory<ze::Pixel32uC3>;
+template class LinearMemory<ze::Pixel32uC4>;
+
+template class LinearMemory<ze::Pixel32sC1>;
+template class LinearMemory<ze::Pixel32sC2>;
+template class LinearMemory<ze::Pixel32sC3>;
+template class LinearMemory<ze::Pixel32sC4>;
+
+template class LinearMemory<ze::Pixel32fC1>;
+template class LinearMemory<ze::Pixel32fC2>;
+template class LinearMemory<ze::Pixel32fC3>;
+template class LinearMemory<ze::Pixel32fC4>;
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/imp_core/test/linearmemory_test.cpp b/RWR/src/ze_oss/imp_core/test/linearmemory_test.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..90925d09aa2a98567f73330d7bfa79187dcdbb27
--- /dev/null
+++ b/RWR/src/ze_oss/imp_core/test/linearmemory_test.cpp
@@ -0,0 +1,179 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#include <gtest/gtest.h>
+
+// system includes
+#include <assert.h>
+#include <cstdint>
+#include <iostream>
+#include <functional>
+#include <limits>
+#include <type_traits>
+
+#include <imp/core/linearmemory.hpp>
+#include <ze/common/random.hpp>
+#include <ze/common/test_utils.hpp>
+
+
+template <typename Pixel>
+class LinearMemoryTest : public ::testing::Test
+{
+ protected:
+  LinearMemoryTest() :
+    linmem_(numel_)
+  {
+    using T = typename Pixel::T;
+    auto random_val_generator = ze::uniformDistribution<T>(ZE_DETERMINISTIC);
+
+    T val1 = random_val_generator();
+    T val2;
+    do
+    {
+      val2 = random_val_generator();
+    } while(std::fabs(val1-val2) < std::numeric_limits<T>::epsilon());
+
+    pixel1_ = Pixel(val1);
+    pixel2_ = Pixel(val2);
+  }
+
+  void setValue()
+  {
+    linmem_.setValue(pixel1_);
+  }
+
+  void setRoi()
+  {
+    linmem_.setRoi(roi_);
+  }
+
+  void setValueRoi()
+  {
+    this->setRoi();
+    linmem_.setValue(pixel2_);
+  }
+
+  uint8_t pixel_size_ = sizeof(Pixel);
+  size_t pixel_bit_depth_ = 8*sizeof(Pixel);
+
+  size_t numel_ = 123;
+  ze::Roi1u roi_ = ze::Roi1u(numel_/3, numel_/3);
+  ze::LinearMemory<Pixel> linmem_;
+
+  Pixel pixel1_;
+  Pixel pixel2_;
+};
+
+// The list of types we want to test.
+typedef testing::Types<
+ze::Pixel8uC1, ze::Pixel8uC2, ze::Pixel8uC3, ze::Pixel8uC4,
+ze::Pixel16uC1, ze::Pixel16uC2, ze::Pixel16uC3, ze::Pixel16uC4,
+ze::Pixel32sC1, ze::Pixel32sC2, ze::Pixel32sC3, ze::Pixel32sC4,
+ze::Pixel32uC1, ze::Pixel32uC2, ze::Pixel32uC3, ze::Pixel32uC4,
+ze::Pixel32fC1, ze::Pixel32fC2, ze::Pixel32fC3, ze::Pixel32fC4> PixelTypes;
+
+TYPED_TEST_CASE(LinearMemoryTest, PixelTypes);
+
+TYPED_TEST(LinearMemoryTest, CheckMemforyAlignment)
+{
+  EXPECT_EQ(0u, (std::uintptr_t)reinterpret_cast<void*>(this->linmem_.data()) % 32);
+}
+
+TYPED_TEST(LinearMemoryTest, CheckLength)
+{
+  EXPECT_EQ(this->numel_, this->linmem_.length());
+}
+
+TYPED_TEST(LinearMemoryTest, CheckNoRoi)
+{
+  EXPECT_EQ(0u, this->linmem_.roi().x());
+  EXPECT_EQ(this->numel_, this->linmem_.roi().length());
+}
+
+TYPED_TEST(LinearMemoryTest, CheckRoi)
+{
+  this->setRoi();
+  EXPECT_EQ(this->roi_.x(), this->linmem_.roi().x());
+  EXPECT_EQ(this->roi_.length(), this->linmem_.roi().length());
+}
+
+TYPED_TEST(LinearMemoryTest, CheckNumBytes)
+{
+  EXPECT_EQ(this->numel_*this->pixel_size_, this->linmem_.bytes());
+}
+
+TYPED_TEST(LinearMemoryTest, CheckNumRoiBytes)
+{
+  this->setRoi();
+  EXPECT_EQ(this->roi_.length()*this->pixel_size_, this->linmem_.roiBytes());
+}
+
+TYPED_TEST(LinearMemoryTest, CheckPixelBitDepth)
+{
+  EXPECT_EQ(this->pixel_bit_depth_, this->linmem_.bitDepth());
+}
+
+TYPED_TEST(LinearMemoryTest, ReturnsFalseForNonGpuMemory)
+{
+  ASSERT_FALSE(this->linmem_.isGpuMemory());
+}
+
+TYPED_TEST(LinearMemoryTest, CheckValues)
+{
+  this->setValue();
+  for (uint32_t i=0u; i<this->numel_; ++i)
+  {
+    EXPECT_EQ(this->linmem_[i], this->pixel1_);
+  }
+}
+
+TYPED_TEST(LinearMemoryTest, CheckValuesInConstLinearMemory)
+{
+  this->setValue();
+  const ze::LinearMemory<TypeParam> const_linmem(this->linmem_);
+  for (uint32_t i=0u; i<this->numel_; ++i)
+  {
+    EXPECT_EQ(const_linmem[i], this->pixel1_);
+  }
+}
+
+
+TYPED_TEST(LinearMemoryTest, CheckRoiValues)
+{
+  this->setValue();
+  this->setValueRoi();
+
+  for (uint32_t i=0u; i<this->numel_; ++i)
+  {
+    if (i>=this->roi_.x() && i<(this->roi_.x()+this->roi_.length()))
+    {
+      EXPECT_EQ(this->pixel2_, this->linmem_[i]);
+    }
+    else
+    {
+      EXPECT_EQ(this->pixel1_, this->linmem_[i]);
+    }
+  }
+}
+
diff --git a/RWR/src/ze_oss/imp_core/test/roi_test.cpp b/RWR/src/ze_oss/imp_core/test/roi_test.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..06b8988cf6e321c97f2de0bffc5c6aab83fec645
--- /dev/null
+++ b/RWR/src/ze_oss/imp_core/test/roi_test.cpp
@@ -0,0 +1,79 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#include <gtest/gtest.h>
+
+// system includes
+#include <assert.h>
+#include <cstdint>
+#include <iostream>
+
+#include <imp/core/roi.hpp>
+
+
+TEST(IMPCoreTestSuite,roiTest)
+{
+  //
+  // 1D case uint
+  //
+  {
+    uint32_t x=1, len=10;
+    ze::Roi1u roi(x,len);
+    ASSERT_TRUE(x == roi.x());
+    ASSERT_TRUE(len == roi.length());
+
+    ASSERT_TRUE(x == roi.lu()[0]);
+    ASSERT_TRUE(len == roi.size()[0]);
+  }
+
+  //
+  // 2D case
+  //
+  {
+    std::int32_t x=1, y=2, w=10, h=13;
+    ze::Roi2i roi(x,y,w,h);
+    ASSERT_TRUE(x == roi.x());
+    ASSERT_TRUE(y == roi.y());
+    ASSERT_TRUE(w == roi.width());
+    ASSERT_TRUE(h == roi.height());
+
+    ASSERT_TRUE(x == roi.lu()[0]);
+    ASSERT_TRUE(y == roi.lu()[1]);
+    ASSERT_TRUE(w == roi.size()[0]);
+    ASSERT_TRUE(h == roi.size()[1]);
+  }
+
+  // operator==, etc.
+  {
+    uint32_t x=1, y=2, w=10, h=13;
+    ze::Roi2u roi1(x,y,w,h);
+    ze::Roi2u roi2(x,y,w,h);
+    ze::Roi2u roi3(x+1,y,w,h);
+
+    ASSERT_TRUE(roi1 == roi2);
+    ASSERT_FALSE(roi1 != roi2);
+    ASSERT_FALSE(roi1 == roi3);
+    ASSERT_TRUE(roi1 != roi3);
+  }
+}
diff --git a/RWR/src/ze_oss/imp_core/test/single_shot_timer_test.cpp b/RWR/src/ze_oss/imp_core/test/single_shot_timer_test.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..817a24069e493fec3324556dd33ca615573db68a
--- /dev/null
+++ b/RWR/src/ze_oss/imp_core/test/single_shot_timer_test.cpp
@@ -0,0 +1,59 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#include <gtest/gtest.h>
+
+// system includes
+#include <assert.h>
+#include <cstdint>
+#include <iostream>
+
+#include <imp/core/timer.hpp>
+
+
+TEST(IMPCoreTestSuite, singleShotTimerTest)
+{
+  ze::SingleShotTimer::TimePoint start_outer = ze::SingleShotTimer::Clock::now();
+  ze::SingleShotTimer timer("unit test timer");
+  ze::SingleShotTimer::TimePoint start_inner = ze::SingleShotTimer::Clock::now();
+
+  for (int i=0; i<10000; ++i)
+  {
+    // idle
+  }
+
+  ze::SingleShotTimer::TimePoint end_inner = ze::SingleShotTimer::Clock::now();
+  ze::SingleShotTimer::Nanoseconds duration = timer.elapsedNs();
+  ze::SingleShotTimer::TimePoint end_outer = ze::SingleShotTimer::Clock::now();
+
+  ze::SingleShotTimer::Nanoseconds duration_inner =
+      std::chrono::duration_cast<ze::SingleShotTimer::Nanoseconds>(
+        end_inner - start_inner);
+  ze::SingleShotTimer::Nanoseconds duration_outer =
+      std::chrono::duration_cast<ze::SingleShotTimer::Nanoseconds>(
+        end_outer - start_outer);
+
+  ASSERT_LE(duration_inner.count(), duration.count());
+  ASSERT_GE(duration_outer.count(), duration.count());
+}
diff --git a/RWR/src/ze_oss/imp_core/test/size_test.cpp b/RWR/src/ze_oss/imp_core/test/size_test.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..28642fbfa5327357bcded159493e94104f2d90fe
--- /dev/null
+++ b/RWR/src/ze_oss/imp_core/test/size_test.cpp
@@ -0,0 +1,75 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#include <gtest/gtest.h>
+
+// system includes
+#include <assert.h>
+#include <cstdint>
+#include <iostream>
+
+#include <imp/core/size.hpp>
+
+TEST(IMPCoreTestSuite,testSize)
+{
+  {
+    ze::Size1u sz1u;
+    EXPECT_EQ(0u, sz1u.length());
+    uint32_t len=101;
+    ze::Size1u sz(len);
+    EXPECT_EQ(len, sz.length());
+    EXPECT_EQ(len, sz.data()[0]);
+    EXPECT_EQ(len, sz);
+  }
+  {
+    ze::Size2u sz2u;
+    EXPECT_EQ(0u, sz2u.width());
+    EXPECT_EQ(0u, sz2u.height());
+
+    // 2D sizes
+    const std::int32_t w=10, h=13;
+    const std::int32_t area = w*h;
+
+    ze::Size2i sz(w,h);
+    EXPECT_EQ(w, sz.width());
+    EXPECT_EQ(h, sz.height());
+
+    EXPECT_EQ(w, sz.data()[0]);
+    EXPECT_EQ(h, sz.data()[1]);
+
+    EXPECT_EQ(area, sz.area());
+    EXPECT_EQ(area, sz.prod());
+
+    // comparison operator tests
+    ze::Size2i a(123,456);
+    ze::Size2i b(123,456);
+    ze::Size2i c(124,456);
+    ze::Size2i d(124,457);
+
+    ASSERT_TRUE((a == b));
+    ASSERT_FALSE((a != b));
+    ASSERT_FALSE((a == c));
+    ASSERT_TRUE((a != c));
+  }
+}
diff --git a/RWR/src/ze_oss/imp_core/test/test_copy.cpp b/RWR/src/ze_oss/imp_core/test/test_copy.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..52152c8b066f95524f615cab1261a62dabdd3e95
--- /dev/null
+++ b/RWR/src/ze_oss/imp_core/test/test_copy.cpp
@@ -0,0 +1,96 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#include <gtest/gtest.h>
+
+#include <imp/core/image.hpp>
+#include <imp/core/image_raw.hpp>
+#include <ze/common/random.hpp>
+
+namespace ze {
+ImageRaw8uC1 generateRandomImage(size_t width, size_t height)
+{
+  auto random_val = uniformDistribution<uint8_t>(ZE_DETERMINISTIC);
+  ImageRaw8uC1 img(width, height);
+  for (size_t y = 0; y < img.height(); ++y)
+  {
+    for (size_t x = 0; x < img.width(); ++x)
+    {
+      uint8_t random_value = random_val();
+      img[y][x] = random_value;
+    }
+  }
+  return img;
+}
+
+} // ze namespace
+
+TEST(IMPCoreTestSuite, testCopy8uC1)
+{
+  using namespace ze;
+  ImageRaw8uC1 img = generateRandomImage(1024, 768);
+  ImageRaw8uC1 img_copy(img.width(), img.height());
+  img_copy.copyFrom(img);
+  for (uint32_t x = 0; x < img_copy.width(); ++x)
+  {
+    for (uint32_t y = 0; y < img_copy.height(); ++y)
+    {
+      EXPECT_EQ(img(x, y), img_copy(x, y));
+    }
+  }
+}
+
+TEST(IMPCoreTestSuite, testCopy8uC1DifferentBytes)
+{
+  using namespace ze;
+
+  constexpr uint32_t width{980};
+  constexpr uint32_t padded_width{1024};
+  constexpr uint32_t height{768};
+  constexpr uint32_t padded_numel{padded_width * height};
+
+  //! Allocate pitched memory and use external data pointer
+  auto random_val = uniformDistribution<uint8_t>(ZE_DETERMINISTIC);
+  Pixel8uC1 data_array[padded_numel];
+  for (uint32_t i = 0; i < padded_numel; ++i)
+  {
+    data_array[i] = random_val();
+  }
+  ImageRaw8uC1 img(
+        data_array,
+        width, height,
+        padded_width * sizeof(Pixel8uC1),
+        true);
+
+  //! Perform copy and test result
+  ImageRaw8uC1 img_copy(img.width(), img.height());
+  img_copy.copyFrom(img);
+  for (uint32_t x = 0; x < img_copy.width(); ++x)
+  {
+    for (uint32_t y = 0; y < img_copy.height(); ++y)
+    {
+      EXPECT_EQ(img(x, y), img_copy(x, y));
+    }
+  }
+}
diff --git a/RWR/src/ze_oss/imp_core/test/test_image.cpp b/RWR/src/ze_oss/imp_core/test/test_image.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..671f87cfb04a051c30069ec4b168d57f09e22cd4
--- /dev/null
+++ b/RWR/src/ze_oss/imp_core/test/test_image.cpp
@@ -0,0 +1,233 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#include <gtest/gtest.h>
+
+// system includes
+#include <assert.h>
+#include <cstdint>
+#include <iostream>
+#include <random>
+#include <functional>
+#include <type_traits>
+
+#include <ze/common/random.hpp>
+#include <ze/common/test_entrypoint.hpp>
+#include <ze/common/test_utils.hpp>
+#include <ze/common/types.hpp>
+
+#include <imp/core/memory_storage.hpp>
+#include <imp/core/image_raw.hpp>
+#include <imp/core/roi.hpp>
+#include <imp/core/size.hpp>
+
+
+DEFINE_bool(visualize, false, "Show input images and results");
+
+template <typename Pixel>
+class ImageRawTest : public ::testing::Test
+{
+ protected:
+  ImageRawTest()
+    : image_512_(size_512_)
+    , image_511_(size_511_)
+  {
+    using T = typename Pixel::T;
+    auto random_val_generator = ze::uniformDistribution<T>(ZE_DETERMINISTIC);
+
+    // initialize two random value and ensure that they are reasonably different
+    T val1 = random_val_generator();
+    T val2;
+    do
+    {
+      val2 = random_val_generator();
+    } while(std::fabs(val1-val2) < std::numeric_limits<T>::epsilon());
+
+    random_value1_ = Pixel(val1);
+    random_value2_ = Pixel(val2);
+  }
+
+  void setValue()
+  {
+    image_512_.setValue(random_value1_);
+    image_511_.setValue(random_value1_);
+  }
+
+  void setRoi()
+  {
+    image_512_.setRoi(roi_);
+    image_511_.setRoi(roi_);
+  }
+
+  void setValueRoi()
+  {
+    this->setRoi();
+    image_512_.setValue(random_value2_);
+    image_511_.setValue(random_value2_);
+  }
+
+protected:
+  uint8_t pixel_size_ = sizeof(Pixel);
+  size_t pixel_bit_depth_ = 8*sizeof(Pixel);
+
+  ze::Size2u size_512_{512u,512u};
+  ze::Size2u size_511_{511u,512u};
+  ze::Roi2u roi_{128,128,256,256};
+
+  ze::ImageRaw<Pixel> image_512_;
+  ze::ImageRaw<Pixel> image_511_;
+
+
+  Pixel random_value1_;
+  Pixel random_value2_;
+};
+
+// The list of types we want to test.
+typedef testing::Types<
+ze::Pixel8uC1, ze::Pixel8uC2, ze::Pixel8uC3, ze::Pixel8uC4,
+ze::Pixel16uC1, ze::Pixel16uC2, ze::Pixel16uC3, ze::Pixel16uC4,
+ze::Pixel16sC1, ze::Pixel16sC2, ze::Pixel16sC3, ze::Pixel16sC4,
+ze::Pixel32sC1, ze::Pixel32sC2, ze::Pixel32sC3, ze::Pixel32sC4,
+ze::Pixel32fC1, ze::Pixel32fC2, ze::Pixel32fC3, ze::Pixel32fC4> PixelTypes;
+
+TYPED_TEST_CASE(ImageRawTest, PixelTypes);
+
+TYPED_TEST(ImageRawTest, CheckMemforyAlignment)
+{
+  EXPECT_EQ(0u, (std::uintptr_t)reinterpret_cast<void*>(this->image_512_.data()) % 32);
+//  EXPECT_TRUE(ze::MemoryStorage::isAligned(this->image_512_.data()));
+
+  EXPECT_EQ(0u, (std::uintptr_t)reinterpret_cast<void*>(this->image_511_.data()) % 32);
+//  EXPECT_TRUE(ze::MemoryStorage::isAligned(this->image_511_.data()));
+
+  EXPECT_EQ(512u, this->image_512_.stride());
+  EXPECT_EQ(512u, this->image_511_.stride());
+
+  EXPECT_EQ(512u*this->pixel_size_, this->image_512_.pitch());
+  EXPECT_EQ(512u*this->pixel_size_, this->image_511_.pitch());
+}
+
+TYPED_TEST(ImageRawTest, CheckSize)
+{
+  EXPECT_EQ(this->size_512_, this->image_512_.size());
+  EXPECT_EQ(this->size_511_, this->image_511_.size());
+}
+
+TYPED_TEST(ImageRawTest, CheckNoRoi)
+{
+  EXPECT_EQ(ze::Roi2u(0,0,512,512), this->image_512_.roi());
+  EXPECT_EQ(ze::Roi2u(this->size_511_), this->image_511_.roi());
+}
+
+TYPED_TEST(ImageRawTest, CheckRoi)
+{
+  this->setRoi();
+  EXPECT_EQ(this->roi_, this->image_512_.roi());
+  EXPECT_EQ(this->roi_, this->image_511_.roi());
+}
+
+TYPED_TEST(ImageRawTest, CheckBytes)
+{
+  EXPECT_EQ(512u*512u*this->pixel_size_, this->image_512_.bytes());
+  EXPECT_EQ(512u*512u*this->pixel_size_, this->image_511_.bytes());
+
+  EXPECT_EQ(this->size_512_[0]*this->pixel_size_, this->image_512_.rowBytes());
+  EXPECT_EQ(this->size_511_[0]*this->pixel_size_, this->image_511_.rowBytes());
+}
+
+TYPED_TEST(ImageRawTest, CheckPixelBitDepth)
+{
+  EXPECT_EQ(this->pixel_bit_depth_, this->image_512_.bitDepth());
+  EXPECT_EQ(this->pixel_bit_depth_, this->image_511_.bitDepth());
+}
+
+TYPED_TEST(ImageRawTest, ReturnsFalseForNonGpuMemory)
+{
+  ASSERT_FALSE(this->image_512_.isGpuMemory());
+  ASSERT_FALSE(this->image_511_.isGpuMemory());
+}
+
+TYPED_TEST(ImageRawTest, CheckValues)
+{
+  this->setValue();
+  for (ze::uint32_t y=0u; y<512; ++y)
+  {
+    for (ze::uint32_t x=0u; x<512; ++x)
+    {
+      EXPECT_EQ(this->image_512_(x,y), this->random_value1_);
+      EXPECT_EQ(this->image_512_[y][x], this->random_value1_);
+      EXPECT_EQ(*this->image_512_.data(x,y), this->random_value1_);
+
+      if (x<511)
+      {
+        EXPECT_EQ(this->image_511_(x,y), this->random_value1_);
+        EXPECT_EQ(this->image_511_[y][x], this->random_value1_);
+        EXPECT_EQ(*this->image_511_.data(x,y), this->random_value1_);
+      }
+    }
+  }
+}
+
+//-----------------------------------------------------------------------------
+TYPED_TEST(ImageRawTest, CheckRoiValues)
+{
+  this->setValue();
+  this->setValueRoi();
+
+  for (ze::uint32_t y=0u; y<512; ++y)
+  {
+    for (ze::uint32_t x=0u; x<512; ++x)
+    {
+      if (x>=this->roi_.x() && x<(this->roi_.x()+this->roi_.width()) &&
+          y>=this->roi_.y() && y<(this->roi_.y()+this->roi_.height()))
+      {
+        EXPECT_EQ(this->image_512_(x,y), this->random_value2_);
+        EXPECT_EQ(this->image_512_[y][x], this->random_value2_);
+        EXPECT_EQ(*this->image_512_.data(x,y), this->random_value2_);
+
+        if (x<511)
+        {
+          EXPECT_EQ(this->image_511_(x,y), this->random_value2_);
+          EXPECT_EQ(this->image_511_[y][x], this->random_value2_);
+          EXPECT_EQ(*this->image_511_.data(x,y), this->random_value2_);
+        }
+      }
+      else
+      {
+        EXPECT_EQ(this->image_512_(x,y), this->random_value1_);
+        EXPECT_EQ(this->image_512_[y][x], this->random_value1_);
+        EXPECT_EQ(*this->image_512_.data(x,y), this->random_value1_);
+
+        if (x<511)
+        {
+          EXPECT_EQ(this->image_511_(x,y), this->random_value1_);
+          EXPECT_EQ(this->image_511_[y][x], this->random_value1_);
+          EXPECT_EQ(*this->image_511_.data(x,y), this->random_value1_);
+        }
+      }
+    }
+  }
+}
+
+ZE_UNITTEST_ENTRYPOINT
diff --git a/RWR/src/ze_oss/imp_core/test/test_main.cpp b/RWR/src/ze_oss/imp_core/test/test_main.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..fecddc79d2f06e4f44c48471d9f98e04dd0ac96a
--- /dev/null
+++ b/RWR/src/ze_oss/imp_core/test/test_main.cpp
@@ -0,0 +1,32 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#include <gtest/gtest.h>
+
+/// Run all the tests that were declared with TEST()
+int main(int argc, char **argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}
diff --git a/RWR/src/ze_oss/imp_core/test/vec_test.cpp b/RWR/src/ze_oss/imp_core/test/vec_test.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..4ab907f2c5d86331e5db171c13cc69c87b5f068c
--- /dev/null
+++ b/RWR/src/ze_oss/imp_core/test/vec_test.cpp
@@ -0,0 +1,60 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#include <gtest/gtest.h>
+
+// system includes
+#include <assert.h>
+#include <cstdint>
+#include <iostream>
+
+#include <imp/core/pixel.hpp>
+
+template <typename T>
+void testNormalize()
+{
+  T tmp(1);
+  T vec = tmp * 2;
+  auto normalized = ze::normalize(vec);
+  ASSERT_FLOAT_EQ(1.f, ze::length(normalized));
+}
+
+TEST(IMPCoreTestSuite,vecTest)
+{
+  testNormalize<ze::Vec8uC2>();
+  testNormalize<ze::Vec8uC3>();
+  testNormalize<ze::Vec8uC4>();
+
+  testNormalize<ze::Vec16uC2>();
+  testNormalize<ze::Vec16uC3>();
+  testNormalize<ze::Vec16uC4>();
+
+  testNormalize<ze::Vec32sC2>();
+  testNormalize<ze::Vec32sC3>();
+  testNormalize<ze::Vec32sC4>();
+
+  testNormalize<ze::Vec32fC2>();
+  testNormalize<ze::Vec32fC3>();
+  testNormalize<ze::Vec32fC4>();
+}
diff --git a/RWR/src/ze_oss/imp_cu_core/CATKIN_IGNORE b/RWR/src/ze_oss/imp_cu_core/CATKIN_IGNORE
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/RWR/src/ze_oss/imp_cu_core/CMakeLists.txt b/RWR/src/ze_oss/imp_cu_core/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..9a89b6bb85677f770c8f01b59bc110a846bcafbd
--- /dev/null
+++ b/RWR/src/ze_oss/imp_cu_core/CMakeLists.txt
@@ -0,0 +1,74 @@
+project(imp_cu_core)
+cmake_minimum_required(VERSION 2.8.0)
+
+if(${CMAKE_MAJOR_VERSION} VERSION_GREATER 3.0)
+  cmake_policy(SET CMP0054 OLD)
+endif(${CMAKE_MAJOR_VERSION} VERSION_GREATER 3.0)
+
+find_package(catkin_simple REQUIRED)
+catkin_simple(ALL_DEPS_REQUIRED)
+
+include(ze_setup)
+include(ze_macros_cuda)
+find_cuda()
+
+set(HEADERS
+  include/imp/cu_core/cu_utils.hpp
+  include/imp/cu_core/cu_pixel_conversion.hpp
+  )
+
+set(SOURCES
+  src/cu_pixel_conversion.cpp
+  )
+
+set(CU_HDRS
+  include/imp/cu_core/cu_memory_storage.cuh
+  include/imp/cu_core/cu_linearmemory.cuh
+  include/imp/cu_core/cu_image_gpu.cuh
+  include/imp/cu_core/cu_texture2d.cuh
+  include/imp/cu_core/cu_texture.cuh
+  include/imp/cu_core/cu_math.cuh
+  include/imp/cu_core/cu_k_setvalue.cuh
+  include/imp/cu_core/cu_k_derivative.cuh
+  include/imp/cu_core/cu_pinhole_camera.cuh
+  include/imp/cu_core/cu_se3.cuh
+  include/imp/cu_core/cu_matrix.cuh
+  include/imp/cu_core/cu_image_reduction.cuh
+  )
+
+set(CU_SRCS
+  src/cu_linearmemory.cu
+  src/cu_image_gpu.cu
+  src/min_max.cu
+  src/sum.cu
+  src/cu_image_reduction.cu
+  src/weighted_sum.cu
+  )
+
+#cuda_compile(CU_OBJ ${CU_SRC})
+
+cs_cuda_add_library(${PROJECT_NAME}
+   ${CU_SRCS} ${CU_HDRS} ${SOURCES} ${HEADERS}
+   )
+#target_link_libraries(${PROJECT_NAME} ${CUDA_LIBRARIES} ${catkin_LIBRARIES})
+
+###
+### GTESTS
+###
+
+catkin_add_gtest(${PROJECT_NAME}-test
+  test/test_main.cpp
+  test/cu_linearmemory_test.cpp
+  test/image_gpu_simple_test.cpp
+  #test/image_gpu_test.cpp
+  test/min_max_test.cpp
+  test/sum_test.cpp
+  test/reduction_test.cpp
+)
+target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME} pthread)
+
+catkin_add_gtest(test_weighted_sum test/test_weighted_sum.cpp)
+target_link_libraries(test_weighted_sum ${PROJECT_NAME})
+
+cs_install()
+cs_export()
diff --git a/RWR/src/ze_oss/imp_cu_core/include/imp/cu_core/cu_gpu_data.cuh b/RWR/src/ze_oss/imp_cu_core/include/imp/cu_core/cu_gpu_data.cuh
new file mode 100644
index 0000000000000000000000000000000000000000..5b0b027ea5464e28f6f138df3e40dc9d0a2a67fc
--- /dev/null
+++ b/RWR/src/ze_oss/imp_cu_core/include/imp/cu_core/cu_gpu_data.cuh
@@ -0,0 +1,97 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#ifndef IMP_CU_GPU_DATA_CUH
+#define IMP_CU_GPU_DATA_CUH
+
+#include <imp/core/types.hpp>
+#include <imp/core/roi.hpp>
+
+
+namespace imp { namespace cu {
+
+template<typename Pixel>
+struct GpuData2D
+{
+  Pixel* data;
+  std::uint32_t width;
+  std::uint32_t height;
+  imp::Roi2u roi;
+  size_t stride;
+
+  __host__ __device__
+  GpuData2D(Pixel* _data, size_t _stride,
+            std::uint32_t _width, std::uint32_t _height,
+            imp::Roi2u _roi=imp::Roi2u())
+    : data(_data)
+    , stride(_stride)
+    , width(_width)
+    , height(_height)
+    //, roi((_roi == imp::Roi2u()) ? imp::Roi2u(0u, 0u, _width, _height) : _roi)
+  {
+  }
+
+  bool valid() { return (data != nullptr && width>0 && height >0); }
+
+  void roiReset() { roi = imp::Roi2u(0,0,width,height); }
+
+  __host__ __device__ __forceinline__
+  bool inRoi(int x, int y)
+  {
+    if (roi != imp::Roi2u())
+      return (x>=roi.x() && y>=roi.y() && x<roi.width() && y<roi.height());
+    else
+      return inBounds(x,y);
+  }
+
+  __host__ __device__ __forceinline__
+  bool inBounds(int x, int y)
+  {
+    return (x>=0 && y>=0 && x<width && y<height);
+  }
+
+  __host__ __device__ __forceinline__
+  size_t pitch()
+  {
+    return stride*sizeof(Pixel);
+  }
+
+  __host__ __device__ __forceinline__
+  Pixel operator()(int x, int y)
+  {
+    return data[y*stride+x];
+  }
+
+  __host__ __device__ __forceinline__
+  Pixel operator[](int x)
+  {
+    return data[x];
+  }
+};
+
+} // namespace cu
+} // namespace imp
+
+#endif // IMP_CU_GPU_DATA_CUH
+
diff --git a/RWR/src/ze_oss/imp_cu_core/include/imp/cu_core/cu_image_gpu.cuh b/RWR/src/ze_oss/imp_cu_core/include/imp/cu_core/cu_image_gpu.cuh
new file mode 100644
index 0000000000000000000000000000000000000000..b68c68ae2a6e4b2e50f5121121865d8140403d3b
--- /dev/null
+++ b/RWR/src/ze_oss/imp_cu_core/include/imp/cu_core/cu_image_gpu.cuh
@@ -0,0 +1,207 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#ifndef IMP_CU_IMAGE_GPU_CUH
+#define IMP_CU_IMAGE_GPU_CUH
+
+#include <memory>
+#include <algorithm>
+
+#include <imp/core/types.hpp>
+#include <imp/core/pixel_enums.hpp>
+#include <imp/cu_core/cu_memory_storage.cuh>
+#include <imp/core/image.hpp>
+#include <imp/cu_core/cu_pixel_conversion.hpp>
+
+
+namespace ze {
+namespace cu {
+
+// forward declarations
+class Texture2D;
+
+/**
+ * @brief The ImageGpu class is an image (surprise) holding raw memory
+ *
+ * The ImageGpu memory can be used to allocate raw memory of a given size, or
+ * take external memory and hold a reference to that. Note that when external
+ * memory is given to the class, the memory won't be managed! You have to take
+ * care about the memory deletion. Instead when you let the class itself allocate
+ * the memory it will take care of freeing the memory again. In addition the allocation
+ * takes care about memory address alignment (default: 32-byte) for the beginning of
+ * every row.
+ *
+ * The template parameters are as follows:
+ *   - Pixel: The pixel's memory representation (e.g. imp::Pixel8uC1 for single-channel unsigned 8-bit images)
+ *
+ * @warning Be careful with 8-bit 3-channel GPU memory as the stride is not divisable by 3!
+ */
+template<typename Pixel>
+class ImageGpu : public ze::Image<Pixel>
+{
+public:
+  using Ptr = typename std::shared_ptr<ImageGpu<Pixel>>;
+  using UPtr = typename std::unique_ptr<ImageGpu<Pixel>>;
+
+  using Base = Image<Pixel>;
+  using Memory = ze::cu::MemoryStorage<Pixel>;
+  using Deallocator = ze::cu::MemoryDeallocator<Pixel>;
+
+public:
+  ImageGpu() = delete;
+  virtual ~ImageGpu();/* = default;*/
+
+  /**
+   * @brief ImageGpu construcs an image of given \a size
+   */
+  ImageGpu(const ze::Size2u& size);
+
+  /**
+   * @brief ImageGpu construcs an image of given size \a width x \a height
+   */
+  ImageGpu(uint32_t width, uint32_t height)
+    : ImageGpu(ze::Size2u(width,height)) {;}
+
+  /**
+   * @brief ImageGpu copy constructs an image from the given image \a from
+   */
+  ImageGpu(const ImageGpu& from);
+
+  /**
+   * @brief ImageGpu copy construcs a GPU image from an arbitrary base image \a from (not necessarily am \a ImageGpu)
+   */
+  ImageGpu(const Base& from);
+
+  /**
+   * @brief ImageGpu copy constructs a GPU image from the 8-bit (cpu) image \a from
+   */
+  //ImageGpu(const Image8uC3& from);
+
+  /**
+   * @brief ImageGpu constructs an image with the given data (copied or refererenced!)
+   * @param data Pointer to the image data.
+   * @param width Image width.
+   * @param height Image height.
+   * @param pitch Length of a row in bytes (including padding).
+   * @param use_ext_data_pointer Flagg if the image should be copied (true) or if the data is just safed as 'reference' (false)
+   */
+//  ImageGpu(Pixel* data, uint32_t width, uint32_t height,
+//           uint32_t pitch, bool use_ext_data_pointer = false);
+
+  /**
+   * @brief copyTo copies the internal image data to another class instance
+   * @param dst Image class that will receive this image's data.
+   */
+  virtual void copyTo(Base& dst) const override;
+
+  /**
+   * @brief copyFrom copies the image data from another class instance to this image
+   * @param from Image class providing the image data.
+   */
+  virtual void copyFrom(const Base& from) override;
+
+  /** Returns a pointer to the pixel data.
+   * The pointer can be offset to position \a (ox/oy).
+   * @param[in] ox Horizontal/Column offset of the pointer array.
+   * @param[in] oy Vertical/Row offset of the pointer array.
+   * @return Pointer to the pixel array.
+   */
+  virtual Pixel* data(uint32_t ox = 0, uint32_t oy = 0) override;
+  virtual const Pixel* data(uint32_t ox = 0, uint32_t oy = 0) const override;
+
+  /** Returns a cuda vector* that is pointing to the beginning for the data buffer.
+   * @note this is mainly for convenience when calling cuda functions / kernels.
+   */
+  auto cuData() -> decltype(ze::cu::toCudaVectorType(this->data()));
+  auto cuData() const -> decltype(ze::cu::toConstCudaVectorType(this->data()));
+
+  /**
+   * @brief setValue Sets image data to the specified \a value.
+   * @param value Value to be set to the whole image data.
+   * @note @todo (MWE) TBD: region-of-interest is considered
+   */
+  virtual void setValue(const Pixel& value) override;
+
+  /** Returns a data structure to operate within a cuda kernel (does not copy any memory!). */
+//  std::unique_ptr<GpuData2D<Pixel>> gpuData() { return gpu_data_; }
+
+  /** Returns the channel descriptor for Cuda's texture memory. */
+  inline cudaChannelFormatDesc channelFormatDesc() const { return channel_format_desc_; }
+
+  /** Returns a cuda texture object. */
+  std::shared_ptr<Texture2D> genTexture(
+      bool normalized_coords = false,
+      cudaTextureFilterMode filter_mode = cudaFilterModePoint,
+      cudaTextureAddressMode address_mode = cudaAddressModeClamp,
+      cudaTextureReadMode read_mode = cudaReadModeElementType) const;
+
+  // operators
+  /** Pixel-wise multiplication. */
+//  template<typename T>
+//  ImageGpu<Pixel, pixel_type>& operator*=(const T& rhs);
+
+  ImageGpu<Pixel>& operator*=(const Pixel& rhs);
+
+
+protected:
+  std::unique_ptr<Pixel, Deallocator> data_; //!< the actual image data
+
+private:
+  cudaChannelFormatDesc channel_format_desc_;
+
+  //std::unique_ptr<GpuData2D<Pixel>> gpu_data_; //!< data collection that can be directly used within a kernel.
+//  GpuData2D<Pixel>* gpu_data_;
+};
+
+//-----------------------------------------------------------------------------
+// convenience typedefs
+// (sync with explicit template class instantiations at the end of the cpp file)
+typedef ImageGpu<ze::Pixel8uC1> ImageGpu8uC1;
+typedef ImageGpu<ze::Pixel8uC2> ImageGpu8uC2;
+typedef ImageGpu<ze::Pixel8uC3> ImageGpu8uC3;
+typedef ImageGpu<ze::Pixel8uC4> ImageGpu8uC4;
+
+typedef ImageGpu<ze::Pixel16uC1> ImageGpu16uC1;
+typedef ImageGpu<ze::Pixel16uC2> ImageGpu16uC2;
+typedef ImageGpu<ze::Pixel16uC3> ImageGpu16uC3;
+typedef ImageGpu<ze::Pixel16uC4> ImageGpu16uC4;
+
+typedef ImageGpu<ze::Pixel32sC1> ImageGpu32sC1;
+typedef ImageGpu<ze::Pixel32sC2> ImageGpu32sC2;
+typedef ImageGpu<ze::Pixel32sC3> ImageGpu32sC3;
+typedef ImageGpu<ze::Pixel32sC4> ImageGpu32sC4;
+
+typedef ImageGpu<ze::Pixel32fC1> ImageGpu32fC1;
+typedef ImageGpu<ze::Pixel32fC2> ImageGpu32fC2;
+typedef ImageGpu<ze::Pixel32fC3> ImageGpu32fC3;
+typedef ImageGpu<ze::Pixel32fC4> ImageGpu32fC4;
+
+template <typename Pixel>
+using ImageGpuPtr = typename std::shared_ptr<ImageGpu<Pixel>>;
+
+} // namespace cu
+} // namespace ze
+
+
+#endif // IMP_CU_IMAGE_GPU_CUH
diff --git a/RWR/src/ze_oss/imp_cu_core/include/imp/cu_core/cu_image_reduction.cuh b/RWR/src/ze_oss/imp_cu_core/include/imp/cu_core/cu_image_reduction.cuh
new file mode 100644
index 0000000000000000000000000000000000000000..cc8e34e6b3fe6d0bcb7bf0074be33fac1367889c
--- /dev/null
+++ b/RWR/src/ze_oss/imp_cu_core/include/imp/cu_core/cu_image_reduction.cuh
@@ -0,0 +1,57 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#pragma once
+
+#include <imp/cu_core/cu_image_gpu.cuh>
+#include <imp/cu_core/cu_utils.hpp>
+#include <imp/cu_core/cu_linearmemory.cuh>
+
+namespace ze {
+namespace cu {
+
+template<typename Pixel>
+class ImageReducer
+{
+public:
+  ImageReducer();
+  ~ImageReducer();
+
+  // Sum image by reduction
+  Pixel sum(const ImageGpu<Pixel>& in_img);
+
+  // Count elements equal to 'value'
+  size_t countEqual(
+      const ImageGpu32sC1& in_img,
+      int32_t value);
+
+private:
+  Fragmentation<> fragm_{dim3(4, 4, 1), dim3(16, 16, 1)};
+  unsigned int sh_mem_size_;
+  cu::LinearMemory<Pixel> dev_final_{1};
+  ImageGpu<Pixel> partial_;
+};
+
+} // cu namespace
+} // ze namespace
diff --git a/RWR/src/ze_oss/imp_cu_core/include/imp/cu_core/cu_k_derivative.cuh b/RWR/src/ze_oss/imp_cu_core/include/imp/cu_core/cu_k_derivative.cuh
new file mode 100644
index 0000000000000000000000000000000000000000..5fadc741fccfba4e224ee7f6dcc0b61b31e0ac76
--- /dev/null
+++ b/RWR/src/ze_oss/imp_cu_core/include/imp/cu_core/cu_k_derivative.cuh
@@ -0,0 +1,178 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#ifndef IMP_CU_K_DERIVATIVE_CUH
+#define IMP_CU_K_DERIVATIVE_CUH
+
+#include <cuda_runtime.h>
+#include <imp/cu_core/cu_texture.cuh>
+
+namespace ze {
+namespace cu {
+
+//-----------------------------------------------------------------------------
+/** compute forward differences in x- and y- direction */
+static __device__ __forceinline__ float2 dp(
+    const ze::cu::Texture2D& tex, size_t x, size_t y)
+{
+  float2 grad = make_float2(0.0f, 0.0f);
+  float cval = tex2DFetch<float>(tex, x, y);
+  grad.x = tex2DFetch<float>(tex, x+1.f, y) - cval;
+  grad.y = tex2DFetch<float>(tex, x, y+1.f) - cval;
+  return grad;
+}
+
+//-----------------------------------------------------------------------------
+/** compute divergence using backward differences (adjugate from dp). */
+static __device__ __forceinline__
+float dpAd(const ze::cu::Texture2D& tex,
+           size_t x, size_t y, size_t width, size_t height)
+{
+  float2 c = tex2DFetch<float2>(tex, x,y);
+  float2 w = tex2DFetch<float2>(tex, x-1.f, y);
+  float2 n = tex2DFetch<float2>(tex, x, y-1.f);
+
+  if (x == 0)
+    w.x = 0.0f;
+  else if (x >= width-1)
+    c.x = 0.0f;
+
+  if (y == 0)
+    n.y = 0.0f;
+  else if (y >= height-1)
+    c.y = 0.0f;
+
+  return (c.x - w.x + c.y - n.y);
+}
+
+
+
+//-----------------------------------------------------------------------------
+/** compute forward differences in x- and y- direction */
+static __device__ __forceinline__ float2 dpWeighted(
+    const ze::cu::Texture2D& tex, const ze::cu::Texture2D& g_tex,
+    size_t x, size_t y)
+{
+  float cval = tex2DFetch<float>(tex, x, y);
+  float g = tex2DFetch<float>(g_tex, x, y);
+
+  return make_float2(
+        g*(tex2DFetch<float>(tex, x+1.f, y) - cval),
+        g*(tex2DFetch<float>(tex, x, y+1.f) - cval));
+}
+
+//-----------------------------------------------------------------------------
+/** compute weighted divergence using backward differences (adjugate from dp). */
+static __device__ __forceinline__
+float dpAdWeighted(const ze::cu::Texture2D& tex, const ze::cu::Texture2D& g_tex,
+                   size_t x, size_t y, size_t width, size_t height)
+{
+  float2 c = tex2DFetch<float2>(tex, x, y);
+  float2 w = tex2DFetch<float2>(tex, x-1.f, y);
+  float2 n = tex2DFetch<float2>(tex, x, y-1);
+
+  float g = tex2DFetch<float>(g_tex, x, y);
+  float g_w = tex2DFetch<float>(g_tex, x-1.f, y);
+  float g_n = tex2DFetch<float>(g_tex, x, y-1);
+
+  if (x == 0)
+    w.x = 0.0f;
+  else if (x >= width-1)
+    c.x = 0.0f;
+
+  if (y == 0)
+    n.y = 0.0f;
+  else if (y >= height-1)
+    c.y = 0.0f;
+
+  return (c.x*g - w.x*g_w + c.y*g - n.y*g_n);
+}
+
+//-----------------------------------------------------------------------------
+/** compute directed divergence using backward differences (adjugate from dp). */
+static __device__ __forceinline__
+float dpAdDirected(const ze::cu::Texture2D& tex,
+                   const ze::cu::Texture2D& tensor_a_tex,
+                   const ze::cu::Texture2D& tensor_b_tex,
+                   const ze::cu::Texture2D& tensor_c_tex,
+                   size_t x, size_t y, size_t width, size_t height)
+{
+  float2 c = tex2DFetch<float2>(tex, x, y);
+  float2 w = tex2DFetch<float2>(tex, x-1.f, y);
+  float2 n = tex2DFetch<float2>(tex, x, y-1);
+
+  float tensor_a   = tex2DFetch<float>(tensor_a_tex, x,     y);
+  float tensor_a_w = tex2DFetch<float>(tensor_a_tex, x-1.f, y);
+  float tensor_b   = tex2DFetch<float>(tensor_b_tex, x,     y);
+  float tensor_b_n = tex2DFetch<float>(tensor_b_tex, x    , y-1.f);
+  float tensor_c   = tex2DFetch<float>(tensor_c_tex, x,     y);
+  float tensor_c_w = tex2DFetch<float>(tensor_c_tex, x-1.f, y);
+  float tensor_c_n = tex2DFetch<float>(tensor_c_tex, x    , y-1.f);
+
+  if (x == 0)
+    w = make_float2(0.0f, 0.0f);
+  else if (x >= width-1)
+    c.x = 0.0f;
+
+  if (y == 0)
+    n = make_float2(0.0f, 0.0f);
+  else if (y >= height-1)
+    c.y = 0.0f;
+
+  return ((c.x*tensor_a + c.y*tensor_c) - (w.x*tensor_a_w + w.y*tensor_c_w) +
+          (c.x*tensor_c + c.y*tensor_b) - (n.x*tensor_c_n + n.y*tensor_b_n));
+}
+
+//-----------------------------------------------------------------------------
+/** compute directed divergence using backward differences (adjugate from dp). */
+static __device__ __forceinline__
+float dpAdTensor(const ze::cu::Texture2D& tex, const ze::cu::Texture2D& g_tex,
+                 size_t x, size_t y, size_t width, size_t height)
+{
+  float2 c = tex2DFetch<float2>(tex, x, y);
+  float2 w = tex2DFetch<float2>(tex, x-1.f, y);
+  float2 n = tex2DFetch<float2>(tex, x, y-1);
+
+  float2 g = tex2DFetch<float2>(g_tex, x, y);
+  float2 g_w = tex2DFetch<float2>(g_tex, x-1.f, y);
+  float2 g_n = tex2DFetch<float2>(g_tex, x, y-1);
+
+  if (x == 0)
+    w.x = 0.0f;
+  else if (x >= width-1)
+    c.x = 0.0f;
+
+  if (y == 0)
+    n.y = 0.0f;
+  else if (y >= height-1)
+    c.y = 0.0f;
+
+  return (c.x*g.x - w.x*g_w.x + c.y*g.y - n.y*g_n.y);
+}
+
+} // namespace cu
+} // namespace ze
+
+#endif // IMP_CU_K_DERIVATIVE_CUH
+
diff --git a/RWR/src/ze_oss/imp_cu_core/include/imp/cu_core/cu_k_setvalue.cuh b/RWR/src/ze_oss/imp_cu_core/include/imp/cu_core/cu_k_setvalue.cuh
new file mode 100644
index 0000000000000000000000000000000000000000..972d32e2a1bc130db052da0d931ead4303946b9a
--- /dev/null
+++ b/RWR/src/ze_oss/imp_cu_core/include/imp/cu_core/cu_k_setvalue.cuh
@@ -0,0 +1,107 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#ifndef IMP_CU_SETVALUE_CUH
+#define IMP_CU_SETVALUE_CUH
+
+//#include <stdio.h>
+
+#include <cuda_runtime_api.h>
+
+#if 0
+#include <imp/cu_core/cu_gpu_data.cuh>
+#endif
+
+namespace ze {
+namespace cu {
+
+//-----------------------------------------------------------------------------
+/**
+ * @brief
+ */
+#if 0
+template<typename Pixel>
+__global__ void k_setValue(GpuData2D<Pixel>* dst, const Pixel value)
+{
+  int x = blockIdx.x*blockDim.x + threadIdx.x;
+  int y = blockIdx.y*blockDim.y + threadIdx.y;
+
+  // account for roi
+  x+=dst->roi.x();
+  y+=dst->roi.y();
+
+  if (dst->inRoi(x,y))
+  {
+    dst->data[y*dst->stride+x] = value;
+  }
+}
+#endif
+
+
+//-----------------------------------------------------------------------------
+template<typename Pixel>
+__global__ void k_setValue(
+    Pixel* d_dst, uint32_t offset, uint32_t roi_length, const Pixel value)
+{
+  int x = blockIdx.x*blockDim.x + threadIdx.x;
+  if (x<roi_length)
+  {
+    d_dst[x+offset] = value;
+  }
+}
+
+
+//-----------------------------------------------------------------------------
+template<typename Pixel>
+__global__ void k_setValue(Pixel* d_dst, size_t stride, const Pixel value,
+                           size_t width, size_t height)
+{
+  int x = blockIdx.x*blockDim.x + threadIdx.x;
+  int y = blockIdx.y*blockDim.y + threadIdx.y;
+
+  if (x>=0 && y>=0 && x<width && y<height)
+  {
+    d_dst[y*stride+x] = value;
+  }
+}
+
+//-----------------------------------------------------------------------------
+template<typename Pixel, typename T>
+__global__ void k_pixelWiseMul(Pixel* d_dst, size_t stride, const T rhs,
+                               size_t width, size_t height)
+{
+  int x = blockIdx.x*blockDim.x + threadIdx.x;
+  int y = blockIdx.y*blockDim.y + threadIdx.y;
+
+  if (x>=0 && y>=0 && x<width && y<height)
+  {
+    d_dst[y*stride+x] = d_dst[y*stride+x]*rhs;
+  }
+}
+
+
+} // namespace cu
+} // namespace ze
+
+#endif // IMP_CU_SETVALUE_CUH
diff --git a/RWR/src/ze_oss/imp_cu_core/include/imp/cu_core/cu_linearmemory.cuh b/RWR/src/ze_oss/imp_cu_core/include/imp/cu_core/cu_linearmemory.cuh
new file mode 100644
index 0000000000000000000000000000000000000000..31214bc0160213669fe9c366924488f3c806072e
--- /dev/null
+++ b/RWR/src/ze_oss/imp_cu_core/include/imp/cu_core/cu_linearmemory.cuh
@@ -0,0 +1,138 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#ifndef IMP_CU_LINEARHOSTMEMORY_CUH
+#define IMP_CU_LINEARHOSTMEMORY_CUH
+
+//#include <stdio.h>
+//#include <assert.h>
+//#include <cstdlib>
+#include <memory>
+
+#include <imp/core/linearmemory_base.hpp>
+#include <imp/core/linearmemory.hpp>
+#include <imp/core/pixel.hpp>
+#include <imp/cu_core/cu_memory_storage.cuh>
+#include <imp/cu_core/cu_pixel_conversion.hpp>
+
+namespace ze {
+namespace cu {
+
+template<typename Pixel>
+class LinearMemory : public LinearMemoryBase
+{
+public:
+  using Memory = ze::cu::MemoryStorage<Pixel>;
+  using Deallocator = ze::cu::MemoryDeallocator<Pixel>;
+
+public:
+  __host__ LinearMemory() = delete;
+  virtual ~LinearMemory() { }
+
+  __host__ LinearMemory(const uint32_t& length);
+  __host__ LinearMemory(const ze::cu::LinearMemory<Pixel>& from);
+  __host__ LinearMemory(const ze::LinearMemory<Pixel>& from);
+//  __host__ LinearMemory(Pixel* host_data, const uint32_t& length,
+//                        bool use_ext_data_pointer = false);
+
+  /**
+   * @brief Returns a pointer to the device buffer.
+   */
+  Pixel* data();
+  const Pixel* data() const;
+
+  /** Returns the buffer as respective cuda vector type that is pointing to the beginning for the data buffer.
+   * @note this is mainly for convenience when calling cuda functions / kernels.
+   */
+  auto cuData() -> decltype(ze::cu::toCudaVectorType(this->data()));
+  auto cuData() const -> decltype(ze::cu::toConstCudaVectorType(this->data()));
+
+  /** Sets a certain value to all pixels in the data vector.
+   */
+  void setValue(const Pixel& value);
+
+  /** Copy data to a host class instance.
+   */
+  void copyTo(ze::cu::LinearMemory<Pixel>& dst);
+
+  /** Copy data from a host class instance.
+   */
+  void copyFrom(const ze::cu::LinearMemory<Pixel>& dst);
+
+  /** Copy data to a host class instance.
+   */
+  void copyTo(ze::LinearMemory<Pixel>& dst);
+
+  /** Copy data from a host class instance.
+   */
+  void copyFrom(const ze::LinearMemory<Pixel>& dst);
+
+  /** Returns the total amount of bytes saved in the data buffer. */
+  virtual size_t bytes() const override { return this->length()*sizeof(Pixel); }
+
+  /** Returns the total amount of bytes for the region-of-interest saved in the data buffer. */
+  virtual size_t roiBytes() const override { return this->roi().length()*sizeof(Pixel); }
+
+
+  /** Returns the bit depth of the data pointer. */
+  virtual std::uint8_t bitDepth() const override { return 8*sizeof(Pixel); }
+
+  /** Returns flag if the image data resides on the device/GPU (TRUE) or host/GPU (FALSE) */
+  virtual bool isGpuMemory() const  override { return true; }
+
+private:
+  std::unique_ptr<Pixel, Deallocator> data_;
+};
+
+// convenience typedefs
+// (sync with explicit template class instantiations at the end of the cpp file)
+typedef LinearMemory<ze::Pixel8uC1> LinearMemory8uC1;
+typedef LinearMemory<ze::Pixel8uC2> LinearMemory8uC2;
+typedef LinearMemory<ze::Pixel8uC3> LinearMemory8uC3;
+typedef LinearMemory<ze::Pixel8uC4> LinearMemory8uC4;
+
+typedef LinearMemory<ze::Pixel16uC1> LinearMemory16uC1;
+typedef LinearMemory<ze::Pixel16uC2> LinearMemory16uC2;
+typedef LinearMemory<ze::Pixel16uC3> LinearMemory16uC3;
+typedef LinearMemory<ze::Pixel16uC4> LinearMemory16uC4;
+
+typedef LinearMemory<ze::Pixel32uC1> LinearMemory32uC1;
+typedef LinearMemory<ze::Pixel32uC2> LinearMemory32uC2;
+typedef LinearMemory<ze::Pixel32uC3> LinearMemory32uC3;
+typedef LinearMemory<ze::Pixel32uC4> LinearMemory32uC4;
+
+typedef LinearMemory<ze::Pixel32sC1> LinearMemory32sC1;
+typedef LinearMemory<ze::Pixel32sC2> LinearMemory32sC2;
+typedef LinearMemory<ze::Pixel32sC3> LinearMemory32sC3;
+typedef LinearMemory<ze::Pixel32sC4> LinearMemory32sC4;
+
+typedef LinearMemory<ze::Pixel32fC1> LinearMemory32fC1;
+typedef LinearMemory<ze::Pixel32fC2> LinearMemory32fC2;
+typedef LinearMemory<ze::Pixel32fC3> LinearMemory32fC3;
+typedef LinearMemory<ze::Pixel32fC4> LinearMemory32fC4;
+
+} // namespace cu
+} // namespace ze
+
+#endif // IMP_LINEARHOSTMEMORY_H
diff --git a/RWR/src/ze_oss/imp_cu_core/include/imp/cu_core/cu_math.cuh b/RWR/src/ze_oss/imp_cu_core/include/imp/cu_core/cu_math.cuh
new file mode 100644
index 0000000000000000000000000000000000000000..21e7315991fe4babbdf46eefd11e26bfcd33205d
--- /dev/null
+++ b/RWR/src/ze_oss/imp_cu_core/include/imp/cu_core/cu_math.cuh
@@ -0,0 +1,85 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#ifndef IMP_CU_MATH_CUH
+#define IMP_CU_MATH_CUH
+
+#include <memory>
+#include <imp/cu_core/cu_image_gpu.cuh>
+
+namespace ze {
+namespace cu {
+
+/**
+ * @brief Finding min and max pixel value of given image
+ * @note For multi-channel images, the seperate channels are not handeled individually.
+ */
+template<typename Pixel>
+void minMax(const ImageGpu<Pixel>& img, Pixel& min, Pixel& max);
+
+template<typename Pixel>
+void minMax(const Texture2D& img_tex, Pixel& min, Pixel& max, const ze::Roi2u& roi);
+
+/**
+ * @brief Computing the sum of all pixels
+ * @note For multi-channel images, the seperate channels are not handeled individually.
+ */
+template<typename Pixel>
+Pixel sum(const ImageGpu<Pixel>& img);
+
+template<typename Pixel>
+Pixel sum(const Texture2D& img_tex, const ze::Roi2u& roi);
+
+/** Weighted sum of two images (dst not allocated).
+ * \param src1 Source image 1.
+ * \param weight1 Multiplicative weight of image 1.
+ * \param src2 Source image 2.
+ * \param weight1 Multiplicative weight of image 1.
+ * \param dst Result image dst=weight1*src1 + weight1*src2.
+ *
+ * \note supported gpu: 8uC1, 32f_C1
+ */
+template<typename Pixel>
+void weightedSum(ImageGpu<Pixel>& dst,
+                 const ImageGpu<Pixel>& src1, const float& weight1,
+                 const ImageGpu<Pixel>& src2, const float& weight2);
+
+/** Weighted sum of two images (dst internally allocated).
+ * \param src1 Source image 1.
+ * \param weight1 Multiplicative weight of image 1.
+ * \param src2 Source image 2.
+ * \param weight1 Multiplicative weight of image 1.
+ * \param dst Result image dst=weight1*src1 + weight1*src2.
+ *
+ * \note supported gpu: 8uC1, 32f_C1
+ */
+template<typename Pixel>
+ImageGpuPtr<Pixel> weightedSum(const ImageGpu<Pixel>& src1, const float& weight1,
+                               const ImageGpu<Pixel>& src2, const float& weight2);
+
+} // namespace cu
+} // namespace ze
+
+#endif // IMP_CU_MATH_CUH
+
diff --git a/RWR/src/ze_oss/imp_cu_core/include/imp/cu_core/cu_matrix.cuh b/RWR/src/ze_oss/imp_cu_core/include/imp/cu_core/cu_matrix.cuh
new file mode 100644
index 0000000000000000000000000000000000000000..131263a729f3f7463bf377a6dd12412612a86971
--- /dev/null
+++ b/RWR/src/ze_oss/imp_cu_core/include/imp/cu_core/cu_matrix.cuh
@@ -0,0 +1,250 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#pragma once
+
+#include <cuda_runtime.h>
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wunused-local-typedefs"
+# include <Eigen/Dense>
+#pragma GCC diagnostic pop
+#include <memory>
+#include <ostream>
+
+#include <imp/core/pixel.hpp>
+#include <ze/common/matrix.hpp>
+
+namespace ze {
+namespace cu {
+
+//------------------------------------------------------------------------------
+template<typename _Type, uint32_t _rows, uint32_t _cols>
+class Matrix
+{
+  using Type = _Type;
+
+public:
+  typedef std::shared_ptr<Matrix> Ptr;
+
+  __host__ __device__
+  Matrix() = default;
+
+  __host__ __device__
+  ~Matrix() = default;
+
+  __host__
+  Matrix(const ze::Matrix3& from)
+  {
+    // copy element by element
+    for (uint32_t row = 0u; row < _rows; ++row)
+    {
+      for (uint32_t col = 0u; col < _cols; ++col)
+      {
+        data_[row*cols_ + col] = from(row, col);
+      }
+    }
+  }
+
+  __host__ __device__ __forceinline__
+  uint32_t rows() const { return rows_; }
+
+  __host__ __device__ __forceinline__
+  uint32_t cols() const { return cols_; }
+
+  /** Data access operator given a \a row and a \a col
+   * @return unchangable value at \a (row,col)
+   */
+  __host__ __device__ __forceinline__
+  const Type& operator()(uint32_t row, uint32_t col) const
+  {
+    return data_[row*cols_ + col];
+  }
+
+  /** Data access operator given a \a row and a \a col
+   * @return changable value at \a (row,col)
+   */
+  __host__ __device__ __forceinline__
+  Type& operator()(uint32_t row, uint32_t col)
+  {
+    return data_[row*cols_ + col];
+  }
+
+  /** Data access operator given an \a index
+   * @return unchangable value at \a (row,col)
+   */
+  __host__ __device__ __forceinline__
+  const Type& operator[](uint32_t ind) const
+  {
+    return data_[ind];
+  }
+
+  /** Data access operator given an \a index
+   * @return changable value at \a (row,col)
+   */
+  __host__ __device__ __forceinline__
+  Type & operator[](uint32_t ind)
+  {
+    return data_[ind];
+  }
+
+  template<uint32_t block_rows,uint32_t block_cols>
+  __host__ __forceinline__
+  Matrix<Type, block_rows, block_cols> block(uint32_t top_left_row, uint32_t top_left_col) const
+  {
+    Matrix<Type, block_rows, block_cols> out;
+    uint32_t data_offset = top_left_row * cols_ + top_left_col;
+    for(uint32_t row = 0u; row < block_rows; ++row)
+    {
+      memcpy(&out[row * block_cols],
+             &data_[data_offset+row*cols_],
+             block_cols * sizeof(Type));
+    }
+    return out;
+  }
+
+#if 0
+  template<typename TypeFrom>
+  __host__ inline Matrix(const Eigen::Matrix<TypeFrom,R,C>& mat)
+  {
+    for (uint32_t row = 0u; row < R; ++row)
+    {
+      for (uint32_t col = 0u; col < C; ++col)
+      {
+        data[row * C + col] = (Type)mat(row,col);
+      }
+    }
+  }
+#endif
+
+private:
+  Type data_[_rows * _cols];
+  uint32_t rows_ = _rows;
+  uint32_t cols_ = _cols;
+};
+
+//------------------------------------------------------------------------------
+// convenience typedefs
+using Matrix3f = Matrix<float,3,3>;
+using Vector3f = Matrix<float,1,3>;
+
+//==============================================================================
+
+
+//------------------------------------------------------------------------------
+template<typename Type, uint32_t _rows, uint32_t CR, uint32_t _cols>
+__host__ __device__ __forceinline__
+Matrix<Type, _rows, _cols> operator*(const Matrix<Type, _rows, CR> & lhs,
+                                     const Matrix<Type, CR, _cols> & rhs)
+{
+  Matrix<Type, _rows, _cols> result;
+  for (uint32_t row = 0u; row < _rows; ++row)
+  {
+    for (uint32_t col = 0u; col < _cols; ++col)
+    {
+      result(row, col) = 0;
+      for (uint32_t i = 0u; i < CR; ++i)
+      {
+        result(row, col) += lhs(row,i) * rhs(i,col);
+      }
+    }
+  }
+  return result;
+}
+
+//------------------------------------------------------------------------------
+template<typename Type>
+__host__ __device__ __forceinline__
+Matrix<Type, 2, 2> invert(const Matrix<Type, 2, 2> & in)
+{
+  Matrix<Type, 2, 2> out;
+  float det = in[0]*in[3] - in[1]*in[2];
+  out[0] =  in[3] / det;
+  out[1] = -in[1] / det;
+  out[2] = -in[2] / det;
+  out[3] =  in[0] / det;
+  return out;
+}
+
+
+//------------------------------------------------------------------------------
+// matrix vector multiplication
+__host__ __device__ __forceinline__
+float3 operator*(const Matrix3f& mat, const float3& v)
+{
+  return make_float3(
+        mat(0,0)*v.x + mat(0,1)*v.y + mat(0,2)*v.z,
+        mat(1,0)*v.x + mat(1,1)*v.y + mat(1,2)*v.z,
+        mat(2,0)*v.x + mat(2,1)*v.y + mat(2,2)*v.z
+        );
+}
+
+//------------------------------------------------------------------------------
+// matrix vector multiplication
+__host__ __device__ __forceinline__
+Vec32fC3 operator*(const Matrix3f& mat, const Vec32fC3& v)
+{
+  return Vec32fC3(
+        mat(0,0)*v.x + mat(0,1)*v.y + mat(0,2)*v.z,
+        mat(1,0)*v.x + mat(1,1)*v.y + mat(1,2)*v.z,
+        mat(2,0)*v.x + mat(2,1)*v.y + mat(2,2)*v.z
+        );
+}
+
+//------------------------------------------------------------------------------
+// transformation matrix three-vector multiplication
+__host__ __device__ __forceinline__
+float3 transform(const Matrix<float,3,4>& T, const float3& v)
+{
+  return make_float3(
+        T(0,0)*v.x + T(0,1)*v.y + T(0,2)*v.z + T(0,3),
+        T(1,0)*v.x + T(1,1)*v.y + T(1,2)*v.z + T(1,3),
+        T(2,0)*v.x + T(2,1)*v.y + T(2,2)*v.z + T(2,3)
+        );
+}
+
+//------------------------------------------------------------------------------
+template<typename T, uint32_t rows, uint32_t cols>
+__host__
+inline std::ostream& operator<<(std::ostream &os,
+                                const cu::Matrix<T, rows, cols>& m)
+{
+  os << "[";
+  for (uint32_t r=0u; r<rows; ++r)
+  {
+    for (uint32_t c=0u; c<cols; ++c)
+    {
+      os << m(r,c);
+      if (c<cols-1)
+      {
+        os << ",";
+      }
+    }
+    os << "; ";
+  }
+  os << "]";
+  return os;
+}
+
+} // namespace cu
+} // namespace ze
diff --git a/RWR/src/ze_oss/imp_cu_core/include/imp/cu_core/cu_memory_storage.cuh b/RWR/src/ze_oss/imp_cu_core/include/imp/cu_core/cu_memory_storage.cuh
new file mode 100644
index 0000000000000000000000000000000000000000..2e9705025d93e7d418bba427f90a2a262a7ac472
--- /dev/null
+++ b/RWR/src/ze_oss/imp_cu_core/include/imp/cu_core/cu_memory_storage.cuh
@@ -0,0 +1,154 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#pragma once
+
+#include <iostream>
+#include <cuda_runtime_api.h>
+#include <imp/core/types.hpp>
+#include <imp/core/pixel_enums.hpp>
+#include <imp/core/size.hpp>
+#include <ze/common/logging.hpp>
+
+namespace ze {
+namespace cu {
+
+template <typename Pixel, ze::PixelType pixel_type = ze::PixelType::undefined>
+struct MemoryStorage
+{
+public:
+  // we don't construct an instance but only use it for the static functions
+  // (at least for the moment)
+  MemoryStorage() = delete;
+  virtual ~MemoryStorage() = delete;
+
+  //! @todo (MWE) do we wanna have a init flag for device memory?
+  static Pixel* alloc(const uint32_t num_elements)
+  {
+    CHECK_GT(num_elements, 0u);
+
+    const uint32_t memory_size = sizeof(Pixel) * num_elements;
+    //std::cout << "cu::MemoryStorage::alloc: memory_size=" << memory_size << "; sizeof(Pixel)=" << sizeof(Pixel) << std::endl;
+
+    Pixel* p_data = nullptr;
+    cudaError_t cu_err = cudaMalloc((void**)&p_data, memory_size);
+
+    CHECK_NE(cu_err, cudaErrorMemoryAllocation);
+    CHECK_EQ(cu_err, cudaSuccess);
+
+    return p_data;
+  }
+
+  /**
+   * @brief alignedAlloc allocates an aligned 2D memory block (\a width x \a height) on the GPU (CUDA)
+   * @param width Image width
+   * @param height Image height
+   * @param pitch Row alignment [bytes]
+   *
+   */
+  static Pixel* alignedAlloc(const uint32_t width, const uint32_t height,
+                             uint32_t* pitch)
+  {
+    CHECK_GT(width, 0u);
+    CHECK_GT(height, 0u);
+
+    uint32_t width_bytes = width * sizeof(Pixel);
+    //std::cout << "width_bytes: " << width_bytes << std::endl;
+    const int align_bytes = 1536;
+    if (pixel_type == ze::PixelType::i8uC3 && width_bytes % align_bytes)
+    {
+      width_bytes += (align_bytes-(width_bytes%align_bytes));
+    }
+    //std::cout << "width_bytes: " << width_bytes << std::endl;
+
+    size_t intern_pitch;
+    Pixel* p_data = nullptr;
+    cudaError_t cu_err = cudaMallocPitch((void **)&p_data, &intern_pitch,
+                                         width_bytes, (uint32_t)height);
+
+    *pitch = static_cast<uint32_t>(intern_pitch);
+    //("pitch: %lu, i_pitch: %lu, width_bytes: %lu\n", *pitch, intern_pitch, width_bytes);
+
+    CHECK_NE(cu_err, cudaErrorMemoryAllocation);
+    CHECK_EQ(cu_err, cudaSuccess);
+
+    return p_data;
+  }
+
+
+  /**
+   * @brief alignedAlloc allocates an aligned 2D memory block of given \a size on the GPU (CUDA)
+   * @param size Image size
+   * @param pitch Row alignment [bytes]
+   * @return
+   */
+  static Pixel* alignedAlloc(ze::Size2u size, uint32_t* pitch)
+  {
+    return alignedAlloc(size[0], size[1], pitch);
+  }
+
+
+  static void free(Pixel* buffer)
+  {
+    cudaFree(buffer);
+  }
+
+};
+
+
+/**
+ * @brief The Deallocator struct offers the ability to have custom deallocation methods.
+ *
+ * The Deallocator struct can be used as e.g. having custom deallocations with
+ * shared pointers. Furthermore it enables the usage of external memory buffers
+ * using shared pointers but not taking ownership of the memory. Be careful when
+ * doing so as an application would behave badly if the memory got deleted although
+ * we are still using it.
+ *
+ */
+template<typename Pixel>
+struct MemoryDeallocator
+{
+  // Default custom deleter assuming we use arrays (new PixelType[length])
+  MemoryDeallocator()
+    : f([](Pixel* p) { cudaFree(p); })
+{ }
+
+// allow us to define a custom deallocator
+explicit MemoryDeallocator(std::function<void(Pixel*)> const &_f)
+  : f(_f)
+{ }
+
+void operator()(Pixel* p) const
+{
+  f(p);
+}
+
+private:
+std::function< void(Pixel* )> f;
+};
+
+} // namespace cu
+} // namespace ze
+
diff --git a/RWR/src/ze_oss/imp_cu_core/include/imp/cu_core/cu_pinhole_camera.cuh b/RWR/src/ze_oss/imp_cu_core/include/imp/cu_core/cu_pinhole_camera.cuh
new file mode 100644
index 0000000000000000000000000000000000000000..c4b116a53c94c68355fc5aea43740ff3ab32e84b
--- /dev/null
+++ b/RWR/src/ze_oss/imp_cu_core/include/imp/cu_core/cu_pinhole_camera.cuh
@@ -0,0 +1,168 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#ifndef IMP_CU_PINHOLE_CAMERA_CUH
+#define IMP_CU_PINHOLE_CAMERA_CUH
+
+#include <cuda_runtime_api.h>
+#include <imp/core/pixel.hpp>
+#include <imp/cu_core/cu_matrix.cuh>
+
+namespace ze {
+namespace cu {
+
+/**
+ * @brief The PinholeCamera class implements a very simple pinhole camera model.
+ *        Points in the image plane are denoted by the coordinate vectors (u,v) whereas
+ *        world coordinates use the (x,y,z) nominclatur.
+ *
+ * @todo (MWE) marry off with other camera implementations (maybe with aslam_cv2 camera models? -> resolve Eigen vs. CUDA issues first)
+ *
+ */
+class PinholeCamera
+{
+public:
+  typedef std::shared_ptr<PinholeCamera> Ptr;
+
+  __host__ PinholeCamera() = default;
+  __host__ ~PinholeCamera() = default;
+
+  __host__ PinholeCamera(float fu, float fv, float cu, float cv)
+    : f_(fu, fv)
+    , c_(cu, cv)
+  {
+  }
+
+  // copy and asignment operator
+  __host__ __device__
+  PinholeCamera(const PinholeCamera& other)
+    : f_(other.f())
+    , c_(other.c())
+  {
+  }
+  __host__ __device__
+  PinholeCamera& operator=(const PinholeCamera& other)
+  {
+    if  (this != &other)
+    {
+      f_ = other.f();
+      c_ = other.c();
+    }
+    return *this;
+  }
+
+  __host__ __device__ __forceinline__
+  ze::cu::Matrix3f intrinsics()
+  {
+    ze::cu::Matrix3f K;
+    K(0,0) = f_.x;
+    K(0,1) = 0.0f;
+    K(0,2) = c_.x;
+    K(1,0) = 0.0f;
+    K(1,1) = f_.y;
+    K(1,2) = c_.y;
+    K(2,0) = 0.0f;
+    K(2,1) = 0.0f;
+    K(2,2) = 1.0f;
+    return K;
+  }
+
+  /** Multiplies the focal length as well as the principal point with the given \a scale \a factor
+   */
+  __host__ __device__ __forceinline__
+  void scale(float scale_factor)
+  {
+    f_ *= scale_factor;
+    c_ *= scale_factor;
+  }
+
+  __host__ __device__ __forceinline__
+  PinholeCamera& operator*=(float rhs)
+  {
+    this->scale(rhs);
+    return *this;
+  }
+  __host__ __device__ __forceinline__
+  PinholeCamera operator*(float rhs) const
+  {
+    PinholeCamera result = *this;
+    result *= rhs;
+    return result;
+  }
+
+
+//  __host__ __device__ __forceinline__
+//  Vec32fC3 cam2world(const Vec32fC2& uv) const
+//  {
+//    return Vec32fC3((uv.x-c_.x)/f_.x,
+//                    (uv.y-c_.y)/f_.y,
+//                    1.0f);
+//  }
+
+//  __host__ __device__ __forceinline__
+//  Vec32fC2 world2cam(const Vec32fC3& p) const
+//  {
+//    return Vec32fC2(f_.x*p.x/p.z + c_.x,
+//                    f_.y*p.y/p.z + c_.y);
+//  }
+
+  __host__ __device__ __forceinline__
+  float3 cam2world(const float2& uv) const
+  {
+    return make_float3((uv.x-c_.x)/f_.x,
+                       (uv.y-c_.y)/f_.y,
+                       1.0f);
+  }
+
+  __host__ __device__ __forceinline__
+  float2 world2cam(const float3& p) const
+  {
+    return make_float2(f_.x*p.x/p.z + c_.x,
+                       f_.y*p.y/p.z + c_.y);
+  }
+
+
+  //
+  // accessors
+  //
+
+  __host__ __device__ __forceinline__ const Vec32fC2& f() const {return f_;}
+  __host__ __device__ __forceinline__ const Vec32fC2& c() const  {return c_;}
+
+  __host__ __device__ __forceinline__ float fx() const  {return f_.x;}
+  __host__ __device__ __forceinline__ float fy() const  {return f_.y;}
+  __host__ __device__ __forceinline__ float cx() const  {return c_.x;}
+  __host__ __device__ __forceinline__ float cy() const  {return c_.y;}
+
+private:
+  Vec32fC2 f_; //!< focal length {fx, fy}
+  Vec32fC2 c_; //!< principal point {cx, cy}
+
+};
+
+}
+}
+
+#endif // IMP_CU_PINHOLE_CAMERA_CUH
+
diff --git a/RWR/src/ze_oss/imp_cu_core/include/imp/cu_core/cu_pixel_conversion.hpp b/RWR/src/ze_oss/imp_cu_core/include/imp/cu_core/cu_pixel_conversion.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..81a11c6488042903ff0a750fd2bc74ab4525f334
--- /dev/null
+++ b/RWR/src/ze_oss/imp_cu_core/include/imp/cu_core/cu_pixel_conversion.hpp
@@ -0,0 +1,123 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#pragma once
+
+#include <imp/core/pixel.hpp>
+#include <imp/core/pixel_enums.hpp>
+
+namespace ze {
+namespace cu {
+
+//
+// uchar
+//
+unsigned char* __host__ __device__ toCudaVectorType(ze::Pixel8uC1* buffer) __attribute__ ((unused));
+uchar2* __host__ __device__ toCudaVectorType(ze::Pixel8uC2* buffer) __attribute__ ((unused));
+uchar3* __host__ __device__ toCudaVectorType(ze::Pixel8uC3* buffer) __attribute__ ((unused));
+uchar4* __host__ __device__ toCudaVectorType(ze::Pixel8uC4* buffer) __attribute__ ((unused));
+
+//
+// ushort
+//
+unsigned short* __host__ __device__ toCudaVectorType(ze::Pixel16uC1* buffer) __attribute__ ((unused));
+ushort2* __host__ __device__ toCudaVectorType(ze::Pixel16uC2* buffer) __attribute__ ((unused));
+ushort3* __host__ __device__ toCudaVectorType(ze::Pixel16uC3* buffer) __attribute__ ((unused));
+ushort4* __host__ __device__ toCudaVectorType(ze::Pixel16uC4* buffer) __attribute__ ((unused));
+
+//
+// unsigned int
+//
+unsigned int* __host__ __device__ toCudaVectorType(ze::Pixel32uC1* buffer) __attribute__ ((unused));
+uint2* __host__ __device__ toCudaVectorType(ze::Pixel32uC2* buffer) __attribute__ ((unused));
+uint3* __host__ __device__ toCudaVectorType(ze::Pixel32uC3* buffer) __attribute__ ((unused));
+uint4* __host__ __device__ toCudaVectorType(ze::Pixel32uC4* buffer) __attribute__ ((unused));
+
+//
+// int
+//
+int* __host__ __device__ toCudaVectorType(ze::Pixel32sC1* buffer) __attribute__ ((unused));
+int2* __host__ __device__ toCudaVectorType(ze::Pixel32sC2* buffer) __attribute__ ((unused));
+int3* __host__ __device__ toCudaVectorType(ze::Pixel32sC3* buffer) __attribute__ ((unused));
+int4* __host__ __device__ toCudaVectorType(ze::Pixel32sC4* buffer) __attribute__ ((unused));
+
+//
+// float
+//
+float* __host__ __device__ toCudaVectorType(ze::Pixel32fC1* buffer) __attribute__ ((unused));
+float2* __host__ __device__ toCudaVectorType(ze::Pixel32fC2* buffer) __attribute__ ((unused));
+float3* __host__ __device__ toCudaVectorType(ze::Pixel32fC3* buffer) __attribute__ ((unused));
+float4* __host__ __device__ toCudaVectorType(ze::Pixel32fC4* buffer) __attribute__ ((unused));
+
+//------------------------------------------------------------------------------
+//
+// uchar
+//
+const unsigned char* __host__ __device__ toConstCudaVectorType(const ze::Pixel8uC1* buffer) __attribute__ ((unused));
+const uchar2* __host__ __device__ toConstCudaVectorType(const ze::Pixel8uC2* buffer) __attribute__ ((unused));
+const uchar3* __host__ __device__ toConstCudaVectorType(const ze::Pixel8uC3* buffer) __attribute__ ((unused));
+const uchar4* __host__ __device__ toConstCudaVectorType(const ze::Pixel8uC4* buffer) __attribute__ ((unused));
+
+//
+// ushort
+//
+const unsigned short* __host__ __device__ toConstCudaVectorType(const ze::Pixel16uC1* buffer) __attribute__ ((unused));
+const ushort2* __host__ __device__ toConstCudaVectorType(const ze::Pixel16uC2* buffer) __attribute__ ((unused));
+const ushort3* __host__ __device__ toConstCudaVectorType(const ze::Pixel16uC3* buffer) __attribute__ ((unused));
+const ushort4* __host__ __device__ toConstCudaVectorType(const ze::Pixel16uC4* buffer) __attribute__ ((unused));
+
+//
+// unsigned int
+//
+const unsigned int* __host__ __device__ toConstCudaVectorType(const ze::Pixel32uC1* buffer) __attribute__ ((unused));
+const uint2* __host__ __device__ toConstCudaVectorType(const ze::Pixel32uC2* buffer) __attribute__ ((unused));
+const uint3* __host__ __device__ toConstCudaVectorType(const ze::Pixel32uC3* buffer) __attribute__ ((unused));
+const uint4* __host__ __device__ toConstCudaVectorType(const ze::Pixel32uC4* buffer) __attribute__ ((unused));
+
+
+//
+// int
+//
+const int* __host__ __device__ toConstCudaVectorType(const ze::Pixel32sC1* buffer) __attribute__ ((unused));
+const int2* __host__ __device__ toConstCudaVectorType(const ze::Pixel32sC2* buffer) __attribute__ ((unused));
+const int3* __host__ __device__ toConstCudaVectorType(const ze::Pixel32sC3* buffer) __attribute__ ((unused));
+const int4* __host__ __device__ toConstCudaVectorType(const ze::Pixel32sC4* buffer) __attribute__ ((unused));
+
+//
+// float
+//
+const float* __host__ __device__ toConstCudaVectorType(const ze::Pixel32fC1* buffer) __attribute__ ((unused));
+const float2* __host__ __device__ toConstCudaVectorType(const ze::Pixel32fC2* buffer) __attribute__ ((unused));
+const float3* __host__ __device__ toConstCudaVectorType(const ze::Pixel32fC3* buffer) __attribute__ ((unused));
+const float4* __host__ __device__ toConstCudaVectorType(const ze::Pixel32fC4* buffer) __attribute__ ((unused));
+
+
+cudaChannelFormatDesc toCudaChannelFormatDesc(ze::PixelType pixel_type) __attribute__ ((unused));
+
+
+
+} // namespace cu
+} // namespace ze
+
+
diff --git a/RWR/src/ze_oss/imp_cu_core/include/imp/cu_core/cu_se3.cuh b/RWR/src/ze_oss/imp_cu_core/include/imp/cu_core/cu_se3.cuh
new file mode 100644
index 0000000000000000000000000000000000000000..9d8d370d86cce142e627665aa5f192d52f4c96c0
--- /dev/null
+++ b/RWR/src/ze_oss/imp_cu_core/include/imp/cu_core/cu_se3.cuh
@@ -0,0 +1,269 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#ifndef IMP_CU_SE3_CUH
+#define IMP_CU_SE3_CUH
+
+#include <ostream>
+#include <imp/core/pixel.hpp>
+#include <imp/cu_core/cu_matrix.cuh>
+#include <ze/common/transformation.hpp>
+
+namespace ze
+{
+namespace cu
+{
+
+template<typename Type>
+class SE3
+{
+public:
+  __host__ __device__ __forceinline__
+  SE3()
+  {
+    data_(0, 0) = 1;
+    data_(0, 1) = 0;
+    data_(0, 2) = 0;
+    data_(1, 0) = 0;
+    data_(1, 1) = 1;
+    data_(1, 2) = 0;
+    data_(2, 0) = 0;
+    data_(2, 1) = 0;
+    data_(2, 2) = 1;
+
+    data_(0, 3) = 0;
+    data_(1, 3) = 0;
+    data_(2, 3) = 0 ;
+  }
+
+  __host__ __forceinline__
+  SE3(const Transformation& T)
+  {
+    const Quaternion q = T.getRotation();
+    const Vector3 t = T.getPosition();
+
+    initializeFromQuaternionAndTranslation(
+          static_cast<Type>(q.w()), static_cast<Type>(q.x()),
+          static_cast<Type>(q.y()), static_cast<Type>(q.z()),
+          static_cast<Type>(t.x()), static_cast<Type>(t.y()),
+          static_cast<Type>(t.z()));
+  }
+
+  /// Constructor from a normalized quaternion and a translation vector
+  __host__ __device__ __forceinline__
+  SE3(Type qw, Type qx, Type qy, Type qz, Type tx, Type ty, Type tz)
+  {
+    initializeFromQuaternionAndTranslation(qw, qx, qy, qz,
+                                           tx, ty, tz);
+  }
+
+  /// Construct from C arrays
+  /// r is rotation matrix row major
+  /// t is the translation vector (x y z)
+  __host__ __device__ __forceinline__
+  SE3(Type * r, Type * t)
+  {
+    data_[0]=r[0]; data_[1]=r[1]; data_[2] =r[2]; data_[3] =t[0];
+    data_[4]=r[3]; data_[5]=r[4]; data_[6] =r[5]; data_[7] =t[1];
+    data_[8]=r[6]; data_[9]=r[7]; data_[10]=r[8]; data_[11]=t[2];
+  }
+
+  /// Construct from C array of a 4x4 transformation matrix
+  /// m is a 4x4 transformation matrix (with [0 0 0 1] in the last row)
+  __host__ __device__ __forceinline__
+  SE3(Type *m)
+  {
+    data_[0]=m[0]; data_[1]=m[1]; data_[2] =m[2]; data_[3] =m[3];
+    data_[4]=m[4]; data_[5]=m[5]; data_[6] =m[6]; data_[7] =m[7];
+    data_[8]=m[8]; data_[9]=m[9]; data_[10]=m[10]; data_[11]=m[11];
+  }
+
+  __host__ __device__ __forceinline__
+  SE3<Type> inv() const
+  {
+    SE3<Type> result;
+    result.data_[0]  = data_[0];
+    result.data_[1]  = data_[4];
+    result.data_[2]  = data_[8];
+    result.data_[4]  = data_[1];
+    result.data_[5]  = data_[5];
+    result.data_[6]  = data_[9];
+    result.data_[8]  = data_[2];
+    result.data_[9]  = data_[6];
+    result.data_[10] = data_[10];
+    result.data_[3]  = -data_[0]*data_[3] -data_[4]*data_[7] -data_[8] *data_[11];
+    result.data_[7]  = -data_[1]*data_[3] -data_[5]*data_[7] -data_[9] *data_[11];
+    result.data_[11] = -data_[2]*data_[3] -data_[6]*data_[7] -data_[10]*data_[11];
+    return result;
+  }
+
+  __host__ __device__ __forceinline__
+  Type operator()(int r, int c) const
+  {
+    return data_(r, c);
+  }
+
+  __host__ __device__ __forceinline__
+  Type& operator()(int r, int c)
+  {
+    return data_(r, c);
+  }
+
+  __host__ __device__ __forceinline__
+  Type& operator[](int idx)
+  {
+    return data_[idx];
+  }
+
+  __host__ __device__ __forceinline__
+  const Type& operator[](int idx) const
+  {
+    return data_[idx];
+  }
+
+  __host__ __device__ __forceinline__
+  Vec32fC3 rotate(const Vec32fC3& p) const
+  {
+    return Vec32fC3(data_(0,0)*p.x + data_(0,1)*p.y + data_(0,2)*p.z,
+                    data_(1,0)*p.x + data_(1,1)*p.y + data_(1,2)*p.z,
+                    data_(2,0)*p.x + data_(2,1)*p.y + data_(2,2)*p.z);
+  }
+  __host__ __device__ __forceinline__
+  float3 rotate(const float3& p) const
+  {
+    return make_float3(data_(0,0)*p.x + data_(0,1)*p.y + data_(0,2)*p.z,
+                       data_(1,0)*p.x + data_(1,1)*p.y + data_(1,2)*p.z,
+                       data_(2,0)*p.x + data_(2,1)*p.y + data_(2,2)*p.z);
+  }
+
+  __host__ __device__ __forceinline__
+  Vec32fC3 translate(const Vec32fC3& p) const
+  {
+    return Vec32fC3(p.x + data_(0,3),
+                    p.y + data_(1,3),
+                    p.z + data_(2,3));
+  }
+
+  __host__ __device__ __forceinline__
+  float3 translate(const float3& p) const
+  {
+    return make_float3(p.x + data_(0,3),
+                       p.y + data_(1,3),
+                       p.z + data_(2,3));
+  }
+
+  __host__ __device__ __forceinline__
+  Vec32fC3 getTranslation() const
+  {
+    return Vec32fC3(data_(0,3),
+                    data_(1,3),
+                    data_(2,3));
+  }
+
+private:
+  __host__ __device__ __forceinline__
+  void initializeFromQuaternionAndTranslation(
+      Type qw, Type qx, Type qy, Type qz,
+      Type tx, Type ty, Type tz)
+  {
+    const Type x  = 2*qx;
+    const Type y  = 2*qy;
+    const Type z  = 2*qz;
+    const Type wx = x*qw;
+    const Type wy = y*qw;
+    const Type wz = z*qw;
+    const Type xx = x*qx;
+    const Type xy = y*qx;
+    const Type xz = z*qx;
+    const Type yy = y*qy;
+    const Type yz = z*qy;
+    const Type zz = z*qz;
+
+    data_(0, 0) = 1-(yy+zz);
+    data_(0, 1) = xy-wz;
+    data_(0, 2) = xz+wy;
+    data_(1, 0) = xy+wz;
+    data_(1, 1) = 1-(xx+zz);
+    data_(1, 2) = yz-wx;
+    data_(2, 0) = xz-wy;
+    data_(2, 1) = yz+wx;
+    data_(2, 2) = 1-(xx+yy);
+
+    data_(0, 3) = tx;
+    data_(1, 3) = ty;
+    data_(2, 3) = tz;
+  }
+  ze::cu::Matrix<Type, 3, 4> data_;
+};
+
+template<typename Type>
+__host__ __device__ __forceinline__
+SE3<Type> operator*(const SE3<Type>& lhs, const SE3<Type>& rhs)
+{
+  SE3<Type> result;
+  result[0]  = lhs[0]*rhs[0] + lhs[1]*rhs[4] + lhs[2]*rhs[8];
+  result[1]  = lhs[0]*rhs[1] + lhs[1]*rhs[5] + lhs[2]*rhs[9];
+  result[2]  = lhs[0]*rhs[2] + lhs[1]*rhs[6] + lhs[2]*rhs[10];
+  result[3]  = lhs[3] + lhs[0]*rhs[3] + lhs[1]*rhs[7] + lhs[2]*rhs[11];
+  result[4]  = lhs[4]*rhs[0] + lhs[5]*rhs[4] + lhs[6]*rhs[8];
+  result[5]  = lhs[4]*rhs[1] + lhs[5]*rhs[5] + lhs[6]*rhs[9];
+  result[6]  = lhs[4]*rhs[2] + lhs[5]*rhs[6] + lhs[6]*rhs[10];
+  result[7]  = lhs[7] + lhs[4]*rhs[3] + lhs[5]*rhs[7] + lhs[6]*rhs[11];
+  result[8]  = lhs[8]*rhs[0] + lhs[9]*rhs[4] + lhs[10]*rhs[8];
+  result[9]  = lhs[8]*rhs[1] + lhs[9]*rhs[5] + lhs[10]*rhs[9];
+  result[10] = lhs[8]*rhs[2] + lhs[9]*rhs[6] + lhs[10]*rhs[10];
+  result[11] = lhs[11] + lhs[8]*rhs[3] + lhs[9]*rhs[7] + lhs[10]*rhs[11];
+  return result;
+}
+
+__host__ __device__ __forceinline__
+Vec32fC3 operator*(const SE3<float>& se3, const Vec32fC3& p)
+{
+  return se3.translate(se3.rotate(p));
+}
+
+__host__ __device__ __forceinline__
+float3 operator*(const SE3<float>& se3, const float3& p)
+{
+  return se3.translate(se3.rotate(p));
+}
+
+template<typename T>
+inline std::ostream& operator<<(std::ostream &os, const cu::SE3<T>& rhs)
+{
+  for (int r=0; r<3; ++r)
+  {
+    for (int c=0; c<4; ++c)
+    {
+      os << rhs(r,c) << ", ";
+    }
+    os << "; ";
+  }
+  return os;
+}
+
+} // namespace cu
+} // namespace ze
+
+#endif // IMP_CU_SE3_CUH
diff --git a/RWR/src/ze_oss/imp_cu_core/include/imp/cu_core/cu_texture.cuh b/RWR/src/ze_oss/imp_cu_core/include/imp/cu_core/cu_texture.cuh
new file mode 100644
index 0000000000000000000000000000000000000000..9762469d5ba1d7b877ebcc6bdf15517983a7a759
--- /dev/null
+++ b/RWR/src/ze_oss/imp_cu_core/include/imp/cu_core/cu_texture.cuh
@@ -0,0 +1,169 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#ifndef IMP_CU_TEXTURE_CUH
+#define IMP_CU_TEXTURE_CUH
+
+#include <cuda_runtime.h>
+#include <imp/cu_core/cu_pixel_conversion.hpp>
+#include <imp/cu_core/cu_texture2d.cuh>
+
+namespace ze {
+namespace cu {
+
+//-----------------------------------------------------------------------------
+__device__ __forceinline__ void tex2DFetch(
+    ze::Pixel8uC1& texel, const Texture2D& tex, float x, float y,
+    float mul_x=1.f, float mul_y=1.f, float add_x=0.f, float add_y=0.f)
+{
+  texel = ze::Pixel8uC1(::tex2D<uchar1>(tex.tex_object, x*mul_x+add_x+0.5f, y*mul_y+add_y+.5f).x);
+}
+
+__device__ __forceinline__ void tex2DFetch(
+    ze::Pixel8uC2& texel, const Texture2D& tex, float x, float y,
+    float mul_x=1.f, float mul_y=1.f, float add_x=0.f, float add_y=0.f)
+{
+  uchar2 val = ::tex2D<uchar2>(tex.tex_object, x*mul_x+add_x+0.5f, y*mul_y+add_y+.5f);
+  texel = ze::Pixel8uC2(val.x, val.y);
+}
+
+__device__ __forceinline__ void tex2DFetch(
+    ze::Pixel8uC4& texel, const Texture2D& tex, float x, float y,
+    float mul_x=1.f, float mul_y=1.f, float add_x=0.f, float add_y=0.f)
+{
+  uchar4 val = ::tex2D<uchar4>(tex.tex_object, x*mul_x+add_x+0.5f, y*mul_y+add_y+.5f);
+  texel = ze::Pixel8uC4(val.x, val.y, val.z, val.w);
+}
+
+//-----------------------------------------------------------------------------
+__device__ __forceinline__ void tex2DFetch(
+    ze::Pixel16uC1& texel, const Texture2D& tex, float x, float y,
+    float mul_x=1.f, float mul_y=1.f, float add_x=0.f, float add_y=0.f)
+{
+  texel = ze::Pixel16uC1(::tex2D<ushort1>(tex.tex_object, x*mul_x+add_x+0.5f, y*mul_y+add_y+.5f).x);
+}
+
+__device__ __forceinline__ void tex2DFetch(
+    ze::Pixel16uC2& texel, const Texture2D& tex, float x, float y,
+    float mul_x=1.f, float mul_y=1.f, float add_x=0.f, float add_y=0.f)
+{
+  ushort2 val = ::tex2D<ushort2>(tex.tex_object, x*mul_x+add_x+0.5f, y*mul_y+add_y+.5f);
+  texel = ze::Pixel16uC2(val.x, val.y);
+}
+
+__device__ __forceinline__ void tex2DFetch(
+    ze::Pixel16uC4& texel, const Texture2D& tex, float x, float y,
+    float mul_x=1.f, float mul_y=1.f, float add_x=0.f, float add_y=0.f)
+{
+  ushort4 val = ::tex2D<ushort4>(tex.tex_object, x*mul_x+add_x+0.5f, y*mul_y+add_y+.5f);
+  texel = ze::Pixel16uC4(val.x, val.y, val.z, val.w);
+}
+
+//-----------------------------------------------------------------------------
+__device__ __forceinline__ void tex2DFetch(
+    ze::Pixel32sC1& texel, const Texture2D& tex, float x, float y,
+    float mul_x=1.f, float mul_y=1.f, float add_x=0.f, float add_y=0.f)
+{
+  texel = ze::Pixel32sC1(::tex2D<int1>(tex.tex_object, x*mul_x+add_x+0.5f, y*mul_y+add_y+.5f).x);
+}
+
+__device__ __forceinline__ void tex2DFetch(
+    ze::Pixel32sC2& texel, const Texture2D& tex, float x, float y,
+    float mul_x=1.f, float mul_y=1.f, float add_x=0.f, float add_y=0.f)
+{
+  int2 val = ::tex2D<int2>(tex.tex_object, x*mul_x+add_x+0.5f, y*mul_y+add_y+.5f);
+  texel = ze::Pixel32sC2(val.x, val.y);
+}
+
+__device__ __forceinline__ void tex2DFetch(
+    ze::Pixel32sC4& texel, const Texture2D& tex, float x, float y,
+    float mul_x=1.f, float mul_y=1.f, float add_x=0.f, float add_y=0.f)
+{
+  int4 val = ::tex2D<int4>(tex.tex_object, x*mul_x+add_x+0.5f, y*mul_y+add_y+.5f);
+  texel = ze::Pixel32sC4(val.x, val.y, val.z, val.w);
+}
+
+//-----------------------------------------------------------------------------
+__device__ __forceinline__ void tex2DFetch(
+    ze::Pixel32fC1& texel, const Texture2D& tex, float x, float y,
+    float mul_x=1.f, float mul_y=1.f, float add_x=0.f, float add_y=0.f)
+{
+  texel = ze::Pixel32fC1(::tex2D<float1>(tex.tex_object, x*mul_x+add_x+0.5f, y*mul_y+add_y+.5f).x);
+}
+
+__device__ __forceinline__ void tex2DFetch(
+    ze::Pixel32fC2& texel, const Texture2D& tex, float x, float y,
+    float mul_x=1.f, float mul_y=1.f, float add_x=0.f, float add_y=0.f)
+{
+  float2 val = ::tex2D<float2>(tex.tex_object, x*mul_x+add_x+0.5f, y*mul_y+add_y+.5f);
+  texel = ze::Pixel32fC2(val.x, val.y);
+}
+
+__device__ __forceinline__ void tex2DFetch(
+    ze::Pixel32fC4& texel, const Texture2D& tex, float x, float y,
+    float mul_x=1.f, float mul_y=1.f, float add_x=0.f, float add_y=0.f)
+{
+  float4 val = ::tex2D<float4>(tex.tex_object, x*mul_x+add_x+0.5f, y*mul_y+add_y+.5f);
+  texel = ze::Pixel32fC4(val.x, val.y, val.z, val.w);
+}
+
+//-----------------------------------------------------------------------------
+__device__ __forceinline__ void tex2DFetch(
+    float& texel, const Texture2D& tex, float x, float y,
+    float mul_x=1.f, float mul_y=1.f, float add_x=0.f, float add_y=0.f)
+{
+  texel = ::tex2D<float>(tex.tex_object, x*mul_x+add_x+0.5f, y*mul_y+add_y+.5f);
+}
+
+//-----------------------------------------------------------------------------
+template<typename T>
+__device__ __forceinline__
+T tex2DFetch(
+    const Texture2D& tex, float x, float y,
+    float mul_x=1.f, float mul_y=1.f, float add_x=0.f, float add_y=0.f)
+{
+  return ::tex2D<T>(
+        tex.tex_object, x*mul_x+add_x+0.5f, y*mul_y+add_y+.5f);
+}
+
+////-----------------------------------------------------------------------------
+//template<typename T>
+//__device__ __forceinline__
+//typename std::enable_if<!std::is_integral<T>::value && !std::is_floating_point<T>::value, T>::type
+//tex2DFetch(
+//    const Texture2D& tex, float x, float y,
+//    float mul_x=1.f, float mul_y=1.f, float add_x=0.f, float add_y=0.f)
+//{
+//  T val;
+//  tex2DFetch(val, tex, x, y, mul_x, mul_y, add_x, add_y);
+//  return val;
+//}
+
+
+
+} // namespace cu
+} // namespace ze
+
+#endif // IMP_CU_TEXTURE_CUH
+
diff --git a/RWR/src/ze_oss/imp_cu_core/include/imp/cu_core/cu_texture2d.cuh b/RWR/src/ze_oss/imp_cu_core/include/imp/cu_core/cu_texture2d.cuh
new file mode 100644
index 0000000000000000000000000000000000000000..ddbadc0226a52277049ae740d8170b85fa90322a
--- /dev/null
+++ b/RWR/src/ze_oss/imp_cu_core/include/imp/cu_core/cu_texture2d.cuh
@@ -0,0 +1,135 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#ifndef IMP_CU_TEXTURE2D_CUH
+#define IMP_CU_TEXTURE2D_CUH
+
+#include <memory>
+#include <cstring>
+#include <cuda_runtime_api.h>
+#include <ze/common/logging.hpp>
+#include <imp/core/pixel.hpp>
+#include <imp/core/pixel_enums.hpp>
+#include <imp/core/size.hpp>
+#include <imp/core/types.hpp>
+
+namespace ze {
+namespace cu {
+
+/**
+ * @brief The Texture2D struct wrappes the cuda texture object
+ */
+struct Texture2D
+{
+  cudaTextureObject_t tex_object;
+  __device__ __forceinline__ operator cudaTextureObject_t() const {return tex_object;}
+
+  using Ptr = std::shared_ptr<Texture2D>;
+  //  using UPtr = std::unique_ptr<Texture2D>;
+
+  __host__ Texture2D()
+    : tex_object(0)
+  {
+  }
+
+  __host__ __device__ Texture2D(cudaTextureObject_t _tex_object)
+    : tex_object(_tex_object)
+  {
+  }
+
+  __host__ Texture2D(const void* data, uint32_t pitch,
+                     cudaChannelFormatDesc channel_desc,
+                     ze::Size2u size,
+                     bool _normalized_coords = false,
+                     cudaTextureFilterMode filter_mode = cudaFilterModePoint,
+                     cudaTextureAddressMode address_mode = cudaAddressModeClamp,
+                     cudaTextureReadMode read_mode = cudaReadModeElementType)
+  {
+    cudaResourceDesc tex_res;
+    std::memset(&tex_res, 0, sizeof(tex_res));
+    tex_res.resType = cudaResourceTypePitch2D;
+    tex_res.res.pitch2D.width = size.width();
+    tex_res.res.pitch2D.height = size.height();
+    tex_res.res.pitch2D.pitchInBytes = pitch;
+    tex_res.res.pitch2D.devPtr = const_cast<void*>(data);
+    tex_res.res.pitch2D.desc = channel_desc;
+
+    cudaTextureDesc tex_desc;
+    std::memset(&tex_desc, 0, sizeof(tex_desc));
+    tex_desc.normalizedCoords = (_normalized_coords==true) ? 1 : 0;
+    tex_desc.filterMode = filter_mode;
+    tex_desc.addressMode[0] = address_mode;
+    tex_desc.addressMode[1] = address_mode;
+    tex_desc.readMode = read_mode;
+
+    cudaError_t err = cudaCreateTextureObject(&tex_object, &tex_res, &tex_desc, 0);
+    CHECK_EQ(err, ::cudaSuccess);
+  }
+
+  __host__ virtual ~Texture2D()
+  {
+    cudaError_t err = cudaDestroyTextureObject(tex_object);
+    CHECK_EQ(err, ::cudaSuccess);
+  }
+
+  // copy and asignment operator enforcing deep copy
+  __host__ __device__
+  Texture2D(const Texture2D& other)
+    : tex_object(other.tex_object)
+  {
+      cudaTextureDesc texture_desc;
+      cudaResourceViewDesc resource_view_desc;
+      cudaResourceDesc resource_desc;
+
+      cudaGetTextureObjectTextureDesc(&texture_desc, other.tex_object);
+      cudaGetTextureObjectResourceViewDesc(&resource_view_desc, other.tex_object);
+      cudaGetTextureObjectResourceDesc(&resource_desc, other.tex_object);
+
+      cudaCreateTextureObject(&this->tex_object, &resource_desc, &texture_desc, &resource_view_desc);
+  }
+  __host__ __device__
+  Texture2D& operator=(const Texture2D& other)
+  {
+    if  (this != &other)
+    {
+      cudaTextureDesc texture_desc;
+      cudaResourceViewDesc resource_view_desc;
+      cudaResourceDesc resource_desc;
+
+      cudaGetTextureObjectTextureDesc(&texture_desc, other.tex_object);
+      cudaGetTextureObjectResourceViewDesc(&resource_view_desc, other.tex_object);
+      cudaGetTextureObjectResourceDesc(&resource_desc, other.tex_object);
+
+      cudaCreateTextureObject(&this->tex_object, &resource_desc, &texture_desc, &resource_view_desc);
+    }
+    return *this;
+  }
+};
+
+
+} // namespace cu
+} // namespace ze
+
+#endif // IMP_CU_TEXTURE2D_CUH
+
diff --git a/RWR/src/ze_oss/imp_cu_core/include/imp/cu_core/cu_utils.hpp b/RWR/src/ze_oss/imp_cu_core/include/imp/cu_core/cu_utils.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..8a1ffea0ddf19025a9f798e1f9f24edadc774873
--- /dev/null
+++ b/RWR/src/ze_oss/imp_cu_core/include/imp/cu_core/cu_utils.hpp
@@ -0,0 +1,325 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#pragma once
+
+#include <cuda_runtime_api.h>
+#include <cstdint>
+#ifndef __CUDA__ARCH__
+#include <cmath>
+#endif
+#include <type_traits>
+#include <ze/common/logging.hpp>
+#include <imp/core/pixel.hpp>
+#include <imp/core/roi.hpp>
+#include <imp/core/size.hpp>
+
+namespace ze {
+namespace cu {
+
+//------------------------------------------------------------------------------
+/** Integer division rounding up to next higher integer
+ * @param a Numerator
+ * @param b Denominator
+ * @return a / b rounded up
+ */
+__host__ __device__ __forceinline__
+uint32_t divUp(uint32_t a, uint32_t b)
+{
+  return (a % b != 0) ? (a / b + 1) : (a / b);
+}
+
+template<typename T>
+__host__ __device__ __forceinline__
+bool isfinite(const T& val)
+{
+  return true;
+//#ifdef __CUDA__ARCH__
+//  return ::isfinite(val);
+//#else
+//  return std::isfinite(val);
+//#endif
+}
+
+template<typename T>
+__host__ __device__ __forceinline__
+typename std::enable_if<std::is_integral<T>::value ||std::is_floating_point<T>::value, T>::type
+min(const T& a, const T& b, bool check_inf_or_nan=true)
+{
+  if (ze::cu::isfinite(a) && ze::cu::isfinite(b))
+    return a<b ? a : b;
+  else if (ze::cu::isfinite(a))
+    return a;
+  else
+    return b;
+
+//    if (check_inf_or_nan)
+//    {
+//      if (isnan(a) || isinf(a))
+//        return b;
+//      if (isnan(b) || isinf(b))
+//        return a;
+//    }
+}
+
+//template<typename T>
+//__host__ __device__ __forceinline__
+//typename std::enable_if<std::is_floating_point<T>::value, T>::type
+//min(const T& a, const T& b, bool check_inf_or_nan=true)
+//{
+//    if (check_inf_or_nan)
+//    {
+//      if (std::isnan(a) || std::isinf(a))
+//        return b;
+//      if (std::isnan(b) || std::isinf(b))
+//        return a;
+//    }
+//  return a<b ? a : b;
+//}
+
+template<typename T>
+__host__ __device__ __forceinline__
+Pixel1<T> min(const Pixel1<T>& a, const Pixel1<T>& b)
+{
+  Pixel1<T> result;
+  for (auto i=0; i<a.numDims(); ++i)
+    result[i] = (a[i]<b[i]) ? a[i] : b[i];
+  return result;
+}
+template<typename T>
+__host__ __device__ __forceinline__
+Pixel2<T> min(const Pixel2<T>& a, const Pixel2<T>& b)
+{
+  Pixel2<T> result;
+  for (auto i=0; i<a.numDims(); ++i)
+    result[i] = (a[i]<b[i]) ? a[i] : b[i];
+  return result;
+}
+template<typename T>
+__host__ __device__ __forceinline__
+Pixel3<T> min(const Pixel3<T>& a, const Pixel3<T>& b)
+{
+  Pixel3<T> result;
+  for (auto i=0; i<a.numDims(); ++i)
+    result[i] = (a[i]<b[i]) ? a[i] : b[i];
+  return result;
+}
+template<typename T>
+__host__ __device__ __forceinline__
+Pixel4<T> min(const Pixel4<T>& a, const Pixel4<T>& b)
+{
+  Pixel4<T> result;
+  for (auto i=0; i<a.numDims(); ++i)
+    result[i] = (a[i]<b[i]) ? a[i] : b[i];
+  return result;
+}
+
+//------------------------------------------------------------------------------
+template<typename T>
+__host__ __device__ __forceinline__
+typename std::enable_if<std::is_integral<T>::value ||std::is_floating_point<T>::value, T>::type
+max(const T& a, const T& b, bool check_inf_or_nan=true)
+{
+  if (ze::cu::isfinite(a) && ze::cu::isfinite(b))
+    return a>b ? a : b;
+  else if (ze::cu::isfinite(a))
+    return a;
+  else
+    return b;
+
+}
+
+//template<typename T>
+//__host__ __device__ __forceinline__
+//typename std::enable_if<std::is_floating_point<T>::value, T>::type
+//max(const T& a, const T& b, bool check_inf_or_nan=true)
+//{
+//    if (check_inf_or_nan)
+//    {
+//      if (std::isnan(a) || std::isinf(a))
+//        return b;
+//      if (std::isnan(b) || std::isinf(b))
+//        return a;
+//    }
+//  return a>b ? a : b;
+//}
+
+template<typename T>
+__host__ __device__ __forceinline__
+Pixel1<T> max(const Pixel1<T>& a, const Pixel1<T>& b)
+{
+  Pixel1<T> result;
+  for (auto i=0; i<a.numDims(); ++i)
+    result[i] = (a[i]>b[i]) ? a[i] : b[i];
+  return result;
+}
+template<typename T>
+__host__ __device__ __forceinline__
+Pixel2<T> max(const Pixel2<T>& a, const Pixel2<T>& b)
+{
+  Pixel2<T> result;
+  for (auto i=0; i<a.numDims(); ++i)
+    result[i] = (a[i]>b[i]) ? a[i] : b[i];
+  return result;
+}
+template<typename T>
+__host__ __device__ __forceinline__
+Pixel3<T> max(const Pixel3<T>& a, const Pixel3<T>& b)
+{
+  Pixel3<T> result;
+  for (auto i=0; i<a.numDims(); ++i)
+    result[i] = (a[i]>b[i]) ? a[i] : b[i];
+  return result;
+}
+template<typename T>
+__host__ __device__ __forceinline__
+Pixel4<T> max(const Pixel4<T>& a, const Pixel4<T>& b)
+{
+  Pixel4<T> result;
+  for (auto i=0; i<a.numDims(); ++i)
+    result[i] = (a[i]>b[i]) ? a[i] : b[i];
+  return result;
+}
+
+template<typename T>
+__host__ __device__ __forceinline__
+typename std::enable_if<!std::is_floating_point<T>::value && !std::is_integral<T>::value, T>::type
+min(const T& a, const T& b, bool check_inf_or_nan=true)
+{
+  return a<b ? a : b;
+}
+
+/// @todo (MWE) rvalue?
+template<typename T>
+__host__ __device__ __forceinline__
+T sqr(const T& a) {return a*a;}
+
+
+/** Fragmentation for gpu / cuda grid blocks
+ */
+template <std::uint16_t block_size_x=32,
+          std::uint16_t block_size_y=32,
+          std::uint16_t block_size_z=1>
+struct Fragmentation
+{
+  //  imp::Size2u size;
+  //  imp::Roi2u roi;
+  dim3 dimBlock = dim3(block_size_x, block_size_y, block_size_z);
+  dim3 dimGrid;
+
+
+  Fragmentation() = delete;
+
+  Fragmentation(size_t length)
+    : dimGrid(divUp(length, dimBlock.x), dimBlock.x, dimBlock.y)
+  {
+  }
+
+  Fragmentation(ze::Size2u sz)
+    : dimGrid(divUp(sz.width(), dimBlock.x), divUp(sz.height(), dimBlock.y))
+  {
+  }
+
+  Fragmentation(ze::Roi2u roi)
+    : dimGrid(divUp(roi.width(), dimBlock.x), divUp(roi.height(), dimBlock.y))
+  {
+  }
+  Fragmentation(uint32_t width, uint32_t height)
+    : dimGrid(divUp(width, dimBlock.x), divUp(height, dimBlock.y))
+  {
+  }
+
+  Fragmentation(dim3 _dimGrid, dim3 _dimBlock)
+    : dimGrid(_dimGrid)
+    , dimBlock(_dimBlock)
+  {
+  }
+};
+
+//------------------------------------------------------------------------------
+template <std::uint16_t block_size_x=16,
+          std::uint16_t block_size_y=16,
+          std::uint16_t block_size_z=1>
+inline std::ostream& operator<<(
+    std::ostream &os,
+    const  Fragmentation<block_size_x, block_size_y, block_size_z>& frag)
+{
+  os << "GPU Fragmentation: block: "
+     << frag.dimBlock.x << "," << frag.dimBlock.y << "," << frag.dimBlock.z
+     << "; grid: " << frag.dimGrid.x << "," << frag.dimGrid.y << "," << frag.dimGrid.z
+     << ";";
+  return os;
+}
+
+//##############################################################################
+
+/** Check for CUDA error */
+static inline void checkCudaErrorState(const char* file, const char* function,
+                                       const int line)
+{
+  cudaDeviceSynchronize();
+  cudaError_t err = cudaGetLastError();
+  CHECK_EQ(err, ::cudaSuccess);
+}
+
+/** Macro for checking on cuda errors
+ * @note This check is only enabled when the compile time flag is set
+ * @todo (MWE) we should enable this whenever we compile in debug mode
+ */
+#ifdef FATAL_CUDA_ERROR
+#  define IMP_CUDA_CHECK() ze::cu::checkCudaErrorState(__FILE__, __FUNCTION__, __LINE__)
+#else
+#  define IMP_CUDA_CHECK() cudaDeviceSynchronize()
+#endif
+
+static inline float getTotalGPUMemory()
+{
+  size_t total = 0;
+  size_t free = 0;
+  cudaMemGetInfo(&free, &total);
+  return total/(1024.0f*1024.0f);   // return value in Megabytes
+}
+
+static inline float getFreeGPUMemory()
+{
+  size_t total = 0;
+  size_t free = 0;
+  cudaMemGetInfo(&free, &total);
+  return free/(1024.0f*1024.0f);   // return value in Megabytes
+}
+
+static inline void printGPUMemoryUsage()
+{
+  float total = ze::cu::getTotalGPUMemory();
+  float free = ze::cu::getFreeGPUMemory();
+
+  printf("GPU memory usage\n");
+  printf("----------------\n");
+  printf("   Total memory: %.2f MiB\n", total);
+  printf("   Used memory:  %.2f MiB\n", total-free);
+  printf("   Free memory:  %.2f MiB\n", free);
+}
+
+} // namespace cu
+} // namespace ze
diff --git a/RWR/src/ze_oss/imp_cu_core/package.xml b/RWR/src/ze_oss/imp_cu_core/package.xml
new file mode 100644
index 0000000000000000000000000000000000000000..aabf50cbb432c995451d322657db9f94d05011e9
--- /dev/null
+++ b/RWR/src/ze_oss/imp_cu_core/package.xml
@@ -0,0 +1,21 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>imp_cu_core</name>
+  <description>
+    IMP core module for GPGPU processing (CUDA)
+  </description>
+  <version>0.1.4</version>
+  <license>ZE</license>
+
+  <maintainer email="code@werlberger.org">Manuel Werlberger</maintainer>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <buildtool_depend>catkin_simple</buildtool_depend>
+
+  <depend>eigen_catkin</depend>
+
+  <depend>ze_cmake</depend>
+  <depend>imp_core</depend>
+
+  <test_depend>gtest</test_depend>
+</package>
diff --git a/RWR/src/ze_oss/imp_cu_core/src/cu_image_gpu.cu b/RWR/src/ze_oss/imp_cu_core/src/cu_image_gpu.cu
new file mode 100644
index 0000000000000000000000000000000000000000..b2289bce2bc3a6e5f671753dc8d5e9de8e4ff0c6
--- /dev/null
+++ b/RWR/src/ze_oss/imp_cu_core/src/cu_image_gpu.cu
@@ -0,0 +1,244 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#include <imp/cu_core/cu_image_gpu.cuh>
+#include <iostream>
+#include <memory>
+#include <ze/common/logging.hpp>
+#include <imp/cu_core/cu_linearmemory.cuh>
+#include <imp/cu_core/cu_texture.cuh>
+#include <imp/cu_core/cu_utils.hpp>
+// kernel includes
+#include <imp/cu_core/cu_k_setvalue.cuh>
+
+namespace ze {
+namespace cu {
+
+//-----------------------------------------------------------------------------
+template<typename Pixel>
+ImageGpu<Pixel>::ImageGpu(const ze::Size2u& size)
+  : Base(size)
+{
+  data_.reset(Memory::alignedAlloc(this->size(), &this->header_.pitch));
+  channel_format_desc_ = toCudaChannelFormatDesc(pixel_type<Pixel>::type);
+  this->header_.memory_type = MemoryType::GpuAligned;
+}
+
+//-----------------------------------------------------------------------------
+template<typename Pixel>
+ImageGpu<Pixel>::ImageGpu(const ImageGpu& from)
+  : ImageGpu(from.size())
+{
+  this->copyFrom(from);
+}
+
+//-----------------------------------------------------------------------------
+template<typename Pixel>
+ImageGpu<Pixel>::ImageGpu(const Image<Pixel>& from)
+  : ImageGpu(from.size())
+{
+  this->copyFrom(from);
+}
+
+////-----------------------------------------------------------------------------
+//template<typename Pixel>
+//ImageGpu<Pixel>
+//::ImageGpu(Pixel* data, uint32_t width, uint32_t height,
+//           uint32_t pitch, bool use_ext_data_pointer)
+//  : Base(width, height)
+//{
+//  CHECK_NOT_NULL(data);
+//
+//  if(use_ext_data_pointer)
+//  {
+//    // This uses the external data pointer as internal data pointer.
+//    auto dealloc_nop = [](Pixel*) { ; };
+//    data_ = std::unique_ptr<Pixel, Deallocator>(
+//          data, Deallocator(dealloc_nop));
+//    this->pitch_ = pitch;
+//  }
+//  else
+//  {
+//    data_.reset(Memory::alignedAlloc(this->width(), this->height(), &this->pitch_));
+//    size_t stride = pitch / sizeof(Pixel);
+
+//    if (this->bytes() == pitch*height)
+//    {
+//      std::copy(data, data+stride*height, data_.get());
+//    }
+//    else
+//    {
+//      for (uint32_t y=0; y<height; ++y)
+//      {
+//        for (uint32_t x=0; x<width; ++x)
+//        {
+//          data_.get()[y*this->stride()+x] = data[y*stride + x];
+//        }
+//      }
+//    }
+//  }
+//}
+
+//-----------------------------------------------------------------------------
+template<typename Pixel>
+ImageGpu<Pixel>::~ImageGpu()
+{
+}
+
+//-----------------------------------------------------------------------------
+template<typename Pixel>
+void ImageGpu<Pixel>::copyTo(ze::Image<Pixel>& dst) const
+{
+  CHECK_EQ(this->size(), dst.size());
+  cudaMemcpyKind memcpy_kind = dst.isGpuMemory() ? cudaMemcpyDeviceToDevice :
+                                                   cudaMemcpyDeviceToHost;
+  const cudaError cu_err = cudaMemcpy2D(dst.data(), dst.pitch(),
+                                        this->data(), this->pitch(),
+                                        this->rowBytes(),
+                                        this->height(), memcpy_kind);
+  CHECK_EQ(cu_err, ::cudaSuccess);
+}
+
+//-----------------------------------------------------------------------------
+template<typename Pixel>
+void ImageGpu<Pixel>::copyFrom(const Image<Pixel>& from)
+{
+  CHECK_EQ(this->size(), from.size());
+  cudaMemcpyKind memcpy_kind = from.isGpuMemory() ? cudaMemcpyDeviceToDevice :
+                                                    cudaMemcpyHostToDevice;
+  const cudaError cu_err = cudaMemcpy2D(this->data(), this->pitch(),
+                                        from.data(), from.pitch(),
+                                        this->rowBytes(),
+                                        this->height(), memcpy_kind);
+  CHECK_EQ(cu_err, ::cudaSuccess);
+}
+
+//-----------------------------------------------------------------------------
+template<typename Pixel>
+Pixel* ImageGpu<Pixel>::data(uint32_t ox, uint32_t oy)
+{
+  CHECK_EQ(0u, ox) << "getting datapointer with offset not allowed for GPU memory";
+  CHECK_EQ(0u, oy) << "getting datapointer with offset not allowed for GPU memory";
+  return data_.get();
+}
+
+//-----------------------------------------------------------------------------
+template<typename Pixel>
+const Pixel* ImageGpu<Pixel>::data(uint32_t ox, uint32_t oy) const
+{
+  CHECK_EQ(ox, 0u) << "Device memory offset access not possible.";
+  CHECK_EQ(oy, 0u) << "Device memory offset access not possible.";
+  return data_.get();
+}
+
+//-----------------------------------------------------------------------------
+template<typename Pixel>
+auto ImageGpu<Pixel>::cuData() -> decltype(ze::cu::toCudaVectorType(this->data()))
+{
+  return ze::cu::toCudaVectorType(this->data());
+}
+
+//-----------------------------------------------------------------------------
+template<typename Pixel>
+auto ImageGpu<Pixel>::cuData() const -> decltype(ze::cu::toConstCudaVectorType(this->data()))
+{
+  return ze::cu::toConstCudaVectorType(this->data());
+}
+
+//-----------------------------------------------------------------------------
+template<typename Pixel>
+void ImageGpu<Pixel>::setValue(const Pixel& value)
+{
+  if (sizeof(Pixel) == 1)
+  {
+    cudaMemset2D((void*)this->data(), this->pitch(), (int)value.c[0], this->rowBytes(), this->height());
+  }
+  else
+  {
+    // fragmentation
+    cu::Fragmentation<> frag(this->size());
+
+    // todo add roi to kernel!
+    ze::cu::k_setValue
+        <<< frag.dimGrid, frag.dimBlock >>> (this->data(), this->stride(), value,
+                                             this->width(), this->height());
+  }
+}
+
+//-----------------------------------------------------------------------------
+template<typename Pixel>
+std::shared_ptr<Texture2D> ImageGpu<Pixel>::genTexture(
+    bool normalized_coords,
+    cudaTextureFilterMode filter_mode,
+    cudaTextureAddressMode address_mode,
+    cudaTextureReadMode read_mode) const
+{
+  // don't blame me for doing a const_cast as binding textures needs a void* but
+  // we want genTexture to be a const function as we don't modify anything here!
+  return std::make_shared<Texture2D>(this->cuData(), this->pitch(), channel_format_desc_, this->size(),
+                                     normalized_coords, filter_mode, address_mode, read_mode);
+}
+
+//-----------------------------------------------------------------------------
+template<typename Pixel>
+//template<typename T>
+ImageGpu<Pixel>& ImageGpu<Pixel>::operator*=(const Pixel& rhs)
+{
+  // fragmentation
+  cu::Fragmentation<> frag(this->size());
+
+  // todo add roi to kernel!
+  ze::cu::k_pixelWiseMul
+      <<< frag.dimGrid, frag.dimBlock >>> (this->data(), this->stride(), rhs,
+                                           this->width(), this->height());
+  return *this;
+
+}
+
+//=============================================================================
+// Explicitely instantiate the desired classes
+// (sync with typedefs at the end of the hpp file)
+template class ImageGpu<ze::Pixel8uC1>;
+template class ImageGpu<ze::Pixel8uC2>;
+// be careful with 8uC3 images as pitch values are not divisable by 3!
+template class ImageGpu<ze::Pixel8uC3>;
+template class ImageGpu<ze::Pixel8uC4>;
+
+template class ImageGpu<ze::Pixel16uC1>;
+template class ImageGpu<ze::Pixel16uC2>;
+template class ImageGpu<ze::Pixel16uC3>;
+template class ImageGpu<ze::Pixel16uC4>;
+
+template class ImageGpu<ze::Pixel32sC1>;
+template class ImageGpu<ze::Pixel32sC2>;
+template class ImageGpu<ze::Pixel32sC3>;
+template class ImageGpu<ze::Pixel32sC4>;
+
+template class ImageGpu<ze::Pixel32fC1>;
+template class ImageGpu<ze::Pixel32fC2>;
+template class ImageGpu<ze::Pixel32fC3>;
+template class ImageGpu<ze::Pixel32fC4>;
+
+} // namespace cu
+} // namespace ze
diff --git a/RWR/src/ze_oss/imp_cu_core/src/cu_image_reduction.cu b/RWR/src/ze_oss/imp_cu_core/src/cu_image_reduction.cu
new file mode 100644
index 0000000000000000000000000000000000000000..d09f145fa62d19b856b438fc0444d342005c0f44
--- /dev/null
+++ b/RWR/src/ze_oss/imp_cu_core/src/cu_image_reduction.cu
@@ -0,0 +1,265 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#include <imp/cu_core/cu_image_reduction.cuh>
+
+namespace ze {
+namespace cu {
+
+// From Reduction SDK sample:
+// Prevent instantiation of the generic struct using an undefined symbol
+// in the function body (so it won't compile)
+template<typename Pixel>
+struct SharedMemory
+{
+  __device__ Pixel *getPointer()
+  {
+    extern __device__ void error(void);
+    error();
+    return NULL;
+  }
+};
+
+// Required specializations
+template<>
+struct SharedMemory<Pixel32sC1>
+{
+  __device__ Pixel32sC1 *getPointer()
+  {
+    extern __shared__ Pixel32sC1 s_int[];
+    return s_int;
+  }
+};
+
+template<>
+struct SharedMemory<Pixel32fC1>
+{
+  __device__ Pixel32fC1 *getPointer()
+  {
+    extern __shared__ Pixel32fC1 s_float[];
+    return s_float;
+  }
+};
+
+// Templated kernels
+template<typename Pixel>
+__global__
+void reductionSumKernel(
+    Pixel* out_dev_ptr,
+    size_t out_stride,
+    const Pixel* in_dev_ptr,
+    size_t in_stride,
+    uint32_t width,
+    uint32_t height)
+{
+  SharedMemory<Pixel> smem;
+  Pixel* s_partial = smem.getPointer();
+
+  Pixel sum = 0;
+
+  // Sum over 2D thread grid, use (x,y) indices
+  for (int x = blockIdx.x * blockDim.x + threadIdx.x;
+       x < width;
+       x += blockDim.x * gridDim.x)
+  {
+    for (int y = blockIdx.y * blockDim.y + threadIdx.y;
+         y < height;
+         y += blockDim.y * gridDim.y)
+    {
+      sum += in_dev_ptr[y*in_stride+x];
+    }
+  }
+  // Sums are written to shared memory, single index
+  s_partial[threadIdx.y*blockDim.x+threadIdx.x] = sum;
+  __syncthreads();
+
+  // Reduce over block sums stored in shared memory
+  // Start using half the block threads,
+  // halve the active threads at each iteration
+  const int tid = threadIdx.y*blockDim.x+threadIdx.x;
+  for (int num_active_threads = (blockDim.x*blockDim.y) >> 1;
+       num_active_threads;
+       num_active_threads >>= 1)
+  {
+    if (tid < num_active_threads)
+    {
+      s_partial[tid] += s_partial[tid+num_active_threads];
+    }
+    __syncthreads();
+  }
+  // Thread 0 writes the result for the block
+  if (0 == tid)
+  {
+    out_dev_ptr[blockIdx.y*out_stride+blockIdx.x] = s_partial[0];
+  }
+}
+
+
+template<typename Pixel>
+__global__
+void reductionCountEqKernel(
+    Pixel* out_dev_ptr,
+    size_t out_stride,
+    const Pixel* in_dev_ptr,
+    size_t in_stride,
+    uint32_t width,
+    uint32_t height,
+    Pixel value)
+{
+  SharedMemory<Pixel> smem;
+  Pixel* s_partial = smem.getPointer();
+
+  int32_t count = 0;
+
+  // Sum over 2D thread grid, use (x,y) indices
+  for (int x = blockIdx.x * blockDim.x + threadIdx.x;
+       x < width;
+       x += blockDim.x * gridDim.x)
+  {
+    for (int y = blockIdx.y * blockDim.y + threadIdx.y;
+         y < height;
+         y += blockDim.y * gridDim.y)
+    {
+      if(static_cast<int32_t>(value) == in_dev_ptr[y*in_stride+x])
+      {
+        count += 1;
+      }
+    }
+  }
+  // Sums are written to shared memory, single index
+  s_partial[threadIdx.y*blockDim.x+threadIdx.x] = count;
+  __syncthreads();
+
+  // Reduce over block sums stored in shared memory
+  // Start using half the block threads,
+  // halve the active threads at each iteration
+  const int tid = threadIdx.y*blockDim.x+threadIdx.x;
+  for (int num_active_threads = (blockDim.x*blockDim.y) >> 1;
+       num_active_threads;
+       num_active_threads >>= 1 )
+  {
+    if (tid < num_active_threads)
+    {
+      s_partial[tid] += s_partial[tid+num_active_threads];
+    }
+    __syncthreads();
+  }
+  // Thread 0 writes the result for the block
+  if (0 == tid)
+  {
+    out_dev_ptr[blockIdx.y*out_stride+blockIdx.x] = s_partial[0];
+  }
+}
+
+
+template<typename Pixel>
+ImageReducer<Pixel>::ImageReducer()
+  : partial_(fragm_.dimGrid.x, fragm_.dimGrid.y)
+{
+  // Compute required amount of shared memory
+  sh_mem_size_ = fragm_.dimBlock.x * fragm_.dimBlock.y * sizeof(Pixel);
+}
+
+template<typename Pixel>
+ImageReducer<Pixel>::~ImageReducer()
+{ }
+
+// Sum image by reduction
+// Cfr. listing 12.1 by N. Wilt, "The CUDA Handbook"
+template<typename Pixel>
+Pixel ImageReducer<Pixel>::sum(const ImageGpu<Pixel>& in_img)
+{
+  //if(is_dev_fin_alloc_ && is_dev_part_alloc_)
+
+  reductionSumKernel<Pixel>
+      <<<
+        fragm_.dimGrid, fragm_.dimBlock, sh_mem_size_
+      >>>
+        (partial_.data(),
+         partial_.stride(),
+         in_img.data(),
+         in_img.stride(),
+         in_img.width(),
+         in_img.height());
+
+  reductionSumKernel<Pixel>
+      <<<
+        1, fragm_.dimBlock, sh_mem_size_
+      >>>
+        (dev_final_.data(),
+         0,
+         partial_.data(),
+         partial_.stride(),
+         fragm_.dimGrid.x,
+         fragm_.dimGrid.y);
+
+  // download sum
+  ze::LinearMemory<Pixel> h_sum(1);
+  dev_final_.copyTo(h_sum);
+  return h_sum(0);
+}
+
+// Count elements equal to 'value'
+// First count over the thread grid,
+// then perform a reduction sum on a single thread block
+template<>
+size_t ImageReducer<Pixel32sC1>::countEqual(
+    const ImageGpu32sC1& in_img,
+    int32_t value)
+{
+
+  reductionCountEqKernel<Pixel32sC1>
+      <<<
+        fragm_.dimGrid, fragm_.dimBlock, sh_mem_size_
+      >>>
+        (partial_.data(),
+         partial_.stride(),
+         in_img.data(),
+         in_img.stride(),
+         in_img.width(),
+         in_img.height(),
+         value);
+
+  reductionSumKernel<Pixel32sC1>
+      <<<
+        1, fragm_.dimBlock, sh_mem_size_
+      >>>
+        (dev_final_.data(),
+         0,
+         partial_.data(),
+         partial_.stride(),
+         fragm_.dimGrid.x,
+         fragm_.dimGrid.y);
+
+  // download count
+  ze::LinearMemory32sC1 h_count{1};
+  dev_final_.copyTo(h_count);
+  return static_cast<size_t>(h_count(0));
+}
+
+template class ImageReducer<Pixel32sC1>;
+template class ImageReducer<Pixel32fC1>;
+
+} // cu namespace
+} // ze namespace
diff --git a/RWR/src/ze_oss/imp_cu_core/src/cu_linearmemory.cu b/RWR/src/ze_oss/imp_cu_core/src/cu_linearmemory.cu
new file mode 100644
index 0000000000000000000000000000000000000000..33b4c68c37a8e0eb8367cdce0e239ba609630ab8
--- /dev/null
+++ b/RWR/src/ze_oss/imp_cu_core/src/cu_linearmemory.cu
@@ -0,0 +1,190 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#include <imp/cu_core/cu_linearmemory.cuh>
+#include <ze/common/logging.hpp>
+#include <imp/cu_core/cu_utils.hpp>
+#include <imp/cu_core/cu_k_setvalue.cuh>
+
+
+namespace ze {
+namespace cu {
+
+//-----------------------------------------------------------------------------
+template<typename Pixel>
+LinearMemory<Pixel>::LinearMemory(const uint32_t& length)
+  : LinearMemoryBase(length)
+  , data_(Memory::alloc(this->length()))
+{
+}
+
+//-----------------------------------------------------------------------------
+template<typename Pixel>
+LinearMemory<Pixel>::LinearMemory(const ze::cu::LinearMemory<Pixel>& from)
+  : ze::cu::LinearMemory<Pixel>(from.length())
+{
+  CHECK_NOTNULL(from.data());
+  this->copyFrom(from);
+}
+
+//-----------------------------------------------------------------------------
+template<typename Pixel>
+LinearMemory<Pixel>::LinearMemory(const ze::LinearMemory<Pixel>& from)
+  : ze::cu::LinearMemory<Pixel>(from.length())
+{
+  CHECK_NOTNULL(from.data());
+  this->copyFrom(from);
+}
+
+//-----------------------------------------------------------------------------
+template<typename Pixel>
+Pixel* LinearMemory<Pixel>::data()
+{
+  return data_.get();
+}
+
+//-----------------------------------------------------------------------------
+template<typename Pixel>
+const Pixel* LinearMemory<Pixel>::data() const
+{
+  return reinterpret_cast<const Pixel*>(data_.get());
+}
+
+//-----------------------------------------------------------------------------
+template<typename Pixel>
+auto LinearMemory<Pixel>::cuData() -> decltype(ze::cu::toCudaVectorType(this->data()))
+{
+  return ze::cu::toCudaVectorType(this->data());
+}
+
+//-----------------------------------------------------------------------------
+template<typename Pixel>
+auto LinearMemory<Pixel>::cuData() const -> decltype(ze::cu::toConstCudaVectorType(this->data()))
+{
+  return ze::cu::toConstCudaVectorType(this->data());
+}
+
+//-----------------------------------------------------------------------------
+template<typename Pixel>
+void LinearMemory<Pixel>::setValue(const Pixel& value)
+{
+  if (sizeof(Pixel) == 1)
+  {
+    cudaMemset((void*)(this->data()+this->roi().x()), (int)value.c[0], this->roiBytes());
+  }
+  else
+  {
+    // fragmentation
+    cu::Fragmentation<32,1> frag(this->roi().length());
+
+    // todo add roi to kernel!
+    ze::cu::k_setValue
+        <<< frag.dimGrid, frag.dimBlock
+        >>> (this->data(), this->roi().x(), this->roi().length(), value);
+  }
+  IMP_CUDA_CHECK();
+}
+
+//-----------------------------------------------------------------------------
+template<typename Pixel>
+void LinearMemory<Pixel>::copyTo(ze::cu::LinearMemory<Pixel>& dst)
+{
+  CHECK_NOTNULL(dst.data());
+  CHECK(data_);
+  CHECK_EQ(this->roiBytes(), dst.roiBytes());
+  const cudaError cu_err =
+      cudaMemcpy(dst.data()+dst.roi().x(), this->data()+this->roi().x(),
+                 this->roiBytes(), cudaMemcpyDeviceToDevice);
+  CHECK_EQ(cu_err, ::cudaSuccess);
+}
+
+//-----------------------------------------------------------------------------
+template<typename Pixel>
+void LinearMemory<Pixel>::copyFrom(const ze::cu::LinearMemory<Pixel>& from)
+{
+  CHECK_NOTNULL(from.data());
+  CHECK(data_);
+  CHECK_EQ(this->roiBytes(), from.roiBytes());
+  const cudaError cu_err =
+      cudaMemcpy(this->data()+this->roi().x(), from.data()+from.roi().x(),
+                 from.roiBytes(), cudaMemcpyDeviceToDevice);
+  CHECK_EQ(cu_err, ::cudaSuccess);
+}
+
+//-----------------------------------------------------------------------------
+template<typename Pixel>
+void LinearMemory<Pixel>::copyTo(ze::LinearMemory<Pixel>& dst)
+{
+  CHECK_NOTNULL(dst.data());
+  CHECK(data_);
+  CHECK_EQ(this->roiBytes(), dst.roiBytes());
+  const cudaError cu_err =
+      cudaMemcpy(dst.data()+dst.roi().x(), this->data()+this->roi().x(),
+                 this->roiBytes(), cudaMemcpyDeviceToHost);
+  CHECK_EQ(cu_err, ::cudaSuccess);
+}
+
+//-----------------------------------------------------------------------------
+template<typename Pixel>
+void LinearMemory<Pixel>::copyFrom(const ze::LinearMemory<Pixel>& from)
+{
+  CHECK_NOTNULL(from.data());
+  CHECK(data_);
+  CHECK_EQ(this->roiBytes(), from.roiBytes());
+  const cudaError cu_err =
+      cudaMemcpy(this->data()+this->roi().x(), from.data()+from.roi().x(),
+                 from.roiBytes(), cudaMemcpyHostToDevice);
+  CHECK_EQ(cu_err, ::cudaSuccess);
+}
+
+
+//=============================================================================
+// Explicitely instantiate the desired classes
+template class LinearMemory<ze::Pixel8uC1>;
+template class LinearMemory<ze::Pixel8uC2>;
+template class LinearMemory<ze::Pixel8uC3>;
+template class LinearMemory<ze::Pixel8uC4>;
+
+template class LinearMemory<ze::Pixel16uC1>;
+template class LinearMemory<ze::Pixel16uC2>;
+template class LinearMemory<ze::Pixel16uC3>;
+template class LinearMemory<ze::Pixel16uC4>;
+
+template class LinearMemory<ze::Pixel32uC1>;
+template class LinearMemory<ze::Pixel32uC2>;
+template class LinearMemory<ze::Pixel32uC3>;
+template class LinearMemory<ze::Pixel32uC4>;
+
+template class LinearMemory<ze::Pixel32sC1>;
+template class LinearMemory<ze::Pixel32sC2>;
+template class LinearMemory<ze::Pixel32sC3>;
+template class LinearMemory<ze::Pixel32sC4>;
+
+template class LinearMemory<ze::Pixel32fC1>;
+template class LinearMemory<ze::Pixel32fC2>;
+template class LinearMemory<ze::Pixel32fC3>;
+template class LinearMemory<ze::Pixel32fC4>;
+
+} // namespace cu
+} // namespace ze
diff --git a/RWR/src/ze_oss/imp_cu_core/src/cu_pixel_conversion.cpp b/RWR/src/ze_oss/imp_cu_core/src/cu_pixel_conversion.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6ecd4b7e3f975f0717113f19d634e0868d1a1bad
--- /dev/null
+++ b/RWR/src/ze_oss/imp_cu_core/src/cu_pixel_conversion.cpp
@@ -0,0 +1,289 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#include <imp/cu_core/cu_pixel_conversion.hpp>
+#include <cuda_runtime.h>
+#include <ze/common/logging.hpp>
+#include <imp/core/pixel.hpp>
+#include <imp/core/pixel_enums.hpp>
+
+namespace ze {
+namespace cu {
+
+/*
+ * Pixel arrays to cuda vector types
+ */
+
+//
+// uchar
+//
+unsigned char* __host__ __device__ toCudaVectorType(ze::Pixel8uC1* buffer)
+{
+  return reinterpret_cast<unsigned char*>(buffer);
+}
+uchar2* __host__ __device__ toCudaVectorType(ze::Pixel8uC2* buffer)
+{
+  return reinterpret_cast<uchar2*>(buffer);
+}
+uchar3* __host__ __device__ toCudaVectorType(ze::Pixel8uC3* buffer)
+{
+  return reinterpret_cast<uchar3*>(buffer);
+}
+uchar4* __host__ __device__ toCudaVectorType(ze::Pixel8uC4* buffer)
+{
+  return reinterpret_cast<uchar4*>(buffer);
+}
+
+//
+// ushort
+//
+unsigned short* __host__ __device__ toCudaVectorType(ze::Pixel16uC1* buffer)
+{
+  return reinterpret_cast<unsigned short*>(buffer);
+}
+ushort2* __host__ __device__ toCudaVectorType(ze::Pixel16uC2* buffer)
+{
+  return reinterpret_cast<ushort2*>(buffer);
+}
+ushort3* __host__ __device__ toCudaVectorType(ze::Pixel16uC3* buffer)
+{
+  return reinterpret_cast<ushort3*>(buffer);
+}
+ushort4* __host__ __device__ toCudaVectorType(ze::Pixel16uC4* buffer)
+{
+  return reinterpret_cast<ushort4*>(buffer);
+}
+
+//
+// unsigned int
+//
+unsigned int* __host__ __device__ toCudaVectorType(ze::Pixel32uC1* buffer)
+{
+  return reinterpret_cast<unsigned int*>(buffer);
+}
+uint2* __host__ __device__ toCudaVectorType(ze::Pixel32uC2* buffer)
+{
+  return reinterpret_cast<uint2*>(buffer);
+}
+uint3* __host__ __device__ toCudaVectorType(ze::Pixel32uC3* buffer)
+{
+  return reinterpret_cast<uint3*>(buffer);
+}
+uint4* __host__ __device__ toCudaVectorType(ze::Pixel32uC4* buffer)
+{
+  return reinterpret_cast<uint4*>(buffer);
+}
+
+//
+// int
+//
+int* __host__ __device__ toCudaVectorType(ze::Pixel32sC1* buffer)
+{
+  return reinterpret_cast<int*>(buffer);
+}
+int2* __host__ __device__ toCudaVectorType(ze::Pixel32sC2* buffer)
+{
+  return reinterpret_cast<int2*>(buffer);
+}
+int3* __host__ __device__ toCudaVectorType(ze::Pixel32sC3* buffer)
+{
+  return reinterpret_cast<int3*>(buffer);
+}
+int4* __host__ __device__ toCudaVectorType(ze::Pixel32sC4* buffer)
+{
+  return reinterpret_cast<int4*>(buffer);
+}
+
+//
+// float
+//
+float* __host__ __device__ toCudaVectorType(ze::Pixel32fC1* buffer)
+{
+  return reinterpret_cast<float*>(buffer);
+}
+float2* __host__ __device__ toCudaVectorType(ze::Pixel32fC2* buffer)
+{
+  return reinterpret_cast<float2*>(buffer);
+}
+float3* __host__ __device__ toCudaVectorType(ze::Pixel32fC3* buffer)
+{
+  return reinterpret_cast<float3*>(buffer);
+}
+float4* __host__ __device__ toCudaVectorType(ze::Pixel32fC4* buffer)
+{
+  return reinterpret_cast<float4*>(buffer);
+}
+
+/*
+ * Pixel arrays to CONST cuda vector types
+ */
+
+//
+// uchar
+//
+const unsigned char* __host__ __device__ toConstCudaVectorType(const ze::Pixel8uC1* buffer)
+{
+  return reinterpret_cast<const unsigned char*>(buffer);
+}
+const uchar2* __host__ __device__ toConstCudaVectorType(const ze::Pixel8uC2* buffer)
+{
+  return reinterpret_cast<const uchar2*>(buffer);
+}
+const uchar3* __host__ __device__ toConstCudaVectorType(const ze::Pixel8uC3* buffer)
+{
+  return reinterpret_cast<const uchar3*>(buffer);
+}
+const uchar4* __host__ __device__ toConstCudaVectorType(const ze::Pixel8uC4* buffer)
+{
+  return reinterpret_cast<const uchar4*>(buffer);
+}
+
+//
+// ushort
+//
+const unsigned short* __host__ __device__ toConstCudaVectorType(const ze::Pixel16uC1* buffer)
+{
+  return reinterpret_cast<const unsigned short*>(buffer);
+}
+const ushort2* __host__ __device__ toConstCudaVectorType(const ze::Pixel16uC2* buffer)
+{
+  return reinterpret_cast<const ushort2*>(buffer);
+}
+const ushort3* __host__ __device__ toConstCudaVectorType(const ze::Pixel16uC3* buffer)
+{
+  return reinterpret_cast<const ushort3*>(buffer);
+}
+const ushort4* __host__ __device__ toConstCudaVectorType(const ze::Pixel16uC4* buffer)
+{
+  return reinterpret_cast<const ushort4*>(buffer);
+}
+
+//
+// unsigned int
+//
+const unsigned int* __host__ __device__ toConstCudaVectorType(const ze::Pixel32uC1* buffer)
+{
+  return reinterpret_cast<const unsigned int*>(buffer);
+}
+const uint2* __host__ __device__ toConstCudaVectorType(const ze::Pixel32uC2* buffer)
+{
+  return reinterpret_cast<const uint2*>(buffer);
+}
+const uint3* __host__ __device__ toConstCudaVectorType(const ze::Pixel32uC3* buffer)
+{
+  return reinterpret_cast<const uint3*>(buffer);
+}
+const uint4* __host__ __device__ toConstCudaVectorType(const ze::Pixel32uC4* buffer)
+{
+  return reinterpret_cast<const uint4*>(buffer);
+}
+
+//
+// int
+//
+const int* __host__ __device__ toConstCudaVectorType(const ze::Pixel32sC1* buffer)
+{
+  return reinterpret_cast<const int*>(buffer);
+}
+const int2* __host__ __device__ toConstCudaVectorType(const ze::Pixel32sC2* buffer)
+{
+  return reinterpret_cast<const int2*>(buffer);
+}
+const int3* __host__ __device__ toConstCudaVectorType(const ze::Pixel32sC3* buffer)
+{
+  return reinterpret_cast<const int3*>(buffer);
+}
+const int4* __host__ __device__ toConstCudaVectorType(const ze::Pixel32sC4* buffer)
+{
+  return reinterpret_cast<const int4*>(buffer);
+}
+
+//
+// float
+//
+const float* __host__ __device__ toConstCudaVectorType(const ze::Pixel32fC1* buffer)
+{
+  return reinterpret_cast<const float*>(buffer);
+}
+const float2* __host__ __device__ toConstCudaVectorType(const ze::Pixel32fC2* buffer)
+{
+  return reinterpret_cast<const float2*>(buffer);
+}
+const float3* __host__ __device__ toConstCudaVectorType(const ze::Pixel32fC3* buffer)
+{
+  return reinterpret_cast<const float3*>(buffer);
+}
+const float4* __host__ __device__ toConstCudaVectorType(const ze::Pixel32fC4* buffer)
+{
+  return reinterpret_cast<const float4*>(buffer);
+}
+
+
+//-----------------------------------------------------------------------------
+cudaChannelFormatDesc toCudaChannelFormatDesc(ze::PixelType pixel_type)
+{
+  switch (pixel_type)
+  {
+  case ze::PixelType::i8uC1:
+    return cudaCreateChannelDesc<unsigned char>();
+  case ze::PixelType::i8uC2:
+    return cudaCreateChannelDesc<uchar2>();
+  case ze::PixelType::i8uC3:
+    return cudaCreateChannelDesc<uchar3>();
+  case ze::PixelType::i8uC4:
+    return cudaCreateChannelDesc<uchar4>();
+  case ze::PixelType::i16uC1:
+    return cudaCreateChannelDesc<unsigned short>();
+  case ze::PixelType::i16uC2:
+    return cudaCreateChannelDesc<ushort2>();
+  case ze::PixelType::i16uC3:
+    return cudaCreateChannelDesc<ushort3>();
+  case ze::PixelType::i16uC4:
+    return cudaCreateChannelDesc<ushort4>();
+  case ze::PixelType::i32sC1:
+    return cudaCreateChannelDesc<int>();
+  case ze::PixelType::i32sC2:
+    return cudaCreateChannelDesc<int2>();
+  case ze::PixelType::i32sC3:
+    return cudaCreateChannelDesc<int3>();
+  case ze::PixelType::i32sC4:
+    return cudaCreateChannelDesc<int4>();
+  case ze::PixelType::i32fC1:
+    return cudaCreateChannelDesc<float>();
+  case ze::PixelType::i32fC2:
+    return cudaCreateChannelDesc<float2>();
+  case ze::PixelType::i32fC3:
+    return cudaCreateChannelDesc<float3>();
+  case ze::PixelType::i32fC4:
+    return cudaCreateChannelDesc<float4>();
+  default:
+    CHECK(false) << "Pixel type not supported to generate a CUDA texture.";
+  }
+}
+
+} // namespace cu
+} // namespace ze
+
+
+
diff --git a/RWR/src/ze_oss/imp_cu_core/src/min_max.cu b/RWR/src/ze_oss/imp_cu_core/src/min_max.cu
new file mode 100644
index 0000000000000000000000000000000000000000..4901aeb0fc2a7fcf8eb6c9a19e602c99d1548ef9
--- /dev/null
+++ b/RWR/src/ze_oss/imp_cu_core/src/min_max.cu
@@ -0,0 +1,168 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#include <imp/core/linearmemory.hpp>
+#include <imp/core/image.hpp>
+#include <imp/cu_core/cu_math.cuh>
+#include <imp/cu_core/cu_image_gpu.cuh>
+#include <imp/cu_core/cu_texture.cuh>
+#include <imp/cu_core/cu_utils.hpp>
+#include <imp/cu_core/cu_linearmemory.cuh>
+
+namespace ze {
+namespace cu {
+
+//-----------------------------------------------------------------------------
+template<typename Pixel>
+__global__ void k_minMax(Pixel* d_col_mins, Pixel* d_col_maxs,
+                         uint32_t roi_x, uint32_t roi_y,
+                         uint32_t roi_width, uint32_t roi_height,
+                         Texture2D img_tex)
+{
+  int x = blockIdx.x*blockDim.x + threadIdx.x;
+
+  if (x<roi_width)
+  {
+    float xx = x + roi_x;
+    float yy = roi_y;
+
+    Pixel cur_min, cur_max;
+    tex2DFetch(cur_min, img_tex, xx, yy++);
+    cur_max = cur_min;
+
+    Pixel val;
+    for (; yy<roi_y+roi_height; ++yy)
+    {
+      tex2DFetch(val, img_tex, xx, yy);
+      if (val<cur_min) cur_min = val;
+      if (val>cur_max) cur_max = val;
+    }
+
+    d_col_mins[x] = cur_min;
+    d_col_maxs[x] = cur_max;
+  }
+}
+
+//-----------------------------------------------------------------------------
+template<typename Pixel, typename SrcPixel>
+__global__ void k_minMax(Pixel* d_col_mins, Pixel* d_col_maxs,
+                         SrcPixel* src, size_t src_stride,
+                         uint32_t roi_x, uint32_t roi_y,
+                         uint32_t roi_width, uint32_t roi_height)
+{
+  int x = blockIdx.x*blockDim.x + threadIdx.x;
+
+  if (x<roi_width)
+  {
+    int xx = x+roi_x;
+    int yy = roi_y;
+
+
+    Pixel cur_min, cur_max;
+    Pixel val = (Pixel)src[yy++*src_stride+xx];
+    cur_min = val;
+    cur_max = val;
+    for (; yy<roi_y+roi_height; ++yy)
+    {
+      val = (Pixel)src[yy*src_stride+xx];
+      cur_min = ze::cu::min(cur_min, val);
+      cur_max = ze::cu::max(cur_max, val);
+    }
+
+    d_col_mins[x] = cur_min;
+    d_col_maxs[x] = cur_max;
+  }
+}
+
+//-----------------------------------------------------------------------------
+template<typename Pixel>
+void minMax(const Texture2D& img_tex, Pixel& min_val, Pixel& max_val, const ze::Roi2u& roi)
+{
+  Fragmentation<512,1> frag(roi.width(), 1);
+
+  ze::cu::LinearMemory<Pixel> d_col_mins(roi.width());
+  ze::cu::LinearMemory<Pixel> d_col_maxs(roi.width());
+  IMP_CUDA_CHECK();
+  d_col_mins.setValue(Pixel(0));
+  d_col_maxs.setValue(Pixel(0));
+
+  k_minMax
+      <<<
+        frag.dimGrid, frag.dimBlock
+      >>> (d_col_mins.data(), d_col_maxs.data(),
+           roi.x(), roi.y(), roi.width(), roi.height(), img_tex);
+  IMP_CUDA_CHECK();
+
+  ze::LinearMemory<Pixel> h_col_mins(d_col_mins.length());
+  ze::LinearMemory<Pixel> h_col_maxs(d_col_maxs.length());
+  h_col_mins.setValue(Pixel(0));
+  h_col_maxs.setValue(Pixel(0));
+
+  d_col_mins.copyTo(h_col_mins);
+  d_col_maxs.copyTo(h_col_maxs);
+
+  min_val = h_col_mins(0);
+  max_val = h_col_maxs(0);
+
+  for (auto i=1u; i<roi.width(); ++i)
+  {
+    min_val = ze::cu::min(min_val, h_col_mins(i));
+    max_val = ze::cu::max(max_val, h_col_maxs(i));
+  }
+
+  IMP_CUDA_CHECK();
+}
+
+//-----------------------------------------------------------------------------
+template<typename Pixel>
+void minMax(const ImageGpu<Pixel>& img, Pixel& min_val, Pixel& max_val)
+{
+  ze::cu::minMax(*img.genTexture(), min_val, max_val, img.roi());
+  IMP_CUDA_CHECK();
+}
+
+
+// template instantiations for all our image types
+template void minMax(const ImageGpu8uC1& img, ze::Pixel8uC1& min, ze::Pixel8uC1& max);
+template void minMax(const ImageGpu8uC2& img, ze::Pixel8uC2& min, ze::Pixel8uC2& max);
+//template void minMax(const ImageGpu8uC3& img, ze::Pixel8uC3& min, ze::Pixel8uC3& max);
+template void minMax(const ImageGpu8uC4& img, ze::Pixel8uC4& min, ze::Pixel8uC4& max);
+
+template void minMax(const ImageGpu16uC1& img, ze::Pixel16uC1& min, ze::Pixel16uC1& max);
+template void minMax(const ImageGpu16uC2& img, ze::Pixel16uC2& min, ze::Pixel16uC2& max);
+//template void minMax(const ImageGpu16uC3& img, ze::Pixel16uC3& min, ze::Pixel16uC3& max);
+template void minMax(const ImageGpu16uC4& img, ze::Pixel16uC4& min, ze::Pixel16uC4& max);
+
+template void minMax(const ImageGpu32sC1& img, ze::Pixel32sC1& min, ze::Pixel32sC1& max);
+template void minMax(const ImageGpu32sC2& img, ze::Pixel32sC2& min, ze::Pixel32sC2& max);
+//template void minMax(const ImageGpu32sC3& img, ze::Pixel32sC3& min, ze::Pixel32sC3& max);
+template void minMax(const ImageGpu32sC4& img, ze::Pixel32sC4& min, ze::Pixel32sC4& max);
+
+template void minMax(const ImageGpu32fC1& img, ze::Pixel32fC1& min, ze::Pixel32fC1& max);
+template void minMax(const ImageGpu32fC2& img, ze::Pixel32fC2& min, ze::Pixel32fC2& max);
+//template void minMax(const ImageGpu32fC3& img, ze::Pixel32fC3& min, ze::Pixel32fC3& max);
+template void minMax(const ImageGpu32fC4& img, ze::Pixel32fC4& min, ze::Pixel32fC4& max);
+
+} // namespace cu
+} // namespace ze
diff --git a/RWR/src/ze_oss/imp_cu_core/src/sum.cu b/RWR/src/ze_oss/imp_cu_core/src/sum.cu
new file mode 100644
index 0000000000000000000000000000000000000000..4682de19b17456cbff6c0c800cb2ce250da58e3a
--- /dev/null
+++ b/RWR/src/ze_oss/imp_cu_core/src/sum.cu
@@ -0,0 +1,123 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#include <imp/core/linearmemory.hpp>
+#include <imp/core/image.hpp>
+#include <imp/cu_core/cu_math.cuh>
+#include <imp/cu_core/cu_image_gpu.cuh>
+#include <imp/cu_core/cu_texture.cuh>
+#include <imp/cu_core/cu_utils.hpp>
+#include <imp/cu_core/cu_linearmemory.cuh>
+
+namespace ze {
+namespace cu {
+
+//-----------------------------------------------------------------------------
+template<typename Pixel>
+__global__
+void k_sumCol(
+    Pixel* d_col_sums,
+    std::uint32_t roi_x,
+    std::uint32_t roi_y,
+    std::uint32_t roi_width,
+    std::uint32_t roi_height,
+    Texture2D img_tex)
+{
+  int x = blockIdx.x*blockDim.x + threadIdx.x;
+  if (x<roi_width)
+  {
+    float xx = x + roi_x;
+
+    Pixel col_sum{0};
+    Pixel val;
+    for (float yy = roi_y; yy<roi_y+roi_height; ++yy)
+    {
+      tex2DFetch(val, img_tex, xx, yy);
+      col_sum += val;
+    }
+    d_col_sums[x] = col_sum;
+  }
+}
+
+//-----------------------------------------------------------------------------
+template<typename Pixel>
+Pixel sum(const Texture2D& img_tex, const ze::Roi2u& roi)
+{
+  Fragmentation<512,1,1> frag(roi.width(), 1);
+  Pixel sum_val;
+  ze::cu::LinearMemory<Pixel> d_col_sums(roi.width());
+  IMP_CUDA_CHECK();
+
+  k_sumCol
+      <<<
+        frag.dimGrid, frag.dimBlock
+      >>> (d_col_sums.data(), roi.x(), roi.y(), roi.width(), roi.height(), img_tex);
+  IMP_CUDA_CHECK();
+
+  ze::LinearMemory<Pixel> h_col_sums(roi.width());
+  d_col_sums.copyTo(h_col_sums);
+
+  sum_val = h_col_sums(0);
+  for (uint32_t i=1u; i<roi.width(); i++)
+  {
+    sum_val += h_col_sums(i);
+  }
+
+  IMP_CUDA_CHECK();
+  return sum_val;
+}
+
+//-----------------------------------------------------------------------------
+template<typename Pixel>
+Pixel sum(const ImageGpu<Pixel>& img)
+{
+  return ze::cu::sum<Pixel>(*img.genTexture(), img.roi());
+}
+
+// template instantiations for all our image types
+// TODO (MPI) only 32fC1 is currently supported
+
+/*
+template ze::Pixel8uC1 sum(const ImageGpu8uC1& img);
+template ze::Pixel8uC2 sum(const ImageGpu8uC2& img);
+template ze::Pixel8uC4 sum(const ImageGpu8uC4& img);
+
+template ze::Pixel16uC1 sum(const ImageGpu16uC1& im);
+template ze::Pixel16uC2 sum(const ImageGpu16uC2& im);
+template ze::Pixel16uC4 sum(const ImageGpu16uC4& im);
+
+template ze::Pixel32sC1 sum(const ImageGpu32sC1& im);
+template ze::Pixel32sC2 sum(const ImageGpu32sC2& im);
+template ze::Pixel32sC4 sum(const ImageGpu32sC4& im);
+*/
+
+template ze::Pixel32fC1 sum(const ImageGpu32fC1& im);
+
+/*
+template ze::Pixel32fC2 sum(const ImageGpu32fC2& im);
+template ze::Pixel32fC4 sum(const ImageGpu32fC4& im);
+*/
+
+} // namespace cu
+} // namespace ze
diff --git a/RWR/src/ze_oss/imp_cu_core/src/weighted_sum.cu b/RWR/src/ze_oss/imp_cu_core/src/weighted_sum.cu
new file mode 100644
index 0000000000000000000000000000000000000000..f4e369d6d586f63602844eded4e6ea5164eef775
--- /dev/null
+++ b/RWR/src/ze_oss/imp_cu_core/src/weighted_sum.cu
@@ -0,0 +1,143 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#include <type_traits>
+#include <imp/core/image.hpp>
+#include <imp/cu_core/cu_math.cuh>
+#include <imp/cu_core/cu_image_gpu.cuh>
+#include <imp/cu_core/cu_texture.cuh>
+#include <imp/cu_core/cu_utils.hpp>
+#include <imp/cu_core/cu_linearmemory.cuh>
+
+namespace ze {
+namespace cu {
+
+//-----------------------------------------------------------------------------
+template<typename Pixel>
+__global__ void k_weightedSum(Pixel* dst, uint32_t dst_stride,
+                              uint32_t dst_roi_x, uint32_t dst_roi_y,
+                              Texture2D src1, const float weight1,
+                              Texture2D src2, const float weight2,
+                              uint32_t src1_roi_x, uint32_t src1_roi_y,
+                              uint32_t src2_roi_x, uint32_t src2_roi_y,
+                              uint32_t roi_width, uint32_t roi_height)
+{
+  uint32_t x = blockIdx.x*blockDim.x + threadIdx.x;
+  uint32_t y = blockIdx.y*blockDim.y + threadIdx.y;
+
+  if (x<roi_width && y<roi_height)
+  {
+    Pixel src1_val, src2_val;
+    tex2DFetch(src1_val, src1, x, y, 1.f, 1.f, src1_roi_x, src1_roi_y);
+    tex2DFetch(src2_val, src2, x, y, 1.f, 1.f, src2_roi_x, src2_roi_y);
+    if (std::is_integral<typename Pixel::T>::value)
+    {
+      dst[(y+dst_roi_y)*dst_stride + (x+dst_roi_x)] =
+          static_cast<Pixel>(weight1 * src1_val + weight2 * src2_val + 0.5f);
+    }
+    else
+    {
+      dst[(y+dst_roi_y)*dst_stride + (x+dst_roi_x)] =
+          weight1 * src1_val + weight2 * src2_val;
+    }
+  }
+}
+
+//-----------------------------------------------------------------------------
+template<typename Pixel>
+void weightedSum(ImageGpu<Pixel>& dst,
+                 const Texture2D& src1_tex, const Roi2u src1_roi, const float& weight1,
+                 const Texture2D& src2_tex, const Roi2u src2_roi, const float& weight2)
+{
+  CHECK_EQ(src1_roi.size(), src2_roi.size());
+  CHECK_EQ(src1_roi.size(), dst.roi().size());
+
+  Roi2u dst_roi = dst.roi();
+  Fragmentation<> frag(dst_roi);
+
+  k_weightedSum
+      <<<
+        frag.dimGrid, frag.dimBlock
+      >>> (dst.data(), dst.stride(),
+           dst.roi().x(), dst.roi().y(),
+           src1_tex, weight1, src2_tex, weight2,
+           src1_roi.x(), src1_roi.y(), src2_roi.x(), src2_roi.y(),
+           dst_roi.width(), dst_roi.height());
+  IMP_CUDA_CHECK();
+}
+
+//-----------------------------------------------------------------------------
+template<typename Pixel>
+void weightedSum(ImageGpu<Pixel>& dst,
+                 const ImageGpu<Pixel>& src1, const float& weight1,
+                 const ImageGpu<Pixel>& src2, const float& weight2)
+{
+  std::shared_ptr<Texture2D> src1_tex = src1.genTexture(false, cudaFilterModePoint);
+  std::shared_ptr<Texture2D> src2_tex = src2.genTexture(false, cudaFilterModePoint);
+  weightedSum(dst,
+              *src1_tex, src1.roi(), weight1,
+              *src2_tex, src2.roi(), weight2);
+  IMP_CUDA_CHECK();
+}
+
+//-----------------------------------------------------------------------------
+template<typename Pixel>
+ImageGpuPtr<Pixel> weightedSum(const ImageGpu<Pixel>& src1, const float& weight1,
+                               const ImageGpu<Pixel>& src2, const float& weight2)
+{
+  ImageGpuPtr<Pixel> dst = std::make_shared<ImageGpu<Pixel>>(src1.size());
+  dst->setRoi(src1.roi());
+  std::shared_ptr<Texture2D> src1_tex = src1.genTexture(false, cudaFilterModePoint);
+  std::shared_ptr<Texture2D> src2_tex = src2.genTexture(false, cudaFilterModePoint);
+  ze::cu::weightedSum(*dst,
+                      *src1_tex, src1.roi(), weight1,
+                      *src2_tex, src2.roi(), weight2);
+  IMP_CUDA_CHECK();
+  return dst;
+}
+
+// template instantiations for all our image types
+template void weightedSum(ImageGpu8uC1& dst,
+                          const Texture2D& src1_tex, const Roi2u src1_roi, const float& weight1,
+                          const Texture2D& src2_tex, const Roi2u src2_roi, const float& weight2);
+template void weightedSum(ImageGpu32fC1& dst,
+                          const Texture2D& src1_tex, const Roi2u src1_roi, const float& weight1,
+                          const Texture2D& src2_tex, const Roi2u src2_roi, const float& weight2);
+
+template void weightedSum(ImageGpu8uC1& dst,
+                          const ImageGpu8uC1& src1, const float& weight1,
+                          const ImageGpu8uC1& src2, const float& weight2);
+template void weightedSum(ImageGpu32fC1& dst,
+                          const ImageGpu32fC1& src1, const float& weight1,
+                          const ImageGpu32fC1& src2, const float& weight2);
+
+template ImageGpu8uC1::Ptr weightedSum(
+    const ImageGpu8uC1& src1, const float& weight1,
+    const ImageGpu8uC1& src2, const float& weight2);
+template ImageGpu32fC1::Ptr weightedSum(
+    const ImageGpu32fC1& src1, const float& weight1,
+    const ImageGpu32fC1& src2, const float& weight2);
+
+} // namespace cu
+} // namespace ze
diff --git a/RWR/src/ze_oss/imp_cu_core/test/cu_linearmemory_test.cpp b/RWR/src/ze_oss/imp_cu_core/test/cu_linearmemory_test.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..3bc25872393a8445d068050934c2688cad126daa
--- /dev/null
+++ b/RWR/src/ze_oss/imp_cu_core/test/cu_linearmemory_test.cpp
@@ -0,0 +1,231 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#include <gtest/gtest.h>
+
+// system includes
+#include <assert.h>
+#include <cstdint>
+#include <iostream>
+#include <functional>
+#include <limits>
+#include <type_traits>
+
+#include <imp/cu_core/cu_utils.hpp>
+#include <imp/cu_core/cu_linearmemory.cuh>
+#include <ze/common/random.hpp>
+#include <ze/common/test_utils.hpp>
+
+template <typename Pixel>
+class CuLinearMemoryTest : public ::testing::Test
+{
+ protected:
+  CuLinearMemoryTest()
+    : linmem_(numel_)
+    , linmem_copy_(numel_)
+    , cu_linmem_(numel_)
+    , cu_linmem_init0_(numel_)
+    , cu_linmem_init_rand_(numel_)
+    , linmem_init0_cp_(numel_)
+    , linmem_init_rand_cp_(numel_)
+    , cu_roi_linmem_(roi_.length())
+    , roi_linmem_copy_(roi_.length())
+  {
+    using T = typename Pixel::T;
+    auto random_val_generator = ze::uniformDistribution<T>(ZE_DETERMINISTIC);
+
+    for (size_t i=0; i<this->numel_; ++i)
+    {
+      linmem_[i] = random_val_generator();
+    }
+
+    cu_linmem_init_rand_pixel_val_ = Pixel(random_val_generator());
+    cu_linmem_roi_init_rand_pixel_val_ = Pixel(random_val_generator());
+  }
+
+  void roundtripCopy()
+  {
+    cu_linmem_.copyFrom(linmem_);
+    IMP_CUDA_CHECK();
+    cu_linmem_.copyTo(linmem_copy_);
+    IMP_CUDA_CHECK();
+  }
+
+  void initZeroAndCopy()
+  {
+    cu_linmem_init0_.setValue(cu_linmem_init0_pixel_val_);
+    IMP_CUDA_CHECK();
+    cu_linmem_init0_.copyTo(linmem_init0_cp_);
+    IMP_CUDA_CHECK();
+  }
+
+  void setRandomValueAndCopy()
+  {
+    cu_linmem_init_rand_.setValue(cu_linmem_init_rand_pixel_val_);
+    IMP_CUDA_CHECK();
+    cu_linmem_init_rand_.copyTo(linmem_init_rand_cp_);
+    IMP_CUDA_CHECK();
+  }
+
+  void setRoi()
+  {
+    cu_linmem_.setRoi(roi_);
+    IMP_CUDA_CHECK();
+  }
+
+  void setRoiRandomValueAndCopy()
+  {
+    cu_linmem_init_rand_.setValue(cu_linmem_init_rand_pixel_val_);
+    IMP_CUDA_CHECK();
+    cu_linmem_init_rand_.setRoi(roi_);
+    cu_linmem_init_rand_.setValue(cu_linmem_roi_init_rand_pixel_val_);
+    cu_linmem_init_rand_.resetRoi();
+    IMP_CUDA_CHECK();
+
+    cu_linmem_init_rand_.copyTo(linmem_init_rand_cp_);
+    IMP_CUDA_CHECK();
+  }
+
+  void roiRoundtripCopy()
+  {
+    linmem_.setRoi(roi_);
+    IMP_CUDA_CHECK();
+    cu_roi_linmem_.copyFrom(linmem_);
+    IMP_CUDA_CHECK();
+    cu_roi_linmem_.copyTo(roi_linmem_copy_);
+    IMP_CUDA_CHECK();
+  }
+
+
+  uint8_t pixel_size_ = sizeof(Pixel);
+  size_t pixel_bit_depth_ = 8*sizeof(Pixel);
+
+  size_t numel_ = 10000;
+  ze::LinearMemory<Pixel> linmem_;
+  ze::LinearMemory<Pixel> linmem_copy_;
+  ze::cu::LinearMemory<Pixel> cu_linmem_;
+  Pixel cu_linmem_init0_pixel_val_ = Pixel(0);
+  ze::cu::LinearMemory<Pixel> cu_linmem_init0_;
+  ze::cu::LinearMemory<Pixel> cu_linmem_init_rand_;
+  Pixel cu_linmem_init_rand_pixel_val_;
+
+  ze::Roi1u roi_ = ze::Roi1u(numel_/3, numel_/3);
+  Pixel cu_linmem_roi_init_rand_pixel_val_;
+  ze::LinearMemory<Pixel> linmem_init0_cp_;
+  ze::LinearMemory<Pixel> linmem_init_rand_cp_;
+
+  ze::cu::LinearMemory<Pixel> cu_roi_linmem_;
+  ze::LinearMemory<Pixel> roi_linmem_copy_;
+};
+
+// The list of types we want to test.
+typedef testing::Types<
+ze::Pixel8uC1, ze::Pixel8uC2, ze::Pixel8uC3, ze::Pixel8uC4,
+ze::Pixel16uC1, ze::Pixel16uC2, ze::Pixel16uC3, ze::Pixel16uC4,
+ze::Pixel32sC1, ze::Pixel32sC2, ze::Pixel32sC3, ze::Pixel32sC4,
+ze::Pixel32uC1, ze::Pixel32uC2, ze::Pixel32uC3, ze::Pixel32uC4,
+ze::Pixel32fC1, ze::Pixel32fC2, ze::Pixel32fC3, ze::Pixel32fC4
+> PixelTypes;
+
+TYPED_TEST_CASE(CuLinearMemoryTest, PixelTypes);
+
+// alignment check deleted on purpose as the alignment is handeled by cuda and we don't mess around with that
+
+TYPED_TEST(CuLinearMemoryTest, CheckLength)
+{
+  ASSERT_EQ(this->numel_, this->cu_linmem_.length());
+}
+
+TYPED_TEST(CuLinearMemoryTest, CheckNumBytes)
+{
+  ASSERT_EQ(this->numel_*this->pixel_size_, this->cu_linmem_.bytes());
+}
+
+TYPED_TEST(CuLinearMemoryTest, CheckPixelBitDepth)
+{
+  ASSERT_EQ(this->pixel_bit_depth_, this->cu_linmem_.bitDepth());
+}
+
+TYPED_TEST(CuLinearMemoryTest, ReturnsTrueForNonGpuMemory)
+{
+  ASSERT_TRUE(this->cu_linmem_.isGpuMemory());
+}
+
+TYPED_TEST(CuLinearMemoryTest, CheckRoundTripCopy)
+{
+  this->roundtripCopy();
+  for (size_t i=0; i<this->numel_; ++i) {
+    ASSERT_EQ(this->linmem_[i], this->linmem_copy_[i]);
+  }
+}
+
+TYPED_TEST(CuLinearMemoryTest, CheckInitZero)
+{
+  this->initZeroAndCopy();
+  for (size_t i=0; i<this->numel_; ++i) {
+    ASSERT_EQ(this->linmem_init0_cp_[i], this->cu_linmem_init0_pixel_val_);
+  }
+}
+
+TYPED_TEST(CuLinearMemoryTest, CheckSetRandomValue)
+{
+  this->setRandomValueAndCopy();
+  for (size_t i=0; i<this->numel_; ++i) {
+    ASSERT_EQ(this->linmem_init_rand_cp_[i], this->cu_linmem_init_rand_pixel_val_);
+  }
+}
+
+TYPED_TEST(CuLinearMemoryTest, CheckNoRoi)
+{
+  ASSERT_EQ(0, this->cu_linmem_.roi().x());
+  ASSERT_EQ(this->numel_, this->cu_linmem_.roi().length());
+}
+
+TYPED_TEST(CuLinearMemoryTest, CheckRoi)
+{
+  this->setRoi();
+  ASSERT_EQ(this->roi_.x(), this->cu_linmem_.roi().x());
+  ASSERT_EQ(this->roi_.length(), this->cu_linmem_.roi().length());
+}
+
+TYPED_TEST(CuLinearMemoryTest, CheckRoiSetRandomValue)
+{
+  this->setRoiRandomValueAndCopy();
+  for (size_t i=0; i<this->numel_; ++i) {
+    if (i>=this->roi_.x() && i<(this->roi_.x()+this->roi_.length())) {
+      ASSERT_EQ(this->linmem_init_rand_cp_[i], this->cu_linmem_roi_init_rand_pixel_val_);
+    }
+    else {
+      ASSERT_EQ(this->linmem_init_rand_cp_[i], this->cu_linmem_init_rand_pixel_val_);
+    }
+  }
+}
+
+TYPED_TEST(CuLinearMemoryTest, CheckRoiRoundTripCopy)
+{
+  this->roiRoundtripCopy();
+  for (size_t i=0; i<this->roi_.length(); ++i) {
+    ASSERT_EQ(this->linmem_[i+this->roi_.x()], this->roi_linmem_copy_[i]);
+  }
+}
diff --git a/RWR/src/ze_oss/imp_cu_core/test/image_gpu_simple_test.cpp b/RWR/src/ze_oss/imp_cu_core/test/image_gpu_simple_test.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..5e50e98443b54045b0842811eb225cc1cc445669
--- /dev/null
+++ b/RWR/src/ze_oss/imp_cu_core/test/image_gpu_simple_test.cpp
@@ -0,0 +1,48 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#include <gtest/gtest.h>
+
+// system includes
+#include <assert.h>
+#include <cstdint>
+#include <iostream>
+
+#include <imp/cu_core/cu_utils.hpp>
+#include <imp/cu_core/cu_image_gpu.cuh>
+
+
+TEST(IMPCuCoreTestSuite,imageGpuSimpleTest)
+{
+  //
+  // 2D image
+  //
+  int width = 123;
+  int height = 324;
+  ze::cu::ImageGpu8uC1 im(width, height);
+  IMP_CUDA_CHECK();
+  ASSERT_EQ(width, im.width());
+  ASSERT_EQ(height, im.height());
+  IMP_CUDA_CHECK();
+}
diff --git a/RWR/src/ze_oss/imp_cu_core/test/image_gpu_test.cpp b/RWR/src/ze_oss/imp_cu_core/test/image_gpu_test.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a1484aecfd3792064a36aa9bdaceb1ea058330d7
--- /dev/null
+++ b/RWR/src/ze_oss/imp_cu_core/test/image_gpu_test.cpp
@@ -0,0 +1,118 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#include <gtest/gtest.h>
+
+// system includes
+#include <assert.h>
+#include <cstdint>
+#include <iostream>
+
+#include <imp/cu_core/cu_image_gpu.cuh>
+
+template<typename _Pixel, imp::PixelType _pixel_type>
+struct TypePair
+{
+  using Pixel = _Pixel;
+  //typedef typename _pixel_type pixel_type;
+  using pixel_type = _pixel_type;
+};
+
+template <class T>
+class ImageGpuTest : public ::testing::Test
+{
+protected:
+  ImageGpuTest() :
+    im_(width_,height_)
+  {
+  }
+
+  size_t pixel_size_ = sizeof(T::Pixel);
+  size_t pixel_bit_depth_ = 8*sizeof(T::Pixel);
+
+  size_t width_ = 256;
+  size_t height_ = 256;
+  imp::cu::ImageGpu<typename T::Pixel, T::pixel_type> im_;
+};
+
+// The list of types we want to test.
+typedef testing::Types<
+TypePair<imp::Pixel8uC1, imp::PixelType::i8uC1>,
+TypePair<imp::Pixel8uC2, imp::PixelType::i8uC2>,
+TypePair<imp::Pixel8uC3, imp::PixelType::i8uC3>,
+TypePair<imp::Pixel8uC4, imp::PixelType::i8uC4>,
+
+TypePair<imp::Pixel16uC1, imp::PixelType::i16uC1>,
+TypePair<imp::Pixel16uC2, imp::PixelType::i16uC2>,
+TypePair<imp::Pixel16uC3, imp::PixelType::i16uC3>,
+TypePair<imp::Pixel16uC4, imp::PixelType::i16uC4>,
+
+TypePair<imp::Pixel32sC1, imp::PixelType::i32sC1>,
+TypePair<imp::Pixel32sC2, imp::PixelType::i32sC2>,
+TypePair<imp::Pixel32sC3, imp::PixelType::i32sC3>,
+TypePair<imp::Pixel32sC4, imp::PixelType::i32sC4>,
+
+TypePair<imp::Pixel32fC1, imp::PixelType::i32fC1>,
+TypePair<imp::Pixel32fC2, imp::PixelType::i32fC2>,
+TypePair<imp::Pixel32fC3, imp::PixelType::i32fC3>,
+TypePair<imp::Pixel32fC4, imp::PixelType::i32fC4>
+> PixelTypePairs;
+
+TYPED_TEST_CASE(ImageGpuTest, PixelTypePairs);
+
+
+TYPED_TEST(ImageGpuTest, CheckWidth)
+{
+  ASSERT_EQ(this->width_, this->im_.width());
+}
+
+//TYPED_TEST(ImageGpuTest, CheckHeight)
+//{
+//  ASSERT_EQ(this->height_, this->im_.height());
+//}
+
+//TYPED_TEST(ImageGpuTest, CheckMemoryAlignment)
+//{
+//  ASSERT_EQ(0, (std::uintptr_t)reinterpret_cast<void*>(this->im_.data()) % 32);
+//}
+
+//TYPED_TEST(ImageGpuTest, CheckNnumel)
+//{
+//  ASSERT_EQ(this->height_*this->width_, this->im_.numel());
+//}
+
+//TYPED_TEST(ImageGpuTest, CheckNumBytes)
+//{
+//  ASSERT_LE(this->height_*this->width_*this->pixel_size_, this->im_.bytes());
+//}
+
+//TYPED_TEST(ImageGpuTest, CheckPixelBitDepth)
+//{
+//  ASSERT_EQ(this->pixel_bit_depth_, this->im_.bitDepth());
+//}
+
+//TYPED_TEST(ImageGpuTest, ReturnsTrueForGpuMemory)
+//{
+//  ASSERT_TRUE(this->im_.isGpuMemory());
+//}
diff --git a/RWR/src/ze_oss/imp_cu_core/test/min_max_test.cpp b/RWR/src/ze_oss/imp_cu_core/test/min_max_test.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d48b1f41a314260a33c144f10cfa688e32a27d8a
--- /dev/null
+++ b/RWR/src/ze_oss/imp_cu_core/test/min_max_test.cpp
@@ -0,0 +1,115 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#include <gtest/gtest.h>
+
+// system includes
+#include <assert.h>
+#include <cstdint>
+#include <cfloat>
+#include <iostream>
+#include <functional>
+#include <limits>
+
+#include <imp/core/image_raw.hpp>
+#include <imp/cu_core/cu_math.cuh>
+#include <imp/cu_core/cu_utils.hpp>
+#include <ze/common/random.hpp>
+#include <ze/common/test_utils.hpp>
+
+
+TEST(IMPCuCoreTestSuite,minMaxTest_8uC1)
+{
+  auto random_val = ze::uniformDistribution<ze::uint8_t>(ZE_DETERMINISTIC);
+
+  size_t width = 123;
+  size_t height = 324;
+  ze::ImageRaw8uC1 im(width,height);
+  std::uint8_t min_val = std::numeric_limits<std::uint8_t>::max();
+  std::uint8_t max_val = std::numeric_limits<std::uint8_t>::lowest();
+  for (size_t y=0; y<height; ++y)
+  {
+    for (size_t x=0; x<width; ++x)
+    {
+      std::uint8_t random_value = random_val();
+      im[y][x] = random_value;
+      min_val = ze::cu::min(min_val, random_value);
+      max_val = ze::cu::max(max_val, random_value);
+    }
+  }
+
+  IMP_CUDA_CHECK();
+  ze::cu::ImageGpu8uC1 cu_im(im);
+  IMP_CUDA_CHECK();
+  ze::Pixel8uC1 min_pixel, max_pixel;
+  IMP_CUDA_CHECK();
+  ze::cu::minMax(cu_im, min_pixel, max_pixel);
+  IMP_CUDA_CHECK();
+
+  ASSERT_EQ(min_val, min_pixel);
+  ASSERT_EQ(max_val, max_pixel);
+}
+
+
+TEST(IMPCuCoreTestSuite,minMaxTest_32fC1)
+{
+  // setup random number generator
+  auto random_val = ze::uniformDistribution<float>(ZE_DETERMINISTIC);
+
+  size_t width = 1250;
+  size_t height = 325;
+  ze::ImageRaw32fC1 im(width,height);
+  float min_val = std::numeric_limits<float>::max();
+  float max_val = std::numeric_limits<float>::lowest();
+  for (size_t y=0; y<height; ++y)
+  {
+    for (size_t x=0; x<width; ++x)
+    {
+      float random_value = random_val();
+      im[y][x] = random_value;
+      min_val = ze::cu::min(min_val, random_value);
+      max_val = ze::cu::max(max_val, random_value);
+
+//      VLOG(1) << "random: [" << x << "][" << y << "]: " << random_value;
+    }
+  }
+
+  VLOG(1) << "numeric:  min, max: " << std::numeric_limits<float>::lowest() << " " << std::numeric_limits<float>::max();
+  VLOG(1) << "numeric2: min, max: " << FLT_MIN << " " << FLT_MAX;
+  VLOG(1) << "CPU min, max: " << min_val << " " << max_val;
+
+  IMP_CUDA_CHECK();
+  ze::cu::ImageGpu32fC1 cu_im(im);
+  IMP_CUDA_CHECK();
+  ze::Pixel32fC1 min_pixel, max_pixel;
+  IMP_CUDA_CHECK();
+  ze::cu::minMax(cu_im, min_pixel, max_pixel);
+  IMP_CUDA_CHECK();
+
+  VLOG(1) << "GPU min, max: " << min_pixel << " " << max_pixel;
+
+
+  ASSERT_EQ(min_val, min_pixel.x);
+  ASSERT_EQ(max_val, max_pixel.x);
+}
diff --git a/RWR/src/ze_oss/imp_cu_core/test/reduction_test.cpp b/RWR/src/ze_oss/imp_cu_core/test/reduction_test.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b34f479c816398762714b8ddf148defcac22cbfc
--- /dev/null
+++ b/RWR/src/ze_oss/imp_cu_core/test/reduction_test.cpp
@@ -0,0 +1,183 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#include <gtest/gtest.h>
+
+// system includes
+#include <assert.h>
+#include <cstdint>
+#include <cfloat>
+#include <iostream>
+#include <random>
+#include <functional>
+#include <limits>
+#include <imp/core/image_raw.hpp>
+#include <imp/cu_core/cu_math.cuh>
+#include <imp/cu_core/cu_utils.hpp>
+#include <imp/cu_core/cu_image_reduction.cuh>
+#include <ze/common/benchmark.hpp>
+#include <ze/common/file_utils.hpp>
+#include <ze/common/random.hpp>
+#include <ze/common/test_utils.hpp>
+
+namespace ze {
+
+double gtSum(const ze::ImageRaw32fC1& im)
+{
+  double sum{0};
+  for (size_t y = 0; y < im.height(); ++y)
+  {
+    for (size_t x = 0; x < im.width(); ++x)
+    {
+      sum += im.pixel(x, y);
+    }
+  }
+  return sum;
+}
+
+ze::ImageRaw32fC1 generateRandomImage(size_t width, size_t height)
+{
+  auto random_val = ze::uniformDistribution<float>(ZE_DETERMINISTIC);
+  ze::ImageRaw32fC1 im(width,height);
+  for (size_t y = 0; y < im.height(); ++y)
+  {
+    for (size_t x = 0; x < im.width(); ++x)
+    {
+      float random_value = random_val();
+      im[y][x] = random_value;
+    }
+  }
+  return im;
+}
+
+ze::ImageRaw32fC1 generateConstantImage(size_t width, size_t height, float val)
+{
+  ze::ImageRaw32fC1 im(width,height);
+  for (size_t y = 0; y < im.height(); ++y)
+  {
+    for (size_t x = 0; x < im.width(); ++x)
+    {
+      im[y][x] = val;
+    }
+  }
+  return im;
+}
+
+} // ze namespace
+
+TEST(IMPCuCoreTestSuite, sumByReductionTestConstImg_32fC1)
+{
+  const size_t width = 752;
+  const size_t height = 480;
+  const float val = 0.1f;
+  ze::ImageRaw32fC1 im =
+      ze::generateConstantImage(width, height, val);
+  VLOG(1) << "test image has been filled with the constant value " << val;
+  double gt_sum = static_cast<double>(width*height) * val;
+
+  IMP_CUDA_CHECK();
+  ze::cu::ImageGpu32fC1 cu_im(im);
+  IMP_CUDA_CHECK();
+
+  ze::cu::ImageReducer<ze::Pixel32fC1> reducer;
+  double cu_sum;
+  auto sumReductionLambda = [&](){
+    cu_sum = reducer.sum(cu_im);
+  };
+  reducer.sum(cu_im); //! Warm-up
+  ze::runTimingBenchmark(
+        sumReductionLambda,
+        20, 40,
+        "sum using parallel reduction",
+        true);
+  const double tolerance = 0.015;
+  EXPECT_NEAR(gt_sum, cu_sum, tolerance);
+  VLOG(1) << "GT sum: " << std::fixed << gt_sum;
+  VLOG(1) << "GPU sum: " << std::fixed << cu_sum;
+  VLOG(1) << "Test tolerance: " << std::fixed << tolerance;
+}
+
+
+TEST(IMPCuCoreTestSuite, sumByReductionTestRndImg_32fC1)
+{
+  const size_t width = 752;
+  const size_t height = 480;
+  ze::ImageRaw32fC1 im =
+      ze::generateRandomImage(width, height);
+  double gt_sum = ze::gtSum(im);
+
+  IMP_CUDA_CHECK();
+  ze::cu::ImageGpu32fC1 cu_im(im);
+  IMP_CUDA_CHECK();
+
+  ze::cu::ImageReducer<ze::Pixel32fC1> reducer;
+  double cu_sum;
+  auto sumReductionLambda = [&](){
+    cu_sum = reducer.sum(cu_im);
+  };
+  reducer.sum(cu_im); //! Warm-up
+  ze::runTimingBenchmark(
+        sumReductionLambda,
+        20, 40,
+        "sum using parallel reduction",
+        true);
+  const double tolerance = 0.015;
+  EXPECT_NEAR(gt_sum, cu_sum, tolerance);
+  VLOG(1) << "GT sum: " << std::fixed << gt_sum;
+  VLOG(1) << "GPU sum: " << std::fixed << cu_sum;
+  VLOG(1) << "Test tolerance: " << std::fixed << tolerance;
+}
+
+TEST(IMPCuCoreTestSuite, countEqualByReductionTestConstImg_32sC1)
+{
+  const size_t width = 752;
+  const size_t height = 480;
+  ze::ImageRaw32sC1 im(width, height);
+  const size_t gt{4};
+  const int probe_val{7};
+  //! Fill GT pixels with gt probe values
+  im.pixel(width/2, height/2) = probe_val;
+  im.pixel(width/4, height/4) = probe_val;
+  im.pixel(width/2, height/4) = probe_val;
+  im.pixel(0, 0) = probe_val;
+
+  IMP_CUDA_CHECK();
+  ze::cu::ImageGpu32sC1 cu_im(im);
+  IMP_CUDA_CHECK();
+
+  ze::cu::ImageReducer<ze::Pixel32sC1> reducer;
+  size_t cu_res;
+  auto countEqualReductionLambda = [&](){
+    cu_res = reducer.countEqual(cu_im, probe_val);
+  };
+  reducer.countEqual(cu_im, probe_val); //! Warm-up
+  ze::runTimingBenchmark(
+        countEqualReductionLambda,
+        20, 40,
+        "countEqual using parallel reduction",
+        true);
+  EXPECT_EQ(gt, cu_res);
+  VLOG(1) << "GT countEqual: " << gt;
+  VLOG(1) << "GPU countEqual: " << cu_res;
+}
diff --git a/RWR/src/ze_oss/imp_cu_core/test/sum_test.cpp b/RWR/src/ze_oss/imp_cu_core/test/sum_test.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..01243798b121930135d5e1c5addecef0fe7efc0a
--- /dev/null
+++ b/RWR/src/ze_oss/imp_cu_core/test/sum_test.cpp
@@ -0,0 +1,77 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#include <gtest/gtest.h>
+
+// system includes
+#include <assert.h>
+#include <cstdint>
+#include <cfloat>
+#include <iostream>
+#include <functional>
+#include <limits>
+#include <imp/core/image_raw.hpp>
+#include <imp/cu_core/cu_math.cuh>
+#include <imp/cu_core/cu_utils.hpp>
+#include <ze/common/benchmark.hpp>
+#include <ze/common/random.hpp>
+#include <ze/common/test_utils.hpp>
+
+TEST(IMPCuCoreTestSuite, sumTest_32fC1)
+{
+  // setup random number generator
+  auto random_val = ze::uniformDistribution<float>(ZE_DETERMINISTIC);
+
+  const size_t width = 752;
+  const size_t height = 480;
+  ze::ImageRaw32fC1 im(width,height);
+
+  double gt_sum = 0.0;
+  for (size_t y = 0; y < height; ++y)
+  {
+    for (size_t x = 0; x < width; ++x)
+    {
+      float random_value = random_val();
+      im[y][x] = random_value;
+      gt_sum += im.pixel(x, y);
+    }
+  }
+  double cu_sum;
+  ze::cu::ImageGpu32fC1 cu_im(im);
+  auto sumTextureLambda = [&](){
+    cu_sum = ze::cu::sum(cu_im);
+  };
+  ze::cu::sum(cu_im); //! Warm-up
+  ze::runTimingBenchmark(
+        sumTextureLambda,
+        20, 40,
+        "sum using texture memory",
+        true);
+  const double tolerance = 0.15;
+
+  EXPECT_NEAR(gt_sum, cu_sum, tolerance);
+  VLOG(1) << "GT sum: " << std::fixed << gt_sum;
+  VLOG(1) << "GPU sum: " << std::fixed << cu_sum;
+  VLOG(1) << "Test tolerance: " << std::fixed << tolerance;
+}
diff --git a/RWR/src/ze_oss/imp_cu_core/test/test_main.cpp b/RWR/src/ze_oss/imp_cu_core/test/test_main.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ddb67c34d745f3f24c9137664d451bf82fb8cce8
--- /dev/null
+++ b/RWR/src/ze_oss/imp_cu_core/test/test_main.cpp
@@ -0,0 +1,42 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#include <gflags/gflags.h>
+#include <gtest/gtest.h>
+#include <cuda_runtime_api.h>
+#include <ze/common/logging.hpp>
+
+/// Run all the tests that were declared with TEST()
+int main(int argc, char **argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  google::InitGoogleLogging(argv[0]);
+  google::ParseCommandLineFlags(&argc, &argv, false);
+  google::InstallFailureSignalHandler();
+  FLAGS_alsologtostderr = true;
+  FLAGS_colorlogtostderr = true;
+  int ret = RUN_ALL_TESTS();
+  cudaDeviceReset();
+  return ret;
+}
diff --git a/RWR/src/ze_oss/imp_cu_core/test/test_weighted_sum.cpp b/RWR/src/ze_oss/imp_cu_core/test/test_weighted_sum.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..fa6b7975e4b2ebf07f708ccb899f1d20782dbf36
--- /dev/null
+++ b/RWR/src/ze_oss/imp_cu_core/test/test_weighted_sum.cpp
@@ -0,0 +1,156 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#include <sstream>
+#include <string>
+
+#include <ze/common/benchmark.hpp>
+#include <ze/common/random.hpp>
+#include <ze/common/test_entrypoint.hpp>
+#include <ze/common/test_utils.hpp>
+#include <ze/common/types.hpp>
+
+#include <imp/cu_core/cu_image_gpu.cuh>
+#include <imp/cu_core/cu_math.cuh>
+#include <imp/core/image_raw.hpp>
+
+template <typename Pixel>
+class CuWeightedSumTestFixture : public ::testing::Test
+{
+protected:
+  CuWeightedSumTestFixture()
+    : image1_(size_)
+    , image2_(size_)
+    , dst_(size_)
+    , cu_image1_(size_)
+    , cu_image2_(size_)
+    , cu_dst_(size_)
+  {
+    auto random_val_generator = ze::uniformDistribution<typename Pixel::T>(ZE_DETERMINISTIC);
+    val1_ = random_val_generator();
+    val2_ = random_val_generator();
+    auto float_random_val_generator = ze::uniformDistribution<float>(ZE_DETERMINISTIC);
+    weight1_ = float_random_val_generator();
+    weight2_ = float_random_val_generator();
+
+    if (std::is_integral<typename Pixel::T>::value)
+    {
+      VLOG(100) << "given Pixel type is integral";
+      dst_val_ = static_cast<Pixel>(val1_*weight1_ + val2_*weight2_ + 0.5f);
+    }
+    else
+    {
+      VLOG(100) << "given Pixel type is non-integral";
+      dst_val_ = val1_*weight1_ + val2_*weight2_;
+    }
+
+    image1_.setValue(val1_);
+    image2_.setValue(val2_);
+
+    VLOG(1) << "val1: '" << val1_ << "', val2: '" << val2_ << "'";
+    VLOG(1) << "weight1: '" << weight1_ << "', weight2: '" << weight2_ << "'";
+    VLOG(1) << "dst_val: '" << dst_val_ << "'";
+
+    VLOG(1) << "image1 | size: " << image1_.size() << ", roi: " << image1_.roi()
+            << " value(10,10): " << image1_.pixel(10,10);
+    VLOG(1) << "image2 | size: " << image2_.size() << ", roi: " << image2_.roi()
+            << " value(10,10): " << image2_.pixel(10,10);
+    cu_image1_.copyFrom(image1_);
+    cu_image2_.copyFrom(image2_);
+  }
+
+  void setRoi()
+  {
+    image1_.setRoi(roi_);
+    image2_.setRoi(roi_);
+    cu_image1_.setRoi(roi_);
+    cu_image2_.setRoi(roi_);
+    dst_.setRoi(roi_);
+    cu_dst_.setRoi(roi_);
+  }
+
+
+  void weightedSum()
+  {
+    ze::cu::weightedSum(cu_dst_, cu_image1_, weight1_, cu_image2_, weight2_);
+    cu_dst_.copyTo(dst_);
+    VLOG(1) << "dst | size: " << dst_.size() << ", roi: " << dst_.roi()
+            << " value(10,10): " << dst_.pixel(10,10);
+  }
+
+protected:
+  //! @todo random size and roi!
+  ze::Size2u size_{511u,512u};
+  ze::Roi2u roi_{128,128,256,256};
+  ze::ImageRaw<Pixel> image1_;
+  ze::ImageRaw<Pixel> image2_;
+  ze::ImageRaw<Pixel> dst_;
+  ze::cu::ImageGpu<Pixel> cu_image1_;
+  ze::cu::ImageGpu<Pixel> cu_image2_;
+  ze::cu::ImageGpu<Pixel> cu_dst_;
+  Pixel val1_;
+  Pixel val2_;
+  Pixel dst_val_;
+  float weight1_;
+  float weight2_;
+};
+
+
+// The list of types we want to test.
+typedef testing::Types<
+ze::Pixel8uC1, ze::Pixel32fC1
+> PixelTypes;
+
+TYPED_TEST_CASE(CuWeightedSumTestFixture, PixelTypes);
+
+TYPED_TEST(CuWeightedSumTestFixture, testweightedSum)
+{
+  using namespace ze::cu;
+
+  this->weightedSum();
+  for (uint32_t y=0; y<this->dst_.height(); ++y)
+  {
+    for (uint32_t x=0; x<this->dst_.width(); ++x)
+    {
+      CHECK_EQ(this->dst_val_, this->dst_.pixel(x,y));
+    }
+  }
+}
+
+TYPED_TEST(CuWeightedSumTestFixture, testweightedSumRoi)
+{
+  using namespace ze::cu;
+
+  this->setRoi();
+  this->weightedSum();
+  for (uint32_t y=this->roi_.y(); y<this->roi_.y()+this->roi_.height(); ++y)
+  {
+    for (uint32_t x=this->roi_.x(); x<this->roi_.x()+this->roi_.width(); ++x)
+    {
+      CHECK_EQ(this->dst_val_, this->dst_.pixel(x,y));
+    }
+  }
+}
+
+ZE_UNITTEST_ENTRYPOINT
diff --git a/RWR/src/ze_oss/imp_cu_correspondence/CATKIN_IGNORE b/RWR/src/ze_oss/imp_cu_correspondence/CATKIN_IGNORE
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/RWR/src/ze_oss/imp_cu_correspondence/CMakeLists.txt b/RWR/src/ze_oss/imp_cu_correspondence/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..53fb3fd0f5c101a19c446ea084a1e5c429295c3f
--- /dev/null
+++ b/RWR/src/ze_oss/imp_cu_correspondence/CMakeLists.txt
@@ -0,0 +1,78 @@
+project(imp_cu_correspondence)
+cmake_minimum_required(VERSION 2.8.0)
+
+if(${CMAKE_MAJOR_VERSION} VERSION_GREATER 3.0)
+  cmake_policy(SET CMP0054 OLD)
+endif(${CMAKE_MAJOR_VERSION} VERSION_GREATER 3.0)
+
+find_package(catkin_simple REQUIRED)
+catkin_simple(ALL_DEPS_REQUIRED)
+
+include(ze_setup)
+include(ze_macros_cuda)
+find_cuda()
+
+set(HEADERS
+  include/imp/cu_correspondence/variational_stereo.hpp
+  include/imp/cu_correspondence/variational_epipolar_stereo.hpp
+  include/imp/cu_correspondence/variational_stereo_parameters.hpp
+  include/imp/cu_correspondence/stereo_solver_enum.hpp
+  include/imp/cu_correspondence/stereo_ctf_warping.hpp
+  include/imp/cu_correspondence/solver_stereo_abstract.hpp
+  # TODO # include/imp/cu_correspondence/solver_epipolar_stereo_abstract.hpp
+  )
+
+set(SOURCES
+  src/variational_stereo.cpp
+  src/variational_epipolar_stereo.cpp
+  src/stereo_ctf_warping.cpp
+  )
+
+set(CU_HDRS
+  include/imp/cu_correspondence/solver_stereo_huber_l1.cuh
+  include/imp/cu_correspondence/solver_stereo_precond_huber_l1.cuh
+  include/imp/cu_correspondence/solver_stereo_precond_huber_l1_weighted.cuh
+  include/imp/cu_correspondence/solver_epipolar_stereo_precond_huber_l1.cuh
+  include/imp/cu_correspondence/occlusion.cuh
+)
+
+
+set(CU_SRCS
+  src/warped_gradients_kernel.cuh
+
+  src/solver_stereo_huber_l1_kernel.cuh
+  src/solver_stereo_huber_l1.cu
+
+  src/solver_precond_huber_l1_kernel.cuh
+  src/solver_stereo_precond_huber_l1.cu
+
+  src/solver_stereo_precond_huber_l1_weighted_kernel.cuh
+  src/solver_stereo_precond_huber_l1_weighted.cu
+
+  src/solver_epipolar_stereo_precond_huber_l1_kernel.cuh
+  src/solver_epipolar_stereo_precond_huber_l1.cu
+
+  src/occlusion.cu
+  #src/occlusion_kernel.cuh
+  )
+
+cs_cuda_add_library(${PROJECT_NAME}
+  ${CU_SRCS} ${CU_HDRS} ${SOURCES} ${HEADERS}
+  )
+
+
+##########
+# GTESTS #
+##########
+
+catkin_add_gtest(test_dense_variational_stereo test/test_dense_variational_stereo.cpp)
+target_link_libraries(test_dense_variational_stereo ${PROJECT_NAME})
+
+catkin_add_gtest(test_dense_variational_epipolar_stereo test/test_dense_variational_epipolar_stereo.cpp)
+target_link_libraries(test_dense_variational_epipolar_stereo ${PROJECT_NAME})
+
+##########
+# EXPORT #
+##########
+cs_install()
+cs_export()
diff --git a/RWR/src/ze_oss/imp_cu_correspondence/include/imp/cu_correspondence/occlusion.cuh b/RWR/src/ze_oss/imp_cu_correspondence/include/imp/cu_correspondence/occlusion.cuh
new file mode 100644
index 0000000000000000000000000000000000000000..8cc390d05695249ddb40b2746472a5f574450235
--- /dev/null
+++ b/RWR/src/ze_oss/imp_cu_correspondence/include/imp/cu_correspondence/occlusion.cuh
@@ -0,0 +1,41 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#ifndef IMP_CU_OCCLUSION_CUH
+#define IMP_CU_OCCLUSION_CUH
+
+#include <memory>
+#include <imp/cu_core/cu_image_gpu.cuh>
+
+namespace ze {
+namespace cu {
+
+void occlusionCandidatesUniqunessMapping(ImageGpu32fC1::Ptr occ,
+                                         const ImageGpu32fC1::Ptr& disp);
+
+} // namespace cu
+} // namespace ze
+
+
+#endif // IMP_CU_OCCLUSION_CUH
diff --git a/RWR/src/ze_oss/imp_cu_correspondence/include/imp/cu_correspondence/solver_epipolar_stereo_precond_huber_l1.cuh b/RWR/src/ze_oss/imp_cu_correspondence/include/imp/cu_correspondence/solver_epipolar_stereo_precond_huber_l1.cuh
new file mode 100644
index 0000000000000000000000000000000000000000..1dcbef2a5a4f5e837af5d818260638602b4a0f6d
--- /dev/null
+++ b/RWR/src/ze_oss/imp_cu_correspondence/include/imp/cu_correspondence/solver_epipolar_stereo_precond_huber_l1.cuh
@@ -0,0 +1,113 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#ifndef IMP_CU_EPIPOLAR_STEREO_PRECOND_HUBER_L1_CUH
+#define IMP_CU_EPIPOLAR_STEREO_PRECOND_HUBER_L1_CUH
+
+#include <cstdint>
+#include <memory>
+
+#include <imp/cu_correspondence/solver_stereo_abstract.hpp>
+#include <imp/cu_core/cu_image_gpu.cuh>
+#include <imp/cu_core/cu_matrix.cuh>
+#include <imp/cu_core/cu_se3.cuh>
+#include <imp/cu_core/cu_pinhole_camera.cuh>
+#include <imp/core/size.hpp>
+
+namespace ze {
+namespace cu {
+
+// forward decl
+class VariationalStereoParameters;
+class Texture2D;
+
+/**
+ * @brief The StereoCtFWarpingLevelPrecondHuberL1 class
+ */
+class SolverEpipolarStereoPrecondHuberL1 : public SolverStereoAbstract
+{
+public:
+  SolverEpipolarStereoPrecondHuberL1() = delete;
+  virtual ~SolverEpipolarStereoPrecondHuberL1();
+
+  SolverEpipolarStereoPrecondHuberL1(const Parameters::Ptr& params,
+                                     ze::Size2u size, size_t level,
+                                     const std::vector<cu::PinholeCamera>& cams,
+                                     const cu::Matrix3f& F,
+                                     const cu::SE3<float>& T_mov_fix,
+                                     const ImageGpu32fC1& depth_proposal,
+                                     const ze::cu::ImageGpu32fC1& depth_proposal_sigma2);
+
+  virtual void init();
+  virtual void init(const SolverStereoAbstract& rhs);
+  virtual inline void setFundamentalMatrix(const cu::Matrix3f& F) {F_ = F;}
+
+  virtual void solve(std::vector<ImageGpu32fC1::Ptr> images);
+
+  virtual inline ImageGpu32fC1::Ptr getDisparities() {return u_;}
+
+protected:
+  ImageGpu32fC1::Ptr u_; //!< disparities (result)
+  std::unique_ptr<ImageGpu32fC1> u_prev_; //!< disparities results from previous iteration
+  std::unique_ptr<ImageGpu32fC1> u0_; //!< disparities results from previous warp
+  std::unique_ptr<ImageGpu32fC2> pu_; //!< dual variable for primal variable
+  std::unique_ptr<ImageGpu32fC1> q_; //!< dual variable for data term
+  std::unique_ptr<ImageGpu32fC1> iw_; //!< warped moving image
+  std::unique_ptr<ImageGpu32fC1> ix_; //!< spatial gradients on moving (warped) image
+  std::unique_ptr<ImageGpu32fC1> it_; //!< temporal gradients between warped and fixed image
+  std::unique_ptr<ImageGpu32fC1> xi_; //!< preconditioner
+  std::unique_ptr<ImageGpu32fC1> g_; //!< for edge weighting
+
+  cu::Matrix3f F_;
+  std::vector<cu::PinholeCamera> cams_;
+  cu::SE3<float> T_mov_fix_;
+  std::unique_ptr<ImageGpu32fC1> depth_proposal_;
+  std::unique_ptr<ImageGpu32fC1> depth_proposal_sigma2_;
+
+  // textures
+  std::shared_ptr<Texture2D> lambda_tex_;
+  std::shared_ptr<Texture2D> i1_tex_;
+  std::shared_ptr<Texture2D> i2_tex_;
+  std::shared_ptr<Texture2D> u_tex_;
+  std::shared_ptr<Texture2D> u_prev_tex_;
+  std::shared_ptr<Texture2D> u0_tex_;
+  std::shared_ptr<Texture2D> pu_tex_;
+  std::shared_ptr<Texture2D> q_tex_;
+  std::shared_ptr<Texture2D> ix_tex_;
+  std::shared_ptr<Texture2D> it_tex_;
+  std::shared_ptr<Texture2D> xi_tex_;
+  std::shared_ptr<Texture2D> g_tex_;
+
+  std::shared_ptr<Texture2D> depth_proposal_tex_;
+  std::shared_ptr<Texture2D> depth_proposal_sigma2_tex_;
+
+//  std::shared_ptr<Texture2D> correspondence_guess_tex_;
+//  std::shared_ptr<Texture2D> epi_vec_tex_;
+
+};
+
+} // namespace cu
+} // namespace ze
+
+#endif // IMP_CU_EPIPOLAR_STEREO_PRECOND_HUBER_L1_CUH
diff --git a/RWR/src/ze_oss/imp_cu_correspondence/include/imp/cu_correspondence/solver_stereo_abstract.hpp b/RWR/src/ze_oss/imp_cu_correspondence/include/imp/cu_correspondence/solver_stereo_abstract.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..bea19b1440a2f0a17a6e1ca0ac7763f57523baf0
--- /dev/null
+++ b/RWR/src/ze_oss/imp_cu_correspondence/include/imp/cu_correspondence/solver_stereo_abstract.hpp
@@ -0,0 +1,92 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#pragma once
+
+#include <cstdint>
+#include <memory>
+
+#include <imp/cu_core/cu_image_gpu.cuh>
+#include <imp/core/size.hpp>
+#include <imp/cu_correspondence/variational_stereo_parameters.hpp>
+
+
+namespace ze {
+namespace cu {
+
+/**
+ * @brief The StereoCtFWarpingLevel class
+ */
+class SolverStereoAbstract
+{
+public:
+  using ImageGpu32fC1 = ze::cu::ImageGpu32fC1;
+  using ImageGpu32fC2 = ze::cu::ImageGpu32fC2;
+  using Parameters = VariationalStereoParameters;
+
+public:
+  SolverStereoAbstract() = delete;
+  virtual ~SolverStereoAbstract() = default;
+
+  SolverStereoAbstract(Parameters::Ptr params,
+                       ze::Size2u size, std::uint16_t level)
+    : params_(params)
+    , size_(size)
+    , level_(level)
+  { ; }
+
+  virtual void init() = 0;
+  virtual void init(const SolverStereoAbstract& rhs) = 0;
+  virtual void solve(std::vector<ImageGpu32fC1::Ptr> images) = 0;
+
+  /**
+   * @brief computePrimalEnergy returns an the primal energy with the current disparity values.
+   * @note There is no need to implement this function so by default a nullptr is returned
+   * @return Pixel-wise primal energy
+   */
+  virtual ImageGpu32fC1::Ptr computePrimalEnergy() {return nullptr;}
+
+  virtual ImageGpu32fC1::Ptr getDisparities() = 0;
+
+  /**
+   * @brief getOcclusion returns an estimate of occluded pixels
+   * @note There is no need to implement this function so by default a nullptr is returned
+   * @return A mask with an estimate of occluded pixels or nullptr if not estimated.
+   */
+  virtual ImageGpu32fC1::Ptr getOcclusion() {return nullptr;}
+
+
+  // setters / getters
+  inline ze::Size2u size() { return size_; }
+  inline std::uint16_t level() { return level_; }
+
+protected:
+  Parameters::Ptr params_; //!< configuration parameters
+  ze::Size2u size_;
+  std::uint16_t level_; //!< level number in the ctf pyramid (0=finest .. n=coarsest)
+};
+
+} // namespace cu
+} // namespace ze
+
diff --git a/RWR/src/ze_oss/imp_cu_correspondence/include/imp/cu_correspondence/solver_stereo_huber_l1.cuh b/RWR/src/ze_oss/imp_cu_correspondence/include/imp/cu_correspondence/solver_stereo_huber_l1.cuh
new file mode 100644
index 0000000000000000000000000000000000000000..72e9f72c9b5be94ded3293d63e99b3df20746ba9
--- /dev/null
+++ b/RWR/src/ze_oss/imp_cu_correspondence/include/imp/cu_correspondence/solver_stereo_huber_l1.cuh
@@ -0,0 +1,82 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#pragma once
+
+#include <cstdint>
+#include <memory>
+
+#include <imp/cu_correspondence/solver_stereo_abstract.hpp>
+#include <imp/cu_core/cu_image_gpu.cuh>
+#include <imp/core/size.hpp>
+
+namespace ze {
+namespace cu {
+
+// forward decl
+class Texture2D;
+
+/**
+ * @brief The SolverStereoHuberL1 class computes the disparities between two views
+ *        by using a Huber-L1 Regularization-Dataterm combination
+ *        optimized with a primal-dual optimization.
+ */
+class SolverStereoHuberL1 : public SolverStereoAbstract
+{
+public:
+  SolverStereoHuberL1() = delete;
+  virtual ~SolverStereoHuberL1();
+
+  SolverStereoHuberL1(const Parameters::Ptr& params,
+                      ze::Size2u size, size_t level);
+
+  virtual void init() override;
+  virtual void init(const SolverStereoAbstract& rhs) override;
+  virtual void solve(std::vector<ImageGpu32fC1::Ptr> images) override;
+
+  virtual inline ImageGpu32fC1::Ptr getDisparities() override {return u_;}
+
+
+protected:
+  ImageGpu32fC1::Ptr u_; //!< disparities (result)
+  std::unique_ptr<ImageGpu32fC1> u_prev_; //!< disparities results from previous iteration
+  std::unique_ptr<ImageGpu32fC1> u0_; //!< disparities results from previous warp
+  std::unique_ptr<ImageGpu32fC2> pu_; //!< dual variable for primal variable
+  std::unique_ptr<ImageGpu32fC1> ix_; //!< spatial gradients on moving (warped) image
+  std::unique_ptr<ImageGpu32fC1> it_; //!< temporal gradients between warped and fixed image
+
+  // textures
+  std::shared_ptr<Texture2D> i1_tex_;
+  std::shared_ptr<Texture2D> i2_tex_;
+  std::shared_ptr<Texture2D> u_tex_;
+  std::shared_ptr<Texture2D> u_prev_tex_;
+  std::shared_ptr<Texture2D> u0_tex_;
+  std::shared_ptr<Texture2D> pu_tex_;
+  std::shared_ptr<Texture2D> ix_tex_;
+  std::shared_ptr<Texture2D> it_tex_;
+
+};
+
+} // namespace cu
+} // namespace ze
diff --git a/RWR/src/ze_oss/imp_cu_correspondence/include/imp/cu_correspondence/solver_stereo_precond_huber_l1.cuh b/RWR/src/ze_oss/imp_cu_correspondence/include/imp/cu_correspondence/solver_stereo_precond_huber_l1.cuh
new file mode 100644
index 0000000000000000000000000000000000000000..579be88de0c9c1c135a271e797c6b2d2eba5a96b
--- /dev/null
+++ b/RWR/src/ze_oss/imp_cu_correspondence/include/imp/cu_correspondence/solver_stereo_precond_huber_l1.cuh
@@ -0,0 +1,87 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#ifndef IMP_CU_STEREO_CTF_WARPING_LEVEL_PRECOND_HUBER_L1_CUH
+#define IMP_CU_STEREO_CTF_WARPING_LEVEL_PRECOND_HUBER_L1_CUH
+
+#include <cstdint>
+#include <memory>
+
+#include <imp/cu_correspondence/solver_stereo_abstract.hpp>
+#include <imp/cu_core/cu_image_gpu.cuh>
+#include <imp/core/size.hpp>
+
+namespace ze {
+namespace cu {
+
+// forward decl
+class Texture2D;
+
+/**
+ * @brief The StereoCtFWarpingLevelPrecondHuberL1 class
+ */
+class SolverStereoPrecondHuberL1 : public SolverStereoAbstract
+{
+public:
+  SolverStereoPrecondHuberL1() = delete;
+  virtual ~SolverStereoPrecondHuberL1();
+
+  SolverStereoPrecondHuberL1(const Parameters::Ptr& params,
+                                      ze::Size2u size, size_t level);
+
+  virtual void init() override;
+  virtual void init(const SolverStereoAbstract& rhs) override;
+  virtual void solve(std::vector<ImageGpu32fC1::Ptr> images) override;
+
+  virtual inline ImageGpu32fC1::Ptr getDisparities() override {return u_;}
+
+
+protected:
+  ImageGpu32fC1::Ptr u_; //!< disparities (result)
+  std::unique_ptr<ImageGpu32fC1> u_prev_; //!< disparities results from previous iteration
+  std::unique_ptr<ImageGpu32fC1> u0_; //!< disparities results from previous warp
+  std::unique_ptr<ImageGpu32fC2> pu_; //!< dual variable for primal variable
+  std::unique_ptr<ImageGpu32fC1> q_; //!< dual variable for data term
+  std::unique_ptr<ImageGpu32fC1> ix_; //!< spatial gradients on moving (warped) image
+  std::unique_ptr<ImageGpu32fC1> it_; //!< temporal gradients between warped and fixed image
+  std::unique_ptr<ImageGpu32fC1> xi_; //!< preconditioner
+
+  // textures
+  std::shared_ptr<Texture2D> i1_tex_;
+  std::shared_ptr<Texture2D> i2_tex_;
+  std::shared_ptr<Texture2D> u_tex_;
+  std::shared_ptr<Texture2D> u_prev_tex_;
+  std::shared_ptr<Texture2D> u0_tex_;
+  std::shared_ptr<Texture2D> pu_tex_;
+  std::shared_ptr<Texture2D> q_tex_;
+  std::shared_ptr<Texture2D> ix_tex_;
+  std::shared_ptr<Texture2D> it_tex_;
+  std::shared_ptr<Texture2D> xi_tex_;
+
+};
+
+} // namespace cu
+} // namespace ze
+
+#endif // IMP_CU_STEREO_CTF_WARPING_LEVEL_PRECOND_HUBER_L1_CUH
diff --git a/RWR/src/ze_oss/imp_cu_correspondence/include/imp/cu_correspondence/solver_stereo_precond_huber_l1_weighted.cuh b/RWR/src/ze_oss/imp_cu_correspondence/include/imp/cu_correspondence/solver_stereo_precond_huber_l1_weighted.cuh
new file mode 100644
index 0000000000000000000000000000000000000000..19d95e87514b49b7b7d18a9abea48ccdf868035b
--- /dev/null
+++ b/RWR/src/ze_oss/imp_cu_correspondence/include/imp/cu_correspondence/solver_stereo_precond_huber_l1_weighted.cuh
@@ -0,0 +1,96 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#ifndef IMP_CU_STEREO_CTF_WARPING_LEVEL_PRECOND_HUBER_L1_WEIGHTED_CUH
+#define IMP_CU_STEREO_CTF_WARPING_LEVEL_PRECOND_HUBER_L1_WEIGHTED_CUH
+
+#include <cstdint>
+#include <memory>
+
+#include <imp/cu_correspondence/solver_stereo_abstract.hpp>
+#include <imp/cu_core/cu_image_gpu.cuh>
+#include <imp/core/size.hpp>
+
+namespace ze {
+namespace cu {
+
+// forward decl
+class Texture2D;
+
+/**
+ * @brief The StereoCtFWarpingLevelPrecondHuberL1 class
+ * @todo better polymorphism! (derive from StereoCtfWarpingLevelPrecondHuberL1
+ */
+class SolverStereoPrecondHuberL1Weighted : public SolverStereoAbstract
+{
+public:
+  SolverStereoPrecondHuberL1Weighted() = delete;
+  virtual ~SolverStereoPrecondHuberL1Weighted();
+
+  SolverStereoPrecondHuberL1Weighted(
+      const Parameters::Ptr& params,
+      ze::Size2u size, size_t level);
+
+  virtual void init() override;
+  virtual void init(const SolverStereoAbstract& rhs) override;
+  virtual void solve(std::vector<ImageGpu32fC1::Ptr> images) override;
+
+  virtual ImageGpu32fC1::Ptr computePrimalEnergy() override;
+
+  virtual inline ImageGpu32fC1::Ptr getDisparities() override {return u_;}
+  virtual inline ImageGpu32fC1::Ptr getOcclusion() override {return occ_;}
+
+protected:
+  ImageGpu32fC1::Ptr u_; //!< disparities (result)
+  std::unique_ptr<ImageGpu32fC1> u_prev_; //!< disparities results from previous iteration
+  std::shared_ptr<ImageGpu32fC1> u0_; //!< disparities results from previous warp
+  std::unique_ptr<ImageGpu32fC2> pu_; //!< dual variable for primal variable
+  std::unique_ptr<ImageGpu32fC1> q_; //!< dual variable for data term
+  std::unique_ptr<ImageGpu32fC1> ix_; //!< spatial gradients on moving (warped) image
+  std::unique_ptr<ImageGpu32fC1> it_; //!< temporal gradients between warped and fixed image
+  std::unique_ptr<ImageGpu32fC1> xi_; //!< preconditioner
+  std::unique_ptr<ImageGpu32fC1> g_; //!< (edge) image for weighting the regularizer
+  ze::cu::ImageGpu32fC1::Ptr occ_; //!< estimation of occluded pixels
+
+  // textures
+  std::shared_ptr<Texture2D> lambda_tex_; //!< For pointwise lambda
+  std::shared_ptr<Texture2D> i1_tex_;
+  std::shared_ptr<Texture2D> i2_tex_;
+  std::shared_ptr<Texture2D> u_tex_;
+  std::shared_ptr<Texture2D> u_prev_tex_;
+  std::shared_ptr<Texture2D> u0_tex_;
+  std::shared_ptr<Texture2D> pu_tex_;
+  std::shared_ptr<Texture2D> q_tex_;
+  std::shared_ptr<Texture2D> ix_tex_;
+  std::shared_ptr<Texture2D> it_tex_;
+  std::shared_ptr<Texture2D> xi_tex_;
+  std::shared_ptr<Texture2D> g_tex_;
+  std::shared_ptr<Texture2D> occ_tex_;
+
+};
+
+} // namespace cu
+} // namespace ze
+
+#endif // IMP_CU_STEREO_CTF_WARPING_LEVEL_PRECOND_HUBER_L1_WEIGHTED_CUH
diff --git a/RWR/src/ze_oss/imp_cu_correspondence/include/imp/cu_correspondence/stereo_ctf_warping.hpp b/RWR/src/ze_oss/imp_cu_correspondence/include/imp/cu_correspondence/stereo_ctf_warping.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..4223bf9c7e1be03432b4ffea8b4a3467bb999fa5
--- /dev/null
+++ b/RWR/src/ze_oss/imp_cu_correspondence/include/imp/cu_correspondence/stereo_ctf_warping.hpp
@@ -0,0 +1,118 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#pragma once
+
+#include <memory>
+#include <vector>
+
+#include <imp/cu_core/cu_image_gpu.cuh>
+#include <imp/cu_imgproc/image_pyramid.hpp>
+#include <imp/cu_core/cu_pinhole_camera.cuh>
+#include <imp/cu_core/cu_se3.cuh>
+#include <imp/cu_core/cu_matrix.cuh>
+
+#include <imp/cu_correspondence/variational_stereo_parameters.hpp>
+
+
+namespace ze {
+namespace cu {
+
+// forward declarations
+class SolverStereoAbstract;
+
+/**
+ * @brief The StereoCtFWarping class
+ * @todo (MWE) better handling of fixed vs. moving images when adding (incremental updates)
+ * @todo (MWE) better interface for multiple input images with fundamental matrix prior
+ */
+class StereoCtFWarping
+{
+public:
+  using Parameters = VariationalStereoParameters;
+
+  using ImageGpu32fC1 = ze::cu::ImageGpu32fC1;
+  using ImageGpu32fC2 = ze::cu::ImageGpu32fC2;
+  using ImagePyramid32fC1 = ze::ImagePyramid32fC1;
+
+  using Cameras = std::vector<cu::PinholeCamera>;
+  using CamerasPyramid = std::vector<Cameras>;
+
+public:
+  StereoCtFWarping() = delete;
+  virtual ~StereoCtFWarping();
+
+  StereoCtFWarping(Parameters::Ptr params);
+  StereoCtFWarping(ze::Size2u image_size, uint8_t num_images,
+                   Parameters::Ptr params);
+
+  void addImage(const ImageGpu32fC1::Ptr& image);
+  void reset();
+  void solve();
+  ImageGpu32fC1::Ptr computePrimalEnergy(size_t level=0);
+  ImageGpu32fC1::Ptr getDisparities(size_t level=0);
+  ImageGpu32fC1::Ptr getOcclusion(size_t level=0);
+
+  // if we have a guess about the correspondence points and the epipolar geometry
+  // given we can set these as a prior
+  inline virtual void setFundamentalMatrix(const cu::Matrix3f& F) {F_ = F;}
+  virtual void setIntrinsics(const Cameras& cams) {cams_ = cams;}
+  virtual void setExtrinsics(const cu::SE3<float>& T_mov_fix) {T_mov_fix_=T_mov_fix;}
+
+  inline virtual void setDepthProposal(
+      const ImageGpu32fC1::Ptr& depth_proposal,
+      const ImageGpu32fC1::Ptr& depth_proposal_sigma2=nullptr)
+  {
+    depth_proposal_ = depth_proposal;
+    depth_proposal_sigma2_ = depth_proposal_sigma2;
+  }
+
+protected:
+  /**
+   * @brief ready checks if everything is setup and initialized.
+   * @return State if everything is ready to solve the given problem.
+   */
+  bool ready();
+
+  /**
+   * @brief init initializes the solvers for the current setup
+   */
+  void init();
+
+private:
+  Parameters::Ptr params_; //!< configuration parameters
+  std::vector<ImageGpu32fC1::Ptr> images_; //!< all unprocessed input images
+  std::vector<ImagePyramid32fC1::Ptr> image_pyramids_; //!< image pyramids corresponding to the unprocesed input images
+  std::vector<std::unique_ptr<SolverStereoAbstract>> levels_;
+
+  cu::Matrix3f F_;
+  std::vector<cu::PinholeCamera> cams_;
+  cu::SE3<float> T_mov_fix_;
+
+  ImageGpu32fC1::Ptr depth_proposal_;
+  ImageGpu32fC1::Ptr depth_proposal_sigma2_;
+};
+
+} // namespace cu
+} // namespace ze
diff --git a/RWR/src/ze_oss/imp_cu_correspondence/include/imp/cu_correspondence/stereo_solver_enum.hpp b/RWR/src/ze_oss/imp_cu_correspondence/include/imp/cu_correspondence/stereo_solver_enum.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..70b9b7d28effbda9cde9e264139aadc861761dad
--- /dev/null
+++ b/RWR/src/ze_oss/imp_cu_correspondence/include/imp/cu_correspondence/stereo_solver_enum.hpp
@@ -0,0 +1,40 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#pragma once
+
+namespace ze {
+namespace cu {
+
+enum class StereoPDSolver
+{
+  HuberL1, //!< Huber regularization + pointwise L1 intensity matching costs
+  PrecondHuberL1, //!< Huber regularization + pointwise L1 intensity matching costs
+  PrecondHuberL1Weighted, //!< weighted Huber regularization + pointwise L1 intensity matching costs
+  EpipolarPrecondHuberL1 //!< Huber regularization + pointwise L1 intensity matching costs applied on generic images with known epipolar geometry (not rectified)
+};
+
+} // namespace cu
+} // namespace ze
+
diff --git a/RWR/src/ze_oss/imp_cu_correspondence/include/imp/cu_correspondence/variational_epipolar_stereo.hpp b/RWR/src/ze_oss/imp_cu_correspondence/include/imp/cu_correspondence/variational_epipolar_stereo.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..e9d882bb339909cb5bce15bdf7da3086a2730113
--- /dev/null
+++ b/RWR/src/ze_oss/imp_cu_correspondence/include/imp/cu_correspondence/variational_epipolar_stereo.hpp
@@ -0,0 +1,66 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#pragma once
+
+#include <cstdint>
+#include <memory>
+
+#include <imp/cu_core/cu_matrix.cuh>
+#include <imp/cu_core/cu_se3.cuh>
+#include <imp/cu_core/cu_pinhole_camera.cuh>
+#include <imp/cu_core/cu_image_gpu.cuh>
+#include <imp/cu_correspondence/variational_stereo_parameters.hpp>
+#include <imp/cu_correspondence/variational_stereo.hpp>
+
+namespace ze {
+namespace cu {
+
+/**
+ * @brief The Stereo class takes an image pair with known epipolar geometry
+ *        (fundamental matrix) and estimates the disparity map
+ */
+class VariationalEpipolarStereo : public VariationalStereo
+{
+public:
+  using VectorImage = ze::cu::ImageGpu32fC2;
+  using Cameras = std::vector<cu::PinholeCamera>;
+
+public:
+  VariationalEpipolarStereo(Parameters::Ptr params=nullptr);
+  virtual ~VariationalEpipolarStereo(); //= default;
+
+  virtual void setFundamentalMatrix(const cu::Matrix3f& F);
+  virtual void setIntrinsics(const Cameras& cams);
+  virtual void setExtrinsics(const cu::SE3<float>& T_mov_fix);
+  virtual void setDepthProposal(
+      const ImageGpu32fC1::Ptr& depth_proposal,
+      const ImageGpu32fC1::Ptr& depth_proposal_sigma2=nullptr);
+
+private:
+};
+
+} // namespace cu
+} // namespace ze
+
diff --git a/RWR/src/ze_oss/imp_cu_correspondence/include/imp/cu_correspondence/variational_stereo.hpp b/RWR/src/ze_oss/imp_cu_correspondence/include/imp/cu_correspondence/variational_stereo.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..a7ee193f84d2088c9cde41b1ef75935941486662
--- /dev/null
+++ b/RWR/src/ze_oss/imp_cu_correspondence/include/imp/cu_correspondence/variational_stereo.hpp
@@ -0,0 +1,70 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#pragma once
+
+#include <cstdint>
+#include <memory>
+
+#include <imp/cu_core/cu_image_gpu.cuh>
+#include <imp/cu_correspondence/variational_stereo_parameters.hpp>
+
+namespace ze {
+namespace cu {
+
+// forward declarations
+class StereoCtFWarping;
+
+/**
+ * @brief The Stereo class takes a stereo image pair and estimates the disparity map
+ */
+class VariationalStereo
+{
+public:
+  //! @todo (MWE) first do the implementation with specific type (32fC1) and later generalize
+  using ImageGpu32fC1 = ze::cu::ImageGpu32fC1;
+  using Parameters = VariationalStereoParameters;
+
+public:
+  VariationalStereo(Parameters::Ptr params=nullptr);
+  virtual ~VariationalStereo(); //= default;
+
+  virtual void addImage(const ImageGpu32fC1::Ptr& image);
+  virtual void reset();
+  virtual void solve();
+
+  virtual ImageGpu32fC1::Ptr computePrimalEnergy(size_t level=0);
+  virtual ImageGpu32fC1::Ptr getDisparities(size_t level=0);
+  virtual ImageGpu32fC1::Ptr getOcclusion(size_t level=0);
+
+  // getters / setters
+  virtual inline Parameters::Ptr parameters() {return params_;}
+
+protected:
+  Parameters::Ptr params_;  //!< configuration parameters
+  std::unique_ptr<StereoCtFWarping> ctf_;  //!< performing a coarse-to-fine warping scheme
+};
+
+} // namespace cu
+} // namespace ze
diff --git a/RWR/src/ze_oss/imp_cu_correspondence/include/imp/cu_correspondence/variational_stereo_parameters.hpp b/RWR/src/ze_oss/imp_cu_correspondence/include/imp/cu_correspondence/variational_stereo_parameters.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..7c80b2b1e662da14872ab2945701eba35da1a099
--- /dev/null
+++ b/RWR/src/ze_oss/imp_cu_correspondence/include/imp/cu_correspondence/variational_stereo_parameters.hpp
@@ -0,0 +1,68 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#pragma once
+
+#include <ze/common/types.hpp>
+#include <ze/common/macros.hpp>
+#include <imp/core/types.hpp>
+#include <imp/cu_correspondence/stereo_solver_enum.hpp>
+#include <imp/cu_core/cu_image_gpu.cuh>
+
+namespace ze {
+namespace cu {
+
+// the parameter struct
+struct VariationalStereoParameters
+{
+  ZE_POINTER_TYPEDEFS(VariationalStereoParameters);
+
+  StereoPDSolver solver=StereoPDSolver::PrecondHuberL1; //!< selected primal-dual solver / model combination
+  float lambda = 30.0f; //!< tradeoff between regularization and matching term (R(u) + \lambda * D(u))
+  ImageGpu32fC1::Ptr lambda_pointwise = nullptr; //!< pointwise variant of lambda
+  float eps_u = 0.05f; //!< tradeoff between L1 and L2 part of the Huber regularization
+
+  float edge_sigma = 1.f;
+  float edge_alpha = 7.f;
+  float edge_q = 0.7f;
+
+  //! @todo (MWE) we might want to define this externally for all ctf approaches?
+  // settings for the ctf warping
+  struct CTF
+  {
+    float scale_factor = 0.8f; //!< multiplicative scale factor between coarse-to-fine pyramid levels
+    uint32_t iters = 100;
+    uint32_t warps =  10;
+    size_t levels = UINT32_MAX;
+    size_t coarsest_level = UINT32_MAX;
+    size_t finest_level = 0;
+    bool apply_median_filter = true;
+  } ctf;
+
+  friend std::ostream& operator<<(std::ostream& stream,
+                                  const VariationalStereoParameters& p);
+};
+
+} // namespace cu
+} // namespace ze
diff --git a/RWR/src/ze_oss/imp_cu_correspondence/package.xml b/RWR/src/ze_oss/imp_cu_correspondence/package.xml
new file mode 100644
index 0000000000000000000000000000000000000000..d829c1ad5a6021b8e20769c7bad848c3670b110e
--- /dev/null
+++ b/RWR/src/ze_oss/imp_cu_correspondence/package.xml
@@ -0,0 +1,26 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>imp_cu_correspondence</name>
+  <description>
+    IMP CUDA depth estimation / range image module
+  </description>
+  <version>0.1.4</version>
+  <license>ZE</license>
+
+  <maintainer email="code@werlberger.org">Manuel Werlberger</maintainer>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <buildtool_depend>catkin_simple</buildtool_depend>
+
+  <depend>glog_catkin</depend>
+  <depend>ze_cmake</depend>
+  <depend>ze_common</depend>
+  <depend>ze_cameras</depend>
+  <depend>ze_geometry</depend>
+  <depend>imp_core</depend>
+  <depend>imp_cu_core</depend>
+  <depend>imp_3rdparty_cuda_toolkit</depend>
+  <depend>imp_cu_imgproc</depend>
+
+  <test_depend>gtest</test_depend>
+</package>
diff --git a/RWR/src/ze_oss/imp_cu_correspondence/src/occlusion.cu b/RWR/src/ze_oss/imp_cu_correspondence/src/occlusion.cu
new file mode 100644
index 0000000000000000000000000000000000000000..74906ca17d0fcd6a8801900be2987fc3d97753c5
--- /dev/null
+++ b/RWR/src/ze_oss/imp_cu_correspondence/src/occlusion.cu
@@ -0,0 +1,92 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#include <imp/cu_correspondence/occlusion.cuh>
+
+#include <cuda_runtime_api.h>
+#include <imp/cu_core/cu_utils.hpp>
+#include <imp/cu_core/cu_texture.cuh>
+
+namespace ze {
+namespace cu {
+
+//------------------------------------------------------------------------------
+__global__ void k_occlusionCandidatesUniqunessMapping(
+    float* occ, size_t stride, uint32_t width, uint32_t height,
+    Texture2D disp_tex)
+{
+  const int x = blockIdx.x*blockDim.x + threadIdx.x /*+ roi_x*/;
+  const int y = blockIdx.y*blockDim.y + threadIdx.y /*+ roi_y*/;
+
+  const int wx = x + static_cast<int>(tex2DFetch<float>(disp_tex, x, y) + 0.5f);
+
+  if (wx>0 && wx<width && y<height)
+  {
+    atomicAdd(&occ[y*stride+wx], 1.f);
+  }
+}
+
+template<typename Pixel>
+__host__ __device__ Pixel clamp(const Pixel& in, const Pixel& low, const Pixel& high)
+{
+  return max(low, min(high, in));
+}
+
+//------------------------------------------------------------------------------
+__global__ void k_clampOcclusion(
+    float* occ, size_t stride, uint32_t width, uint32_t height,
+    Texture2D occ_tex)
+{
+  const int x = blockIdx.x*blockDim.x + threadIdx.x /*+ roi_x*/;
+  const int y = blockIdx.y*blockDim.y + threadIdx.y /*+ roi_y*/;
+  if (x<width && y<height)
+  {
+    occ[y*stride+x] = 1.f - clamp(tex2DFetch<float>(occ_tex, x, y)-1.f, 0.f, 1.f);
+  }
+}
+
+//------------------------------------------------------------------------------
+void occlusionCandidatesUniqunessMapping(
+    ImageGpu32fC1::Ptr occ,
+    const ImageGpu32fC1::Ptr& disp)
+{
+  occ->setValue(0.0f);
+  std::shared_ptr<ze::cu::Texture2D> disp_tex = disp->genTexture();
+  ze::cu::Fragmentation<> frag(disp->size());
+  k_occlusionCandidatesUniqunessMapping
+      <<<
+        frag.dimGrid, frag.dimBlock
+      >>> (occ->cuData(), occ->stride(), occ->width(), occ->height(),
+           *disp_tex);
+
+  // clamp the occlusions to get a nice mask with 0 and 1 entries
+  std::shared_ptr<Texture2D> occ_tex = occ->genTexture();
+  k_clampOcclusion
+      <<<
+        frag.dimGrid, frag.dimBlock
+      >>> (occ->cuData(), occ->stride(), occ->width(), occ->height(), *occ_tex);
+}
+
+} // namespace cu
+} // namespace ze
diff --git a/RWR/src/ze_oss/imp_cu_correspondence/src/occlusion_kernel.cuh b/RWR/src/ze_oss/imp_cu_correspondence/src/occlusion_kernel.cuh
new file mode 100644
index 0000000000000000000000000000000000000000..bedd556295946f0fe73e855a173519bae94bcc68
--- /dev/null
+++ b/RWR/src/ze_oss/imp_cu_correspondence/src/occlusion_kernel.cuh
@@ -0,0 +1,78 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#ifndef IMP_CU_OCCLUSION_KERNEL
+#define IMP_CU_OCCLUSION_KERNEL
+
+#include <cuda_runtime_api.h>
+#include <imp/core/types.hpp>
+#include <imp/cu_core/cu_utils.hpp>
+#include <imp/cu_core/cu_k_derivative.cuh>
+#include <imp/cuda_toolkit/helper_math.hpp>
+
+
+namespace imp {
+namespace cu {
+
+
+//------------------------------------------------------------------------------
+__global__ void k_occlusionCandidatesUniqunessMapping(
+    float* occ, size_t stride, std::uint32_t width, std::uint32_t height,
+    Texture2D disp_tex)
+{
+  const int x = blockIdx.x*blockDim.x + threadIdx.x /*+ roi_x*/;
+  const int y = blockIdx.y*blockDim.y + threadIdx.y /*+ roi_y*/;
+
+  const int wx = x + static_cast<int>(disp_tex.fetch<float>(x,y) + 0.5f);
+
+  if (wx>0 && wx<width && y<height)
+  {
+    atomicAdd(&occ[y*stride+wx], 1.f);
+  }
+}
+
+template<typename Pixel>
+__host__ __device__ Pixel clamp(const Pixel& in, const Pixel& low, const Pixel& high)
+{
+  return max(low, min(high, in));
+}
+
+//------------------------------------------------------------------------------
+__global__ void k_clampOcclusion(
+    float* occ, size_t stride, std::uint32_t width, std::uint32_t height,
+    Texture2D occ_tex)
+{
+  const int x = blockIdx.x*blockDim.x + threadIdx.x /*+ roi_x*/;
+  const int y = blockIdx.y*blockDim.y + threadIdx.y /*+ roi_y*/;
+  if (x<width && y<height)
+  {
+    occ[y*stride+x] = 1.f - clamp(occ_tex.fetch<float>(x,y)-1.f, 0.f, 1.f);
+  }
+}
+
+
+}
+}
+
+#endif
diff --git a/RWR/src/ze_oss/imp_cu_correspondence/src/solver_epipolar_stereo_precond_huber_l1.cu b/RWR/src/ze_oss/imp_cu_correspondence/src/solver_epipolar_stereo_precond_huber_l1.cu
new file mode 100644
index 0000000000000000000000000000000000000000..3c5a615455eae056bfbb8a3f238512adcaaf2e79
--- /dev/null
+++ b/RWR/src/ze_oss/imp_cu_correspondence/src/solver_epipolar_stereo_precond_huber_l1.cu
@@ -0,0 +1,296 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#include <imp/cu_correspondence/solver_epipolar_stereo_precond_huber_l1.cuh>
+
+#include <cuda_runtime.h>
+
+#include <glog/logging.h>
+
+#include <imp/cu_correspondence/variational_stereo_parameters.hpp>
+#include <imp/cu_core/cu_image_gpu.cuh>
+#include <imp/cu_imgproc/cu_image_filter.cuh>
+#include <imp/cu_imgproc/cu_resample.cuh>
+#include <imp/cu_core/cu_utils.hpp>
+#include <imp/cu_core/cu_texture.cuh>
+#include <imp/cu_core/cu_math.cuh>
+#include <imp/cu_core/cu_k_setvalue.cuh>
+#include <imp/cu_imgproc/edge_detectors.cuh>
+
+#include "warped_gradients_kernel.cuh"
+#include "solver_precond_huber_l1_kernel.cuh"
+#include "solver_stereo_precond_huber_l1_weighted_kernel.cuh"
+//#include "solver_epipolar_stereo_precond_huber_l1_kernel.cuh"
+
+#define USE_EDGES 1
+
+namespace ze {
+namespace cu {
+
+//------------------------------------------------------------------------------
+SolverEpipolarStereoPrecondHuberL1::~SolverEpipolarStereoPrecondHuberL1()
+{
+  // thanks to smart pointers
+}
+
+//------------------------------------------------------------------------------
+SolverEpipolarStereoPrecondHuberL1::SolverEpipolarStereoPrecondHuberL1(
+    const std::shared_ptr<Parameters>& params, ze::Size2u size, size_t level,
+    const std::vector<cu::PinholeCamera>& cams,
+    const cu::Matrix3f& F,
+    const cu::SE3<float>& T_mov_fix,
+    const ze::cu::ImageGpu32fC1& depth_proposal,
+    const ze::cu::ImageGpu32fC1& depth_proposal_sigma2)
+  : SolverStereoAbstract(params, size, level)
+{
+  u_.reset(new ImageGpu32fC1(size));
+  u_prev_.reset(new ImageGpu32fC1(size));
+  u0_.reset(new ImageGpu32fC1(size));
+  pu_.reset(new ImageGpu32fC2(size));
+  q_.reset(new ImageGpu32fC1(size));
+  iw_.reset(new ImageGpu32fC1(size));
+  ix_.reset(new ImageGpu32fC1(size));
+  it_.reset(new ImageGpu32fC1(size));
+  xi_.reset(new ImageGpu32fC1(size));
+  g_.reset(new ImageGpu32fC1(size));
+
+  depth_proposal_.reset(new ImageGpu32fC1(size));
+  depth_proposal_sigma2_.reset(new ImageGpu32fC1(size));
+
+  u_tex_ = u_->genTexture(false, cudaFilterModeLinear);
+  u_prev_tex_ =  u_prev_->genTexture(false, cudaFilterModeLinear);
+  u0_tex_ =  u0_->genTexture(false, cudaFilterModeLinear);
+  pu_tex_ =  pu_->genTexture(false, cudaFilterModeLinear);
+  q_tex_ =  q_->genTexture(false, cudaFilterModeLinear);
+  ix_tex_ =  ix_->genTexture(false, cudaFilterModeLinear);
+  it_tex_ =  it_->genTexture(false, cudaFilterModeLinear);
+  xi_tex_ =  xi_->genTexture(false, cudaFilterModeLinear);
+  g_tex_ =  g_->genTexture(false, cudaFilterModeLinear);
+  depth_proposal_tex_ =  depth_proposal_->genTexture(false, cudaFilterModeLinear);
+  depth_proposal_sigma2_tex_ =  depth_proposal_sigma2_->genTexture(false, cudaFilterModeLinear);
+
+
+  float scale_factor = std::pow(params->ctf.scale_factor, level);
+
+  if (depth_proposal.size() == size)
+  {
+    VLOG(10) << "Copy depth proposals " << depth_proposal.size() << " to level0 "
+             << depth_proposal_->size();
+    depth_proposal.copyTo(*depth_proposal_);
+    depth_proposal_sigma2.copyTo(*depth_proposal_sigma2_);
+  }
+  else
+  {
+    float downscale_factor = 0.5f*((float)size.width()/(float)depth_proposal.width()+
+                                   (float)size.height()/(float)depth_proposal.height());
+
+    VLOG(10) << "depth proposal downscaled to level: " << level << "; size: " << size
+             << "; downscale_factor: " << downscale_factor;
+
+    ze::cu::resample(*depth_proposal_, depth_proposal);
+    ze::cu::resample(*depth_proposal_sigma2_, depth_proposal_sigma2);
+  }
+
+  F_ = F;
+  T_mov_fix_ = T_mov_fix;
+
+  // assuming we receive the camera matrix for level0
+  if  (level == 0)
+  {
+    cams_ = cams;
+  }
+  else
+  {
+    for (auto cam : cams)
+    {
+      cu::PinholeCamera scaled_cam = cam * scale_factor;
+      cams_.push_back(scaled_cam);
+    }
+  }
+}
+
+//------------------------------------------------------------------------------
+void SolverEpipolarStereoPrecondHuberL1::init()
+{
+  u_->setValue(0.0f);
+  pu_->setValue(0.0f);
+  q_->setValue(0.0f);
+  // other variables are init and/or set when needed!
+}
+
+//------------------------------------------------------------------------------
+void SolverEpipolarStereoPrecondHuberL1::init(const SolverStereoAbstract& rhs)
+{
+  const SolverEpipolarStereoPrecondHuberL1* from =
+      dynamic_cast<const SolverEpipolarStereoPrecondHuberL1*>(&rhs);
+
+  float inv_sf = 1./params_->ctf.scale_factor; // >1 for adapting prolongated disparities
+
+  if(params_->ctf.apply_median_filter)
+  {
+    ze::cu::filterMedian3x3(*from->u0_, *from->u_);
+    ze::cu::resample(*u_, *from->u0_, ze::InterpolationMode::Point, false);
+  }
+  else
+  {
+    ze::cu::resample(*u_, *from->u_, ze::InterpolationMode::Point, false);
+  }
+  *u_ *= inv_sf;
+
+  ze::cu::resample(*pu_, *from->pu_, ze::InterpolationMode::Point, false);
+  ze::cu::resample(*q_, *from->q_, ze::InterpolationMode::Point, false);
+}
+
+//------------------------------------------------------------------------------
+void SolverEpipolarStereoPrecondHuberL1::solve(std::vector<ImageGpu32fC1::Ptr> images)
+{
+  VLOG(100) << "SolverEpipolarStereoPrecondHuberL1: solving level " << level_ << " with " << images.size() << " images";
+
+  // sanity check:
+  // TODO
+
+
+  // image textures
+  i1_tex_ = images.at(0)->genTexture(false, cudaFilterModeLinear);
+  i2_tex_ = images.at(1)->genTexture(false, cudaFilterModeLinear);
+
+
+  // constants
+  constexpr float tau = 0.95f;
+  constexpr float sigma = 0.95f;
+  float lin_step = 0.5f;
+  Fragmentation<> frag(size_);
+  constexpr float eta = 2.0f;
+
+  // init
+  u_->copyTo(*u_prev_);
+
+
+  // check if a pointwise lambda is set in the parameters. otherwise we create
+  // a local one to simplify kernel interfaces
+  cu::ImageGpu32fC1::Ptr lambda;
+  if (params_->lambda_pointwise)
+  {
+    lambda = params_->lambda_pointwise;
+  }
+  else
+  {
+    // make it as small as possible to reduce memory overhead. access is then
+    // handled by the texture
+    lambda.reset(new ImageGpu32fC1(1,1));
+    lambda->setValue(params_->lambda);
+  }
+  lambda_tex_ = lambda->genTexture(false,cudaFilterModePoint,
+                                   cudaAddressModeClamp, cudaReadModeElementType);
+
+  // compute edge weight
+  ze::cu::naturalEdges(*g_, *images.at(0),
+                        params_->edge_sigma, params_->edge_alpha, params_->edge_q);
+
+  // warping
+  for (uint32_t warp = 0; warp < params_->ctf.warps; ++warp)
+  {
+    VLOG(100) << "SOLVING warp iteration of Huber-L1 stereo model." << std::endl;
+
+    u_->copyTo(*u0_);
+
+
+    // compute warped spatial and temporal gradients
+    k_warpedGradientsEpipolarConstraint
+        <<<
+          frag.dimGrid, frag.dimBlock
+        >>> (iw_->data(), ix_->data(), it_->data(), ix_->stride(), ix_->width(), ix_->height(),
+             cams_.at(0), cams_.at(1), F_, T_mov_fix_,
+             *i1_tex_, *i2_tex_, *u0_tex_,
+             *depth_proposal_tex_);
+
+    // compute preconditioner
+#if USE_EDGES
+    k_preconditionerWeighted
+        <<<
+          frag.dimGrid, frag.dimBlock
+        >>> (xi_->data(), xi_->stride(), xi_->width(), xi_->height(),
+             *lambda_tex_, *ix_tex_, *g_tex_);
+#else
+    k_preconditioner
+        <<<
+          frag.dimGrid, frag.dimBlock
+        >>> (xi_->data(), xi_->stride(), xi_->width(), xi_->height(),
+             params_->lambda, *ix_tex_);
+#endif
+
+    for (uint32_t iter = 0; iter < params_->ctf.iters; ++iter)
+    {
+#if USE_EDGES
+      // dual update kernel
+      k_dualUpdateWeighted
+          <<<
+            frag.dimGrid, frag.dimBlock
+          >>> (pu_->data(), pu_->stride(), q_->data(), q_->stride(),
+               size_.width(), size_.height(),
+               params_->eps_u, sigma, eta, *lambda_tex_,
+               *u_prev_tex_, *u0_tex_, *pu_tex_, *q_tex_, *ix_tex_, *it_tex_, *g_tex_);
+
+      // and primal update kernel
+      k_primalUpdateWeighted
+          <<<
+            frag.dimGrid, frag.dimBlock
+          >>> (u_->data(), u_prev_->data(), u_->stride(),
+               size_.width(), size_.height(),
+               tau, lin_step, *lambda_tex_,
+               *u_tex_, *u0_tex_, *pu_tex_, *q_tex_, *ix_tex_, *xi_tex_, *g_tex_);
+#else
+      // dual update kernel
+      k_dualUpdate
+          <<<
+            frag.dimGrid, frag.dimBlock
+          >>> (pu_->data(), pu_->stride(), q_->data(), q_->stride(),
+               size_.width(), size_.height(),
+               params_->eps_u, sigma, eta, *lambda_tex_,
+               *u_prev_tex_, *u0_tex_, *pu_tex_, *q_tex_, *ix_tex_, *it_tex_);
+
+      // and primal update kernel
+      k_primalUpdate
+          <<<
+            frag.dimGrid, frag.dimBlock
+          >>> (u_->data(), u_prev_->data(), u_->stride(),
+               size_.width(), size_.height(),
+               tau, lin_step, *lambda_tex_,
+               *u_tex_, *u0_tex_, *pu_tex_, *q_tex_, *ix_tex_, *xi_tex_);
+#endif
+    } // iters
+    lin_step /= 1.2f;
+
+  } // warps
+
+
+
+  IMP_CUDA_CHECK();
+}
+
+
+
+} // namespace cu
+} // namespace ze
+
diff --git a/RWR/src/ze_oss/imp_cu_correspondence/src/solver_epipolar_stereo_precond_huber_l1_kernel.cuh b/RWR/src/ze_oss/imp_cu_correspondence/src/solver_epipolar_stereo_precond_huber_l1_kernel.cuh
new file mode 100644
index 0000000000000000000000000000000000000000..ab3a48b5a0fa459c46f5a97ac840d06811862efe
--- /dev/null
+++ b/RWR/src/ze_oss/imp_cu_correspondence/src/solver_epipolar_stereo_precond_huber_l1_kernel.cuh
@@ -0,0 +1,150 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#ifndef IMP_CU_K_EPIPOLAR_STEREO_PRECOND_HUBER_L1_CUH
+#define IMP_CU_K_EPIPOLAR_STEREO_PRECOND_HUBER_L1_CUH
+
+#include <cuda_runtime_api.h>
+#include <imp/core/types.hpp>
+#include <imp/cu_core/cu_utils.hpp>
+#include <imp/cu_core/cu_k_derivative.cuh>
+#include <imp/cuda_toolkit/helper_math.hpp>
+
+
+namespace ze {
+namespace cu {
+
+
+//-----------------------------------------------------------------------------
+template<typename Pixel>
+__global__ void k_preconditioner(Pixel* xi, size_t stride,
+                                 uint32_t width, uint32_t height,
+                                 // uint32_t roi_x, uint32_t roi_y,
+                                 float lambda, Texture2D ix_tex)
+{
+  const int x = blockIdx.x*blockDim.x + threadIdx.x;
+  const int y = blockIdx.y*blockDim.y + threadIdx.y;
+
+  if (x<width && y<height)
+  {
+    Pixel ix;
+    tex2DFetch(ix, ix_tex, x, y);
+    xi[y*stride+x] = 4 + sqr(lambda) * sqr(ix);
+  }
+}
+
+//-----------------------------------------------------------------------------
+/** restricts the udpate to +/- lin_step around the given value in lin_tex
+ * @note \a d_srcdst and the return value is identical.
+ * @todo (MWE) move function to common kernel def file for all stereo models
+ */
+template<typename Pixel>
+__device__ Pixel k_linearized_update(Pixel& d_srcdst, Texture2D& lin_tex,
+                                     const float lin_step,
+                                     const int x, const int y)
+{
+  Pixel lin = lin_tex.fetch<Pixel>(x, y);
+  d_srcdst = max(lin-lin_step,
+                 min(lin+lin_step, d_srcdst));
+  return d_srcdst;
+}
+
+//-----------------------------------------------------------------------------
+/**
+ * @brief k_primalUpdate is the Huber-L1-Precondition model's primal update kernel
+ * @note PPixel and DPixel denote for the Pixel type/dimension of primal and dual variable
+ */
+template<typename PPixel>
+__global__ void k_primalUpdate(PPixel* d_u, PPixel* d_u_prev, const size_t stride,
+                               uint32_t width, uint32_t height,
+                               const float lambda, const float tau,
+                               const float lin_step,
+                               Texture2D u_tex, Texture2D u0_tex,
+                               Texture2D pu_tex, Texture2D q_tex,
+                               Texture2D ix_tex, Texture2D xi_tex)
+{
+  const int x = blockIdx.x*blockDim.x + threadIdx.x;
+  const int y = blockIdx.y*blockDim.y + threadIdx.y;
+
+  if (x<width && y<height)
+  {
+    float u_prev = tex2DFetch<float>(u_tex, x, y);
+    float q = tex2DFetch<float>(q_tex, x, y);
+    float ix = tex2DFetch<float>(ix_tex, x, y);
+    float xi = tex2DFetch<float>(xi_tex, x, y);
+
+    float div = dpAd(pu_tex, x, y, width, height);
+
+    float u = u_prev - tau/xi * (-div + lambda*ix*q);
+
+    u = k_linearized_update(u, u0_tex, lin_step, x, y);
+    d_u[y*stride+x] = u;
+    d_u_prev[y*stride+x] = 2.f*u - u_prev;
+  }
+}
+
+//-----------------------------------------------------------------------------
+template<typename PPixel, typename DPixel>
+__global__ void k_dualUpdate(DPixel* d_pu, const size_t stride_pu,
+                             PPixel* d_q, const size_t stride_q,
+                             uint32_t width, uint32_t height,
+                             const float lambda, const float eps_u,
+                             const float sigma, const float eta,
+                             Texture2D u_prev_tex, Texture2D u0_tex,
+                             Texture2D pu_tex, Texture2D q_tex,
+                             Texture2D ix_tex, Texture2D it_tex)
+{
+  const int x = blockIdx.x*blockDim.x + threadIdx.x;
+  const int y = blockIdx.y*blockDim.y + threadIdx.y;
+  if (x<width && y<height)
+  {
+    const float sigma_by_eta = sigma/eta;
+
+    // update pu
+    float2 du = dp(u_prev_tex, x, y);
+    float2 pu = tex2DFetch<float2>(pu_tex, x,y);
+    pu  = (pu + sigma_by_eta*du) / (1.f + sigma_by_eta*eps_u);
+    pu = pu / max(1.0f, length(pu));
+    d_pu[y*stride_pu+x] = {pu.x, pu.y};
+
+    // update q
+    float u_prev = tex2DFetch<float>(u_prev_tex, x, y);
+    float u0 = tex2DFetch<float>(u0_tex, x, y);
+    float q = tex2DFetch<float>(q_tex, x, y);
+    float ix = tex2DFetch<float>(ix_tex, x, y);
+    float it = tex2DFetch<float>(it_tex, x, y);
+    const float sigma_q = sigma / max(1e-6f, lambda * fabs(ix));
+    q = q + lambda*sigma_q * (it + ix*(u_prev-u0));
+    d_q[y*stride_q+x] = max(-1.f, min(1.f, q));
+  }
+}
+
+
+} // namespace cu
+} // namespace ze
+
+
+
+#endif // IMP_CU_K_EPIPOLAR_STEREO_PRECOND_HUBER_L1_CU
+
diff --git a/RWR/src/ze_oss/imp_cu_correspondence/src/solver_precond_huber_l1_kernel.cuh b/RWR/src/ze_oss/imp_cu_correspondence/src/solver_precond_huber_l1_kernel.cuh
new file mode 100644
index 0000000000000000000000000000000000000000..09d1d0d15ab79d17de5f899bb8c16aed3a8c4ab1
--- /dev/null
+++ b/RWR/src/ze_oss/imp_cu_correspondence/src/solver_precond_huber_l1_kernel.cuh
@@ -0,0 +1,223 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#ifndef IMP_CU_K_STEREO_CTF_WARPING_LEVEL_HUBER_CUH
+#define IMP_CU_K_STEREO_CTF_WARPING_LEVEL_HUBER_CUH
+
+#include <cuda_runtime_api.h>
+#include <imp/core/types.hpp>
+#include <imp/cu_core/cu_utils.hpp>
+#include <imp/cu_core/cu_k_derivative.cuh>
+#include <imp/cuda_toolkit/helper_math.hpp>
+
+
+namespace ze {
+namespace cu {
+
+
+//-----------------------------------------------------------------------------
+template<typename Pixel>
+__global__ void k_preconditioner(Pixel* xi, size_t stride,
+                                 uint32_t width, uint32_t height,
+                                 // uint32_t roi_x, uint32_t roi_y,
+                                 float lambda, Texture2D ix_tex)
+{
+  const int x = blockIdx.x*blockDim.x + threadIdx.x;
+  const int y = blockIdx.y*blockDim.y + threadIdx.y;
+
+  if (x<width && y<height)
+  {
+    Pixel ix;
+    tex2DFetch(ix, ix_tex, x, y);
+    xi[y*stride+x] = 4 + fabs(lambda*ix);
+  }
+}
+
+//-----------------------------------------------------------------------------
+/** restricts the udpate to +/- lin_step around the given value in lin_tex
+ * @note \a d_srcdst and the return value is identical.
+ * @todo (MWE) move function to common kernel def file for all stereo models
+ */
+template<typename Pixel>
+__device__ Pixel k_linearized_update(Pixel& d_srcdst, Texture2D& lin_tex,
+                                     const float lin_step,
+                                     const int x, const int y)
+{
+  Pixel lin = tex2DFetch<Pixel>(lin_tex, x, y);
+  d_srcdst = max(lin-lin_step,
+                 min(lin+lin_step, d_srcdst));
+  return d_srcdst;
+}
+
+//-----------------------------------------------------------------------------
+/**
+ * @brief k_primalUpdate is the Huber-L1-Precondition model's primal update kernel
+ * @note PPixel and DPixel denote for the Pixel type/dimension of primal and dual variable
+ */
+template<typename PPixel>
+__global__ void k_primalUpdate(PPixel* d_u, PPixel* d_u_prev, const size_t stride,
+                               uint32_t width, uint32_t height,
+                               const float lambda, const float tau,
+                               const float lin_step,
+                               Texture2D u_tex, Texture2D u0_tex,
+                               Texture2D pu_tex, Texture2D q_tex,
+                               Texture2D ix_tex, Texture2D xi_tex)
+{
+  const int x = blockIdx.x*blockDim.x + threadIdx.x;
+  const int y = blockIdx.y*blockDim.y + threadIdx.y;
+
+  if (x<width && y<height)
+  {
+    float u_prev = tex2DFetch<float>(u_tex, x, y);
+    float q = tex2DFetch<float>(q_tex, x, y);
+    float ix = tex2DFetch<float>(ix_tex, x, y);
+    float xi = tex2DFetch<float>(xi_tex, x, y);
+
+    float div = dpAd(pu_tex, x, y, width, height);
+
+    float u = u_prev - tau/xi * (-div + lambda*ix*q);
+
+    u = k_linearized_update(u, u0_tex, lin_step, x, y);
+    d_u[y*stride+x] = u;
+    d_u_prev[y*stride+x] = 2.f*u - u_prev;
+  }
+}
+
+//-----------------------------------------------------------------------------
+/**
+ * @brief k_primalUpdate is the Huber-L1-Precondition model's primal update kernel
+ * @note PPixel and DPixel denote for the Pixel type/dimension of primal and dual variable
+ */
+template<typename PPixel>
+__global__ void k_primalUpdate(PPixel* d_u, PPixel* d_u_prev, const size_t stride,
+                               uint32_t width, uint32_t height,
+                               const float tau, const float lin_step,
+                               Texture2D lambda_tex,
+                               Texture2D u_tex, Texture2D u0_tex,
+                               Texture2D pu_tex, Texture2D q_tex,
+                               Texture2D ix_tex, Texture2D xi_tex)
+{
+  const int x = blockIdx.x*blockDim.x + threadIdx.x;
+  const int y = blockIdx.y*blockDim.y + threadIdx.y;
+
+  if (x<width && y<height)
+  {
+    float u_prev = tex2DFetch<float>(u_tex, x, y);
+    float q = tex2DFetch<float>(q_tex, x, y);
+    float ix = tex2DFetch<float>(ix_tex, x, y);
+    float xi = tex2DFetch<float>(xi_tex, x, y);
+
+    float div = dpAd(pu_tex, x, y, width, height);
+
+    float lambda = tex2DFetch<float>(lambda_tex, x,y);
+    float u = u_prev - tau/xi * (-div + lambda*ix*q);
+
+    u = k_linearized_update(u, u0_tex, lin_step, x, y);
+    d_u[y*stride+x] = u;
+    d_u_prev[y*stride+x] = 2.f*u - u_prev;
+  }
+}
+
+//-----------------------------------------------------------------------------
+template<typename PPixel, typename DPixel>
+__global__ void k_dualUpdate(DPixel* d_pu, const size_t stride_pu,
+                             PPixel* d_q, const size_t stride_q,
+                             uint32_t width, uint32_t height,
+                             const float lambda, const float eps_u,
+                             const float sigma, const float eta,
+                             Texture2D u_prev_tex, Texture2D u0_tex,
+                             Texture2D pu_tex, Texture2D q_tex,
+                             Texture2D ix_tex, Texture2D it_tex)
+{
+  const int x = blockIdx.x*blockDim.x + threadIdx.x;
+  const int y = blockIdx.y*blockDim.y + threadIdx.y;
+  if (x<width && y<height)
+  {
+    const float sigma_by_eta = sigma/eta;
+
+    // update pu
+    float2 du = dp(u_prev_tex, x, y);
+    float2 pu = tex2DFetch<float2>(pu_tex, x,y);
+    pu  = (pu + sigma_by_eta*du) / (1.f + sigma_by_eta*eps_u);
+    pu = pu / max(1.0f, length(pu));
+    d_pu[y*stride_pu+x] = {pu.x, pu.y};
+
+    // update q
+    float u_prev = tex2DFetch<float>(u_prev_tex, x, y);
+    float u0 = tex2DFetch<float>(u0_tex, x, y);
+    float q = tex2DFetch<float>(q_tex, x, y);
+    float ix = tex2DFetch<float>(ix_tex, x, y);
+    float it = tex2DFetch<float>(it_tex, x, y);
+    const float sigma_q = sigma / max(1e-6f, lambda * fabs(ix));
+    q = q + lambda*sigma_q * (it + ix*(u_prev-u0));
+    d_q[y*stride_q+x] = max(-1.f, min(1.f, q));
+  }
+}
+
+//-----------------------------------------------------------------------------
+template<typename PPixel, typename DPixel>
+__global__ void k_dualUpdate(DPixel* d_pu, const size_t stride_pu,
+                             PPixel* d_q, const size_t stride_q,
+                             uint32_t width, uint32_t height,
+                             const float eps_u,
+                             const float sigma, const float eta,
+                             Texture2D lambda_tex,
+                             Texture2D u_prev_tex, Texture2D u0_tex,
+                             Texture2D pu_tex, Texture2D q_tex,
+                             Texture2D ix_tex, Texture2D it_tex)
+{
+  const int x = blockIdx.x*blockDim.x + threadIdx.x;
+  const int y = blockIdx.y*blockDim.y + threadIdx.y;
+  if (x<width && y<height)
+  {
+    const float sigma_by_eta = sigma/eta;
+
+    // update pu
+    float2 du = dp(u_prev_tex, x, y);
+    float2 pu = tex2DFetch<float2>(pu_tex, x,y);
+    pu  = (pu + sigma_by_eta*du) / (1.f + sigma_by_eta*eps_u);
+    pu = pu / max(1.0f, length(pu));
+    d_pu[y*stride_pu+x] = {pu.x, pu.y};
+
+    // update q
+    float u_prev = tex2DFetch<float>(u_prev_tex, x, y);
+    float u0 = tex2DFetch<float>(u0_tex, x, y);
+    float q = tex2DFetch<float>(q_tex, x, y);
+    float ix = tex2DFetch<float>(ix_tex, x, y);
+    float it = tex2DFetch<float>(it_tex, x, y);
+    float lambda = tex2DFetch<float>(lambda_tex, x,y);
+    const float sigma_q = sigma / max(1e-6f, lambda * fabs(ix));
+    q = q + lambda*sigma_q * (it + ix*(u_prev-u0));
+    d_q[y*stride_q+x] = max(-1.f, min(1.f, q));
+  }
+}
+
+
+} // namespace cu
+} // namespace ze
+
+
+
+#endif // IMP_CU_K_STEREO_CTF_WARPING_LEVEL_HUBER_CUH
+
diff --git a/RWR/src/ze_oss/imp_cu_correspondence/src/solver_stereo_huber_l1.cu b/RWR/src/ze_oss/imp_cu_correspondence/src/solver_stereo_huber_l1.cu
new file mode 100644
index 0000000000000000000000000000000000000000..3d5a2a95b543838e009ef39d03560211b6c9865f
--- /dev/null
+++ b/RWR/src/ze_oss/imp_cu_correspondence/src/solver_stereo_huber_l1.cu
@@ -0,0 +1,169 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#include <imp/cu_correspondence/solver_stereo_huber_l1.cuh>
+
+#include <cmath>
+#include <cuda_runtime.h>
+#include <glog/logging.h>
+
+#include <imp/cu_correspondence/variational_stereo_parameters.hpp>
+#include <imp/cu_core/cu_image_gpu.cuh>
+#include <imp/cu_imgproc/cu_image_filter.cuh>
+#include <imp/cu_imgproc/cu_resample.cuh>
+#include <imp/cu_core/cu_utils.hpp>
+#include <imp/cu_core/cu_texture.cuh>
+#include <imp/cu_core/cu_math.cuh>
+
+#include "warped_gradients_kernel.cuh"
+#include "solver_stereo_huber_l1_kernel.cuh"
+
+namespace ze {
+namespace cu {
+
+
+//------------------------------------------------------------------------------
+SolverStereoHuberL1::~SolverStereoHuberL1()
+{
+  // thanks to smart pointers
+}
+
+//------------------------------------------------------------------------------
+SolverStereoHuberL1::SolverStereoHuberL1(
+    const Parameters::Ptr& params,
+    ze::Size2u size,
+    size_t level)
+  : SolverStereoAbstract(params, size, level)
+{
+  u_.reset(new ImageGpu32fC1(size));
+  u_prev_.reset(new ImageGpu32fC1(size));
+  u0_.reset(new ImageGpu32fC1(size));
+  pu_.reset(new ImageGpu32fC2(size));
+  ix_.reset(new ImageGpu32fC1(size));
+  it_.reset(new ImageGpu32fC1(size));
+
+  // and its textures
+  u_tex_ = u_->genTexture(false, cudaFilterModeLinear);
+  u_prev_tex_ =  u_prev_->genTexture(false, cudaFilterModeLinear);
+  u0_tex_ =  u0_->genTexture(false, cudaFilterModeLinear);
+  pu_tex_ =  pu_->genTexture(false, cudaFilterModeLinear);
+  ix_tex_ =  ix_->genTexture(false, cudaFilterModeLinear);
+  it_tex_ =  it_->genTexture(false, cudaFilterModeLinear);
+}
+
+//------------------------------------------------------------------------------
+void SolverStereoHuberL1::init()
+{
+  u_->setValue(0.0f);
+  pu_->setValue(0.0f);
+  // other variables are init and/or set when needed!
+}
+
+//------------------------------------------------------------------------------
+void SolverStereoHuberL1::init(const SolverStereoAbstract& rhs)
+{
+  const SolverStereoHuberL1* from =
+      dynamic_cast<const SolverStereoHuberL1*>(&rhs);
+
+  float inv_sf = 1./params_->ctf.scale_factor; // >1 for adapting prolongated disparities
+
+  if(params_->ctf.apply_median_filter)
+  {
+    ze::cu::filterMedian3x3(*from->u0_, *from->u_);
+    ze::cu::resample(*u_, *from->u0_, ze::InterpolationMode::Point, false);
+  }
+  else
+  {
+    ze::cu::resample(*u_, *from->u_, ze::InterpolationMode::Point, false);
+  }
+  *u_ *= inv_sf;
+
+  ze::cu::resample(*pu_, *from->pu_, ze::InterpolationMode::Point, false);
+}
+
+//------------------------------------------------------------------------------
+void SolverStereoHuberL1::solve(std::vector<ImageGpu32fC1::Ptr> images)
+{
+  VLOG(100) << "StereoCtFWarpingLevelPrecondHuberL1: solving level "
+            << level_ << " with " << images.size() << " images";
+
+  // sanity check:
+  // TODO
+
+  i1_tex_ = images.at(0)->genTexture(false, cudaFilterModeLinear);
+  i2_tex_ = images.at(1)->genTexture(false, cudaFilterModeLinear);
+  u_->copyTo(*u_prev_);
+  Fragmentation<> frag(size_);
+
+  // constants
+  const float L = std::sqrt(8.f);
+  const float tau = 1.f/L;
+  const float sigma = 1.f/L;
+  float lin_step = 0.5f;
+
+  // warping
+  for (uint32_t warp = 0; warp < params_->ctf.warps; ++warp)
+  {
+    VLOG(101) << "SOLVING warp iteration of Huber-L1 stereo model. warp: " << warp;
+
+    u_->copyTo(*u0_);
+
+    // compute warped spatial and temporal gradients
+    k_warpedGradients
+        <<<
+          frag.dimGrid, frag.dimBlock
+        >>> (ix_->data(), it_->data(), ix_->stride(), ix_->width(), ix_->height(),
+             *i1_tex_, *i2_tex_, *u0_tex_);
+
+    for (uint32_t iter = 0; iter < params_->ctf.iters; ++iter)
+    {
+      // dual kernel
+      k_dualUpdate
+          <<<
+            frag.dimGrid, frag.dimBlock
+          >>> (pu_->data(), pu_->stride(),
+               size_.width(), size_.height(),
+               params_->eps_u, sigma,
+               *u_prev_tex_, *pu_tex_);
+
+      // and primal kernel
+      k_primalUpdate
+          <<<
+            frag.dimGrid, frag.dimBlock
+          >>> (u_->data(), u_prev_->data(), u_->stride(),
+               size_.width(), size_.height(),
+               params_->lambda, tau, lin_step,
+               *u_tex_, *u0_tex_, *pu_tex_, *ix_tex_, *it_tex_);
+    } // iters
+    lin_step /= 1.2f;
+
+  } // warps
+  IMP_CUDA_CHECK();
+}
+
+
+
+} // namespace cu
+} // namespace ze
+
diff --git a/RWR/src/ze_oss/imp_cu_correspondence/src/solver_stereo_huber_l1_kernel.cuh b/RWR/src/ze_oss/imp_cu_correspondence/src/solver_stereo_huber_l1_kernel.cuh
new file mode 100644
index 0000000000000000000000000000000000000000..0fd29982f2be280736895b9018b84c4b0e3c4061
--- /dev/null
+++ b/RWR/src/ze_oss/imp_cu_correspondence/src/solver_stereo_huber_l1_kernel.cuh
@@ -0,0 +1,134 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#pragma once
+
+#include <cuda_runtime_api.h>
+#include <imp/core/types.hpp>
+#include <imp/cu_core/cu_utils.hpp>
+#include <imp/cu_core/cu_k_derivative.cuh>
+#include <imp/cuda_toolkit/helper_math.hpp>
+
+
+namespace ze {
+namespace cu {
+
+//-----------------------------------------------------------------------------
+/** restricts the udpate to +/- lin_step around the given value in lin_tex
+ * @note \a d_srcdst and the return value is identical.
+ * @todo (MWE) move function to common kernel def file for all stereo models
+ */
+template<typename Pixel>
+__device__ Pixel k_linearized_update(Pixel& d_srcdst, Texture2D& lin_tex,
+                                     const float lin_step,
+                                     const int x, const int y)
+{
+  Pixel lin;
+  tex2DFetch(lin, lin_tex, x, y);
+  d_srcdst = ze::cu::max(lin-lin_step,
+                          ze::cu::min(lin+lin_step, d_srcdst));
+  return d_srcdst;
+}
+
+//-----------------------------------------------------------------------------
+/**
+ * @brief k_primalUpdate is the Huber-L1-Precondition model's primal update kernel
+ * @note PPixel and DPixel denote for the Pixel type/dimension of primal and dual variable
+ */
+template<typename PPixel>
+__global__ void k_primalUpdate(PPixel* d_u, PPixel* d_u_prev, const size_t stride,
+                               uint32_t width, uint32_t height,
+                               const float lambda, const float tau,
+                               const float lin_step,
+                               Texture2D u_tex, Texture2D u0_tex,
+                               Texture2D pu_tex, Texture2D ix_tex, Texture2D it_tex)
+{
+  const int x = blockIdx.x*blockDim.x + threadIdx.x;
+  const int y = blockIdx.y*blockDim.y + threadIdx.y;
+
+  if (x<width && y<height)
+  {
+    float u0 = tex2DFetch<float>(u0_tex, x,y);
+    float it = tex2DFetch<float>(it_tex, x, y);
+    float ix = tex2DFetch<float>(ix_tex, x, y);
+
+    // divergence operator (dpAD) of dual var
+    float div = dpAd(pu_tex, x, y, width, height);
+
+    // save current u
+    float u_prev = tex2DFetch<float>(u_tex, x, y);
+    float u = u_prev;
+    u += tau*div;
+
+    // prox operator
+    float prox = it + (u-u0)*ix;
+    prox /= max(1e-9f, ix*ix);
+    float tau_lambda = tau*lambda;
+
+    if(prox < -tau_lambda)
+    {
+      u += tau_lambda*ix;
+    }
+    else if(prox > tau_lambda)
+    {
+      u -= tau_lambda*ix;
+    }
+    else
+    {
+      u -= prox*ix;
+    }
+
+    // restrict update step because of linearization only valid in small neighborhood
+    u = k_linearized_update(u, u0_tex, lin_step, x, y);
+
+    d_u[y*stride+x] = u;
+    d_u_prev[y*stride+x] = 2.f*u - u_prev;
+  }
+}
+
+//-----------------------------------------------------------------------------
+template<typename DPixel>
+__global__ void k_dualUpdate(DPixel* d_pu, const size_t stride_pu,
+                             uint32_t width, uint32_t height,
+                             const float eps_u, const float sigma,
+                             Texture2D u_prev_tex, Texture2D pu_tex)
+{
+  const int x = blockIdx.x*blockDim.x + threadIdx.x;
+  const int y = blockIdx.y*blockDim.y + threadIdx.y;
+  if (x<width && y<height)
+  {
+    // update pu
+    float2 du = dp(u_prev_tex, x, y);
+    float2 pu = tex2DFetch<float2>(pu_tex, x,y);
+    pu  = (pu + sigma*du) / (1.f + sigma*eps_u);
+    pu = pu / max(1.0f, length(pu));
+
+    d_pu[y*stride_pu+x] = {pu.x, pu.y};
+  }
+}
+
+
+} // namespace cu
+} // namespace ze
+
diff --git a/RWR/src/ze_oss/imp_cu_correspondence/src/solver_stereo_precond_huber_l1.cu b/RWR/src/ze_oss/imp_cu_correspondence/src/solver_stereo_precond_huber_l1.cu
new file mode 100644
index 0000000000000000000000000000000000000000..869dc739bed0acd79f1d75adfc051108a3337315
--- /dev/null
+++ b/RWR/src/ze_oss/imp_cu_correspondence/src/solver_stereo_precond_huber_l1.cu
@@ -0,0 +1,184 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#include <imp/cu_correspondence/solver_stereo_precond_huber_l1.cuh>
+
+#include <cuda_runtime.h>
+#include <glog/logging.h>
+
+#include <imp/cu_correspondence/variational_stereo_parameters.hpp>
+#include <imp/cu_core/cu_image_gpu.cuh>
+#include <imp/cu_imgproc/cu_image_filter.cuh>
+#include <imp/cu_imgproc/cu_resample.cuh>
+#include <imp/cu_core/cu_utils.hpp>
+#include <imp/cu_core/cu_texture.cuh>
+#include <imp/cu_core/cu_math.cuh>
+
+#include "warped_gradients_kernel.cuh"
+#include "solver_precond_huber_l1_kernel.cuh"
+
+namespace ze {
+namespace cu {
+
+//------------------------------------------------------------------------------
+SolverStereoPrecondHuberL1::~SolverStereoPrecondHuberL1()
+{
+  // thanks to smart pointers
+}
+
+//------------------------------------------------------------------------------
+SolverStereoPrecondHuberL1::SolverStereoPrecondHuberL1(
+    const Parameters::Ptr& params,
+    ze::Size2u size,
+    size_t level)
+  : SolverStereoAbstract(params, size, level)
+{
+  u_.reset(new ImageGpu32fC1(size));
+  u_prev_.reset(new ImageGpu32fC1(size));
+  u0_.reset(new ImageGpu32fC1(size));
+  pu_.reset(new ImageGpu32fC2(size));
+  q_.reset(new ImageGpu32fC1(size));
+  ix_.reset(new ImageGpu32fC1(size));
+  it_.reset(new ImageGpu32fC1(size));
+  xi_.reset(new ImageGpu32fC1(size));
+
+  // and its textures
+  u_tex_ = u_->genTexture(false, cudaFilterModeLinear);
+  u_prev_tex_ =  u_prev_->genTexture(false, cudaFilterModeLinear);
+  u0_tex_ =  u0_->genTexture(false, cudaFilterModeLinear);
+  pu_tex_ =  pu_->genTexture(false, cudaFilterModeLinear);
+  q_tex_ =  q_->genTexture(false, cudaFilterModeLinear);
+  ix_tex_ =  ix_->genTexture(false, cudaFilterModeLinear);
+  it_tex_ =  it_->genTexture(false, cudaFilterModeLinear);
+  xi_tex_ =  xi_->genTexture(false, cudaFilterModeLinear);
+}
+
+//------------------------------------------------------------------------------
+void SolverStereoPrecondHuberL1::init()
+{
+  u_->setValue(0.0f);
+  pu_->setValue(0.0f);
+  q_->setValue(0.0f);
+  // other variables are init and/or set when needed!
+}
+
+//------------------------------------------------------------------------------
+void SolverStereoPrecondHuberL1::init(const SolverStereoAbstract& rhs)
+{
+  const SolverStereoPrecondHuberL1* from =
+      dynamic_cast<const SolverStereoPrecondHuberL1*>(&rhs);
+
+  float inv_sf = 1./params_->ctf.scale_factor; // >1 for adapting prolongated disparities
+
+  if(params_->ctf.apply_median_filter)
+  {
+    ze::cu::filterMedian3x3(*from->u0_, *from->u_);
+    ze::cu::resample(*u_, *from->u0_, ze::InterpolationMode::Point, false);
+  }
+  else
+  {
+    ze::cu::resample(*u_, *from->u_, ze::InterpolationMode::Point, false);
+  }
+  *u_ *= inv_sf;
+
+  ze::cu::resample(*pu_, *from->pu_, ze::InterpolationMode::Point, false);
+  ze::cu::resample(*q_, *from->q_, ze::InterpolationMode::Point, false);
+}
+
+//------------------------------------------------------------------------------
+void SolverStereoPrecondHuberL1::solve(std::vector<ImageGpu32fC1::Ptr> images)
+{
+  VLOG(100) << "StereoCtFWarpingLevelPrecondHuberL1: solving level "
+            << level_ << " with " << images.size() << " images";
+
+  // sanity check:
+  // TODO
+
+  // image textures
+  i1_tex_ = images.at(0)->genTexture(false, cudaFilterModeLinear);
+  i2_tex_ = images.at(1)->genTexture(false, cudaFilterModeLinear);
+  u_->copyTo(*u_prev_);
+  Fragmentation<> frag(size_);
+
+  // constants
+  constexpr float tau = 0.95f;
+  constexpr float sigma = 0.95f;
+  float lin_step = 0.5f;
+
+  // precond
+  constexpr float eta = 2.0f;
+
+  // warping
+  for (uint32_t warp = 0; warp < params_->ctf.warps; ++warp)
+  {
+    VLOG(101) << "SOLVING warp iteration of Huber-L1 stereo model. warp: " << warp;
+
+    u_->copyTo(*u0_);
+
+    // compute warped spatial and temporal gradients
+    k_warpedGradients
+        <<<
+          frag.dimGrid, frag.dimBlock
+        >>> (ix_->data(), it_->data(), ix_->stride(), ix_->width(), ix_->height(),
+             *i1_tex_, *i2_tex_, *u0_tex_);
+
+    // compute preconditioner
+    k_preconditioner
+        <<<
+          frag.dimGrid, frag.dimBlock
+        >>> (xi_->data(), xi_->stride(), xi_->width(), xi_->height(),
+             params_->lambda, *ix_tex_);
+
+
+    for (uint32_t iter = 0; iter < params_->ctf.iters; ++iter)
+    {
+      // dual update kernel
+      k_dualUpdate
+          <<<
+            frag.dimGrid, frag.dimBlock
+          >>> (pu_->data(), pu_->stride(), q_->data(), q_->stride(),
+               size_.width(), size_.height(),
+               params_->lambda, params_->eps_u, sigma, eta,
+               *u_prev_tex_, *u0_tex_, *pu_tex_, *q_tex_, *ix_tex_, *it_tex_);
+
+      // and primal update kernel
+      k_primalUpdate
+          <<<
+            frag.dimGrid, frag.dimBlock
+          >>> (u_->data(), u_prev_->data(), u_->stride(),
+               size_.width(), size_.height(),
+               params_->lambda, tau, lin_step,
+               *u_tex_, *u0_tex_, *pu_tex_, *q_tex_, *ix_tex_, *xi_tex_);
+    } // iters
+    lin_step /= 1.2f;
+
+  } // warps
+  IMP_CUDA_CHECK();
+}
+
+
+
+} // namespace cu
+} // namespace ze
+
diff --git a/RWR/src/ze_oss/imp_cu_correspondence/src/solver_stereo_precond_huber_l1_weighted.cu b/RWR/src/ze_oss/imp_cu_correspondence/src/solver_stereo_precond_huber_l1_weighted.cu
new file mode 100644
index 0000000000000000000000000000000000000000..a92f47f57e71a20f8c8ada9b79ff1d27785e1078
--- /dev/null
+++ b/RWR/src/ze_oss/imp_cu_correspondence/src/solver_stereo_precond_huber_l1_weighted.cu
@@ -0,0 +1,235 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#include <imp/cu_correspondence/solver_stereo_precond_huber_l1_weighted.cuh>
+
+#include <cuda_runtime.h>
+#include <glog/logging.h>
+
+#include <imp/cu_correspondence/variational_stereo_parameters.hpp>
+#include <imp/cu_core/cu_image_gpu.cuh>
+#include <imp/cu_core/cu_utils.hpp>
+#include <imp/cu_core/cu_texture.cuh>
+#include <imp/cu_core/cu_math.cuh>
+#include <imp/cu_imgproc/cu_image_filter.cuh>
+#include <imp/cu_imgproc/cu_resample.cuh>
+#include <imp/cu_imgproc/edge_detectors.cuh>
+#include <imp/cu_correspondence/occlusion.cuh>
+
+#include "warped_gradients_kernel.cuh"
+#include "solver_precond_huber_l1_kernel.cuh"
+#include "solver_stereo_precond_huber_l1_weighted_kernel.cuh"
+//#include "occlusion_kernel.cuh"
+
+namespace ze {
+namespace cu {
+
+//------------------------------------------------------------------------------
+SolverStereoPrecondHuberL1Weighted::~SolverStereoPrecondHuberL1Weighted()
+{
+  // thanks to smart pointers
+}
+
+//------------------------------------------------------------------------------
+SolverStereoPrecondHuberL1Weighted::SolverStereoPrecondHuberL1Weighted(
+    const std::shared_ptr<Parameters>& params, ze::Size2u size, size_t level)
+  : SolverStereoAbstract(params, size, level)
+{
+  u_.reset(new ImageGpu32fC1(size));
+  u_prev_.reset(new ImageGpu32fC1(size));
+  u0_.reset(new ImageGpu32fC1(size));
+  pu_.reset(new ImageGpu32fC2(size));
+  q_.reset(new ImageGpu32fC1(size));
+  ix_.reset(new ImageGpu32fC1(size));
+  it_.reset(new ImageGpu32fC1(size));
+  xi_.reset(new ImageGpu32fC1(size));
+  g_.reset(new ImageGpu32fC1(size));
+  g_->setValue(1.0f);
+  occ_.reset(new ImageGpu32fC1(size));
+
+  u_tex_ = u_->genTexture(false, cudaFilterModeLinear);
+  u_prev_tex_ =  u_prev_->genTexture(false, cudaFilterModeLinear);
+  u0_tex_ =  u0_->genTexture(false, cudaFilterModeLinear);
+  pu_tex_ =  pu_->genTexture(false, cudaFilterModeLinear);
+  q_tex_ =  q_->genTexture(false, cudaFilterModeLinear);
+  ix_tex_ =  ix_->genTexture(false, cudaFilterModeLinear);
+  it_tex_ =  it_->genTexture(false, cudaFilterModeLinear);
+  xi_tex_ =  xi_->genTexture(false, cudaFilterModeLinear);
+  g_tex_  = g_->genTexture(false, cudaFilterModeLinear);
+  occ_tex_ = occ_->genTexture();
+}
+
+//------------------------------------------------------------------------------
+void SolverStereoPrecondHuberL1Weighted::init()
+{
+  u_->setValue(0.0f);
+  pu_->setValue(0.0f);
+  q_->setValue(0.0f);
+  // other variables are init and/or set when needed!
+}
+
+//------------------------------------------------------------------------------
+void SolverStereoPrecondHuberL1Weighted::init(const SolverStereoAbstract& rhs)
+{
+  const SolverStereoPrecondHuberL1Weighted* from =
+      dynamic_cast<const SolverStereoPrecondHuberL1Weighted*>(&rhs);
+
+  float inv_sf = 1./params_->ctf.scale_factor; // >1 for adapting prolongated disparities
+
+  if(params_->ctf.apply_median_filter)
+  {
+    ze::cu::filterMedian3x3(*from->u0_, *from->u_);
+    ze::cu::resample(*u_, *from->u0_, ze::InterpolationMode::Point, false);
+  }
+  else
+  {
+    ze::cu::resample(*u_, *from->u_, ze::InterpolationMode::Point, false);
+  }
+  *u_ *= inv_sf;
+
+  ze::cu::resample(*pu_, *from->pu_, ze::InterpolationMode::Point, false);
+  ze::cu::resample(*q_, *from->q_, ze::InterpolationMode::Point, false);
+}
+
+//------------------------------------------------------------------------------
+void SolverStereoPrecondHuberL1Weighted::solve(std::vector<ImageGpu32fC1::Ptr> images)
+{
+  VLOG(100) << "StereoCtFWarpingLevelPrecondHuberL1: solving level "
+            << level_ << " with " << images.size() << " images";
+
+  // sanity check:
+  // TODO
+
+
+  ImageGpu32fC1::Ptr ep = std::make_shared<ImageGpu32fC1>(size_);
+
+  Fragmentation<> frag(size_);
+  u_->copyTo(*u_prev_);
+
+  // compute edge weight
+  naturalEdges(*g_, *images.at(0),
+               params_->edge_sigma, params_->edge_alpha, params_->edge_q);
+
+
+  // constants
+  constexpr float tau = 0.95f;
+  constexpr float sigma = 0.95f;
+  float lin_step = 0.5f;
+
+  // precond
+  constexpr float eta = 2.0f;
+
+  // warping
+  for (uint32_t warp = 0; warp < params_->ctf.warps; ++warp)
+  {
+    VLOG(101) << "SOLVING warp iteration of the gradient weighted Huber-L1 stereo model. warp: " << warp;
+
+    u_->copyTo(*u0_);
+#if 1
+    occlusionCandidatesUniqunessMapping(occ_, u0_);
+#else
+    occ_->setValue(0);
+    k_occlusionCandidatesUniqunessMapping
+        <<<
+          frag.dimGrid, frag.dimBlock
+        >>> (occ_->cuData(), occ_->stride(), occ_->width(), occ_->height(),
+             *u0_tex_);
+    k_clampOcclusion
+        <<<
+          frag.dimGrid, frag.dimBlock
+        >>> (occ_->cuData(), occ_->stride(), occ_->width(), occ_->height(),
+             *occ_tex_);
+#endif
+    i1_tex_ = images.at(0)->genTexture(false, cudaFilterModeLinear);
+    i2_tex_ = images.at(1)->genTexture(false, cudaFilterModeLinear);
+
+    k_warpedGradients
+        <<<
+          frag.dimGrid, frag.dimBlock
+        >>> (ix_->data(), it_->data(), ix_->stride(), ix_->width(), ix_->height(),
+             *i1_tex_, *i2_tex_, *u0_tex_);
+
+    k_preconditionerWeighted
+        <<<
+          frag.dimGrid, frag.dimBlock
+        >>> (xi_->data(), xi_->stride(), xi_->width(), xi_->height(),
+             params_->lambda, *ix_tex_, *g_tex_);
+
+
+    for (uint32_t iter = 0; iter < params_->ctf.iters; ++iter)
+    {
+      k_dualUpdate
+          <<<
+            frag.dimGrid, frag.dimBlock
+          >>> (pu_->data(), pu_->stride(), q_->data(), q_->stride(),
+               size_.width(), size_.height(),
+               params_->lambda, params_->eps_u, sigma, eta,
+               *u_prev_tex_, *u0_tex_, *pu_tex_, *q_tex_, *ix_tex_, *it_tex_);
+
+      k_primalUpdateWeighted
+          <<<
+            frag.dimGrid, frag.dimBlock
+          >>> (u_->data(), u_prev_->data(), u_->stride(),
+               size_.width(), size_.height(),
+               params_->lambda, tau, lin_step,
+               *u_tex_, *u0_tex_, *pu_tex_, *q_tex_, *ix_tex_, *xi_tex_, *g_tex_);
+
+    } // iters
+    lin_step /= 1.2f;
+
+#if 0
+      k_primalEnergy
+          <<<
+            frag.dimGrid, frag.dimBlock
+          >>> (ep->cuData(), ep->stride(), ep->width(), ep->height(),
+               params_->lambda,
+               *u_tex_, *g_tex_, *i1_tex_, *i2_tex_);
+      Pixel32fC1 ep_min, ep_max;
+      imp::cu::minMax(*ep, ep_min, ep_max);
+      std::cout << "ENERGY: " << ep_min.x << ", " << ep_max.x << std::endl;
+#endif
+
+  } // warps
+  IMP_CUDA_CHECK();
+}
+
+//------------------------------------------------------------------------------
+ImageGpu32fC1::Ptr SolverStereoPrecondHuberL1Weighted::computePrimalEnergy()
+{
+  ImageGpu32fC1::Ptr ep = std::make_shared<ImageGpu32fC1>(size_);
+  Fragmentation<> frag(size_);
+  k_primalEnergy
+      <<<
+        frag.dimGrid, frag.dimBlock
+      >>> (ep->cuData(), ep->stride(), ep->width(), ep->height(),
+           params_->lambda,
+           *u_tex_, *g_tex_, *i1_tex_, *i2_tex_);
+  return ep;
+}
+
+
+
+} // namespace cu
+} // namespace ze
+
diff --git a/RWR/src/ze_oss/imp_cu_correspondence/src/solver_stereo_precond_huber_l1_weighted_kernel.cuh b/RWR/src/ze_oss/imp_cu_correspondence/src/solver_stereo_precond_huber_l1_weighted_kernel.cuh
new file mode 100644
index 0000000000000000000000000000000000000000..869cdfb7aba4349adbcdb4e0919ee07ed391a8d5
--- /dev/null
+++ b/RWR/src/ze_oss/imp_cu_correspondence/src/solver_stereo_precond_huber_l1_weighted_kernel.cuh
@@ -0,0 +1,224 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#ifndef IMP_CU_K_STEREO_CTF_WARPING_LEVEL_PRECOND_HUBER_L1_WEIGHTED_CUH
+#define IMP_CU_K_STEREO_CTF_WARPING_LEVEL_PRECOND_HUBER_L1_WEIGHTED_CUH
+
+#include <cuda_runtime_api.h>
+#include <imp/core/types.hpp>
+#include <imp/cu_core/cu_utils.hpp>
+#include <imp/cu_core/cu_k_derivative.cuh>
+#include <imp/cuda_toolkit/helper_math.hpp>
+
+
+namespace ze {
+namespace cu {
+
+//-----------------------------------------------------------------------------
+template<typename Pixel>
+__global__ void k_preconditionerWeighted(Pixel* xi, size_t stride,
+                                         uint32_t width, uint32_t height,
+                                         // uint32_t roi_x, uint32_t roi_y,
+                                         float lambda, Texture2D ix_tex, Texture2D g_tex)
+{
+  const int x = blockIdx.x*blockDim.x + threadIdx.x;
+  const int y = blockIdx.y*blockDim.y + threadIdx.y;
+
+  if (x<width && y<height)
+  {
+    Pixel ix, g;
+    tex2DFetch(ix, ix_tex, x, y);
+    tex2DFetch(g, g_tex, x, y);
+    xi[y*stride+x] = 4*g + fabs(lambda * ix);
+    //    xi[y*stride+x] = 4*g + sqr(lambda) * sqr(ix);
+  }
+}
+
+//-----------------------------------------------------------------------------
+template<typename Pixel>
+__global__ void k_preconditionerWeighted(Pixel* xi, size_t stride,
+                                         uint32_t width, uint32_t height,
+                                         // uint32_t roi_x, uint32_t roi_y,
+                                         Texture2D lambda_tex, Texture2D ix_tex,
+                                         Texture2D g_tex)
+{
+  const int x = blockIdx.x*blockDim.x + threadIdx.x;
+  const int y = blockIdx.y*blockDim.y + threadIdx.y;
+
+  if (x<width && y<height)
+  {
+    Pixel ix, g;
+    tex2DFetch(ix, ix_tex, x, y);
+    tex2DFetch(g, g_tex, x, y);
+    xi[y*stride+x] = 4*g + fabs(tex2D<float>(lambda_tex, x, y)*ix);
+    //    xi[y*stride+x] = 4*g + sqr(lambda) * sqr(ix);
+  }
+}
+
+//-----------------------------------------------------------------------------
+/**
+ * @brief k_primalUpdateWeighted is the weighted Huber-L1-Precondition model's primal update kernel
+ * @note PPixel and DPixel denote for the Pixel type/dimension of primal and dual variable
+ */
+template<typename PPixel>
+__global__ void k_primalUpdateWeighted(PPixel* d_u, PPixel* d_u_prev, const size_t stride,
+                                       uint32_t width, uint32_t height,
+                                       const float lambda, const float tau,
+                                       const float lin_step,
+                                       Texture2D u_tex, Texture2D u0_tex,
+                                       Texture2D pu_tex, Texture2D q_tex,
+                                       Texture2D ix_tex, Texture2D xi_tex,
+                                       Texture2D g_tex)
+{
+  const int x = blockIdx.x*blockDim.x + threadIdx.x;
+  const int y = blockIdx.y*blockDim.y + threadIdx.y;
+
+  if (x<width && y<height)
+  {
+    float u_prev = tex2DFetch<float>(u_tex, x, y);
+    float q = tex2DFetch<float>(q_tex, x, y);
+    float ix = tex2DFetch<float>(ix_tex, x, y);
+    float xi = max(1e-6f, tex2DFetch<float>(xi_tex, x, y));
+
+    float div = dpAdWeighted(pu_tex, g_tex, x, y, width, height);
+
+    float u = u_prev - tau/xi * (-div + lambda*ix*q);
+
+    d_u[y*stride+x] = u = k_linearized_update(u, u0_tex, lin_step, x, y);
+    d_u_prev[y*stride+x] = 2.f*u - u_prev;
+  }
+}
+
+
+//-----------------------------------------------------------------------------
+/**
+ * @brief k_primalUpdateWeighted is the weighted Huber-L1-Precondition model's primal update kernel
+ * @note PPixel and DPixel denote for the Pixel type/dimension of primal and dual variable
+ */
+template<typename PPixel>
+__global__ void k_primalUpdateWeighted(PPixel* d_u, PPixel* d_u_prev, const size_t stride,
+                                       uint32_t width, uint32_t height,
+                                       const float tau, const float lin_step,
+                                       Texture2D lambda_tex,
+                                       Texture2D u_tex, Texture2D u0_tex,
+                                       Texture2D pu_tex, Texture2D q_tex,
+                                       Texture2D ix_tex, Texture2D xi_tex,
+                                       Texture2D g_tex)
+{
+  const int x = blockIdx.x*blockDim.x + threadIdx.x;
+  const int y = blockIdx.y*blockDim.y + threadIdx.y;
+
+  if (x<width && y<height)
+  {
+    float u_prev = tex2DFetch<float>(u_tex, x, y);
+    float q = tex2DFetch<float>(q_tex, x, y);
+    float ix = tex2DFetch<float>(ix_tex, x, y);
+    float xi = max(1e-6f, tex2DFetch<float>(xi_tex, x, y));
+
+    float div = dpAdWeighted(pu_tex, g_tex, x, y, width, height);
+    //    float div = dpAd(pu_tex, x, y, width, height);
+
+    float lambda = tex2DFetch<float>(lambda_tex, x,y);
+    float u = u_prev - tau/xi * (-div + lambda*ix*q);
+
+    u = k_linearized_update(u, u0_tex, lin_step, x, y);
+    d_u[y*stride+x] = u;
+    d_u_prev[y*stride+x] = 2.f*u - u_prev;
+  }
+}
+
+//-----------------------------------------------------------------------------
+template<typename PPixel, typename DPixel>
+__global__ void k_dualUpdateWeighted(DPixel* d_pu, const size_t stride_pu,
+                                     PPixel* d_q, const size_t stride_q,
+                                     uint32_t width, uint32_t height,
+                                     const float eps_u,
+                                     const float sigma, const float eta,
+                                     Texture2D lambda_tex,
+                                     Texture2D u_prev_tex, Texture2D u0_tex,
+                                     Texture2D pu_tex, Texture2D q_tex,
+                                     Texture2D ix_tex, Texture2D it_tex,
+                                     Texture2D g_tex)
+{
+  const int x = blockIdx.x*blockDim.x + threadIdx.x;
+  const int y = blockIdx.y*blockDim.y + threadIdx.y;
+  if (x<width && y<height)
+  {
+    const float sigma_by_eta = sigma/eta;
+
+    // update pu
+    float2 du = dpWeighted(u_prev_tex, g_tex, x, y);
+    float2 pu = tex2DFetch<float2>(pu_tex, x,y);
+    pu  = (pu + sigma_by_eta*du) / (1.f + sigma_by_eta*eps_u);
+    pu = pu / max(1.0f, length(pu));
+    d_pu[y*stride_pu+x] = {pu.x, pu.y};
+
+    // update q
+    float u_prev = tex2DFetch<float>(u_prev_tex, x, y);
+    float u0 = tex2DFetch<float>(u0_tex, x, y);
+    float q = tex2DFetch<float>(q_tex, x, y);
+    float ix = tex2DFetch<float>(ix_tex, x, y);
+    float it = tex2DFetch<float>(it_tex, x, y);
+    float lambda = tex2DFetch<float>(lambda_tex, x,y);
+    const float sigma_q = sigma / max(1e-6f, lambda * fabs(ix));
+    q = q + lambda*sigma_q * (it + ix*(u_prev-u0));
+    d_q[y*stride_q+x] = max(-1.f, min(1.f, q));
+  }
+}
+
+//-----------------------------------------------------------------------------
+template<typename T>
+__global__ void k_primalEnergy(T* ep, const size_t stride,
+                               const uint32_t width, const uint32_t height,
+                               const float lambda,
+                               Texture2D u_tex, Texture2D g_tex,
+                               Texture2D i1_tex, Texture2D i2_tex)
+{
+  const int x = blockIdx.x*blockDim.x + threadIdx.x;
+  const int y = blockIdx.y*blockDim.y + threadIdx.y;
+  if (x<width && y<height)
+  {
+    float2 du = dpWeighted(u_tex, g_tex, x, y);
+    float u = tex2DFetch<float>(u_tex, x,y);
+    float wx = x+u;
+    float dat = 0.f;
+
+    float bd = .5f;
+    if ((wx > bd) && (x > bd) && (wx < width-bd-1) && (x < width-bd-1) &&
+        (y>bd) && (y<height-bd-1))
+    {
+      dat = tex2DFetch<float>(i2_tex, wx,y) - tex2DFetch<float>(i1_tex, x,y);
+    }
+
+    ep[y*stride+x] = length(du) + lambda * fabs(dat);
+  }
+}
+
+} // namespace cu
+} // namespace ze
+
+
+
+#endif // IMP_CU_K_STEREO_CTF_WARPING_LEVEL_PRECOND_HUBER_L1_WEIGHTED_CUH
+
diff --git a/RWR/src/ze_oss/imp_cu_correspondence/src/stereo_ctf_warping.cpp b/RWR/src/ze_oss/imp_cu_correspondence/src/stereo_ctf_warping.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..dc94db6ca2f1b70de7f9180632f293223433643f
--- /dev/null
+++ b/RWR/src/ze_oss/imp_cu_correspondence/src/stereo_ctf_warping.cpp
@@ -0,0 +1,215 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#include <imp/cu_correspondence/stereo_ctf_warping.hpp>
+
+#include <memory>
+
+#include <glog/logging.h>
+
+#include <imp/cu_correspondence/solver_stereo_huber_l1.cuh>
+#include <imp/cu_correspondence/solver_stereo_precond_huber_l1.cuh>
+#include <imp/cu_correspondence/solver_stereo_precond_huber_l1_weighted.cuh>
+#include <imp/cu_correspondence/solver_epipolar_stereo_precond_huber_l1.cuh>
+
+#include <imp/cu_core/cu_utils.hpp>
+
+namespace ze {
+namespace cu {
+
+//------------------------------------------------------------------------------
+StereoCtFWarping::StereoCtFWarping(Parameters::Ptr params)
+  : params_(params)
+{
+}
+
+//------------------------------------------------------------------------------
+StereoCtFWarping::~StereoCtFWarping()
+{
+  // thanks to managed ptrs
+}
+
+//------------------------------------------------------------------------------
+void StereoCtFWarping::init()
+{
+  CHECK(!image_pyramids_.empty());
+  for (size_t i=params_->ctf.finest_level; i<=params_->ctf.coarsest_level; ++i)
+  {
+    Size2u sz = image_pyramids_.front()->size(i);
+    switch (params_->solver)
+    {
+    case StereoPDSolver::HuberL1:
+      levels_.emplace_back(new SolverStereoHuberL1(params_, sz, i));
+      break;
+    case StereoPDSolver::PrecondHuberL1:
+      levels_.emplace_back(new SolverStereoPrecondHuberL1(params_, sz, i));
+      break;
+    case StereoPDSolver::PrecondHuberL1Weighted:
+      levels_.emplace_back(new SolverStereoPrecondHuberL1Weighted(params_, sz, i));
+      break;
+    case StereoPDSolver::EpipolarPrecondHuberL1:
+    {
+      if (!depth_proposal_)
+      {
+        depth_proposal_.reset(new ImageGpu32fC1(image_pyramids_.front()->size(0)));
+        depth_proposal_->setValue(0.f);
+      }
+      if (!depth_proposal_sigma2_)
+      {
+        depth_proposal_sigma2_.reset(new ImageGpu32fC1(image_pyramids_.front()->size(0)));
+        depth_proposal_sigma2_->setValue(0.f);
+      }
+
+      levels_.emplace_back(new SolverEpipolarStereoPrecondHuberL1(
+                             params_, sz, i, cams_, F_, T_mov_fix_,
+                             *depth_proposal_, *depth_proposal_sigma2_));
+    }
+      break;
+    }
+  }
+}
+
+//------------------------------------------------------------------------------
+bool StereoCtFWarping::ready()
+{
+  // check if all vectors are of the same length and not empty
+  size_t desired_num_levels =
+      params_->ctf.coarsest_level - params_->ctf.finest_level + 1;
+
+  if (images_.empty() || image_pyramids_.empty() || levels_.empty() ||
+      params_->ctf.coarsest_level < params_->ctf.finest_level ||
+      images_.size() < 2 || // at least two images -> maybe adapt to the algorithm?
+      image_pyramids_.front()->numLevels() < desired_num_levels ||
+      levels_.size() < desired_num_levels)
+  {
+    return false;
+  }
+  return true;
+}
+
+//------------------------------------------------------------------------------
+void StereoCtFWarping::addImage(const ImageGpu32fC1::Ptr& image)
+{
+  // generate image pyramid
+  ImagePyramid32fC1::Ptr pyr =
+      createImagePyramidGpu<Pixel32fC1>(image, params_->ctf.scale_factor);
+
+  // update number of levels
+  if (params_->ctf.levels > pyr->numLevels())
+  {
+    params_->ctf.levels = pyr->numLevels();
+  }
+  if (params_->ctf.coarsest_level > params_->ctf.levels - 1)
+  {
+    params_->ctf.coarsest_level = params_->ctf.levels - 1;
+  }
+
+  images_.push_back(image);
+  image_pyramids_.push_back(pyr);
+
+  VLOG(1) << "we have now " << images_.size() << " images and "
+          <<  image_pyramids_.size() << " pyramids in the CTF instance. "
+           << "params_->ctf.levels: " << params_->ctf.levels
+           << " (" << params_->ctf.coarsest_level
+           << " -> " << params_->ctf.finest_level << ")";
+}
+
+//------------------------------------------------------------------------------
+void StereoCtFWarping::reset()
+{
+  images_.clear();
+  image_pyramids_.clear();
+}
+
+//------------------------------------------------------------------------------
+void StereoCtFWarping::solve()
+{
+  if (levels_.empty())
+  {
+    this->init();
+  }
+  CHECK(this->ready()) << "not initialized correctly; bailing out;";
+
+  // the image vector that is used as input for the level solvers
+  std::vector<ImageGpu32fC1::Ptr> lev_images;
+
+
+  // the first level is initialized differently so we solve this one first
+  size_t lev = params_->ctf.coarsest_level;
+  levels_.at(lev)->init();
+  // gather images of current scale level
+  lev_images.clear();
+  for (auto pyr : image_pyramids_)
+  {
+    lev_images.push_back(pyr->atShared(lev)->as<ImageGpu32fC1>());
+  }
+  levels_.at(lev)->solve(lev_images);
+
+  // and then loop until we reach the finest level
+  // note that we loop with +1 idx as we would result in a buffer underflow
+  // due to operator-- on size_t which is an unsigned type.
+  for (; lev > params_->ctf.finest_level; --lev)
+  {
+    levels_.at(lev-1)->init(*levels_.at(lev));
+
+    // gather images of current scale level
+    lev_images.clear();
+    for (auto pyr : image_pyramids_)
+    {
+      lev_images.push_back(pyr->atShared(lev-1)->as<ImageGpu32fC1>());
+    }
+    levels_.at(lev-1)->solve(lev_images);
+  }
+}
+
+//------------------------------------------------------------------------------
+ImageGpu32fC1::Ptr StereoCtFWarping::computePrimalEnergy(size_t level)
+{
+  CHECK(this->ready()) << "not initialized correctly; bailing out;";
+  level = max(params_->ctf.finest_level,
+              min(params_->ctf.coarsest_level, level));
+  return levels_.at(level)->computePrimalEnergy();
+}
+
+//------------------------------------------------------------------------------
+StereoCtFWarping::ImageGpu32fC1::Ptr StereoCtFWarping::getDisparities(size_t level)
+{
+  CHECK(this->ready()) << "not initialized correctly; bailing out;";
+  level = max(params_->ctf.finest_level,
+              min(params_->ctf.coarsest_level, level));
+  return levels_.at(level)->getDisparities();
+}
+
+//------------------------------------------------------------------------------
+StereoCtFWarping::ImageGpu32fC1::Ptr StereoCtFWarping::getOcclusion(size_t level)
+{
+  CHECK(this->ready()) << "not initialized correctly; bailing out;";
+  level = max(params_->ctf.finest_level,
+              min(params_->ctf.coarsest_level, level));
+  return levels_.at(level)->getOcclusion();
+}
+
+} // namespace cu
+} // namespace ze
+
diff --git a/RWR/src/ze_oss/imp_cu_correspondence/src/variational_epipolar_stereo.cpp b/RWR/src/ze_oss/imp_cu_correspondence/src/variational_epipolar_stereo.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6d37ff33704f343b05b960cf9c9bb5dd0538b24d
--- /dev/null
+++ b/RWR/src/ze_oss/imp_cu_correspondence/src/variational_epipolar_stereo.cpp
@@ -0,0 +1,71 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#include <imp/cu_correspondence/variational_epipolar_stereo.hpp>
+
+#include <imp/cu_correspondence/stereo_ctf_warping.hpp>
+
+namespace ze {
+namespace cu {
+
+//------------------------------------------------------------------------------
+VariationalEpipolarStereo::VariationalEpipolarStereo(Parameters::Ptr params)
+  : VariationalStereo(params)
+{ ; }
+
+//------------------------------------------------------------------------------
+VariationalEpipolarStereo::~VariationalEpipolarStereo()
+{ ; }
+
+//------------------------------------------------------------------------------
+void VariationalEpipolarStereo::setFundamentalMatrix(const cu::Matrix3f& F)
+{
+  ctf_->setFundamentalMatrix(F);
+}
+
+
+//------------------------------------------------------------------------------
+void VariationalEpipolarStereo::setIntrinsics(const Cameras& cams)
+{
+  ctf_->setIntrinsics(cams);
+}
+
+//------------------------------------------------------------------------------
+void VariationalEpipolarStereo::setExtrinsics(const cu::SE3<float>& T_mov_fix)
+{
+  ctf_->setExtrinsics(T_mov_fix);
+}
+
+//------------------------------------------------------------------------------
+void VariationalEpipolarStereo::setDepthProposal(
+    const ImageGpu32fC1::Ptr& depth_proposal,
+    const ImageGpu32fC1::Ptr& depth_proposal_sigma2)
+{
+  ctf_->setDepthProposal(depth_proposal, depth_proposal_sigma2);
+}
+
+
+} // namespace cu
+} // namespace ze
+
diff --git a/RWR/src/ze_oss/imp_cu_correspondence/src/variational_stereo.cpp b/RWR/src/ze_oss/imp_cu_correspondence/src/variational_stereo.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6a779ce4c98c99d0e4450dd82416c70b625acdd3
--- /dev/null
+++ b/RWR/src/ze_oss/imp_cu_correspondence/src/variational_stereo.cpp
@@ -0,0 +1,95 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#include <imp/cu_correspondence/variational_stereo.hpp>
+#include <imp/cu_correspondence/stereo_ctf_warping.hpp>
+
+namespace ze {
+namespace cu {
+
+//------------------------------------------------------------------------------
+VariationalStereo::VariationalStereo(Parameters::Ptr params)
+{
+  if (params)
+  {
+    params_ = params;
+  }
+  else
+  {
+    params_ = std::make_shared<Parameters>();
+  }
+
+  ctf_.reset(new StereoCtFWarping(params_));
+}
+
+//------------------------------------------------------------------------------
+VariationalStereo::~VariationalStereo()
+{ ; }
+
+
+//------------------------------------------------------------------------------
+void VariationalStereo::addImage(const ImageGpu32fC1::Ptr& image)
+{
+  CHECK(image != nullptr) << "Invalid input image.";
+  ctf_->addImage(image);
+}
+
+//------------------------------------------------------------------------------
+void VariationalStereo::reset()
+{
+  ctf_->reset();
+}
+
+//------------------------------------------------------------------------------
+void VariationalStereo::solve()
+{
+  ctf_->solve();
+}
+
+//------------------------------------------------------------------------------
+ImageGpu32fC1::Ptr VariationalStereo::computePrimalEnergy(size_t level)
+{
+  CHECK_LT(level, params_->ctf.coarsest_level);
+  return ctf_->computePrimalEnergy(level);
+}
+
+//------------------------------------------------------------------------------
+VariationalStereo::ImageGpu32fC1::Ptr VariationalStereo::getDisparities(size_t level)
+{
+  CHECK_LT(level, params_->ctf.coarsest_level);
+  return ctf_->getDisparities(level);
+}
+
+
+//------------------------------------------------------------------------------
+VariationalStereo::ImageGpu32fC1::Ptr VariationalStereo::getOcclusion(size_t level)
+{
+  CHECK_LT(level, params_->ctf.coarsest_level);
+  return ctf_->getOcclusion(level);
+}
+
+
+} // namespace cu
+} // namespace ze
+
diff --git a/RWR/src/ze_oss/imp_cu_correspondence/src/warped_gradients_kernel.cuh b/RWR/src/ze_oss/imp_cu_correspondence/src/warped_gradients_kernel.cuh
new file mode 100644
index 0000000000000000000000000000000000000000..d062c015949d99fcc4d2f972f88235bc1ffdc412
--- /dev/null
+++ b/RWR/src/ze_oss/imp_cu_correspondence/src/warped_gradients_kernel.cuh
@@ -0,0 +1,212 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#ifndef IMP_CU_K_WARPED_GRADIENTS_CUH
+#define IMP_CU_K_WARPED_GRADIENTS_CUH
+
+#include <cuda_runtime_api.h>
+#include <imp/core/types.hpp>
+#include <imp/cuda_toolkit/helper_math.hpp>
+#include <imp/cu_core/cu_pinhole_camera.cuh>
+#include <imp/cu_core/cu_matrix.cuh>
+#include <imp/cu_core/cu_se3.cuh>
+
+
+namespace ze {
+namespace cu {
+
+//------------------------------------------------------------------------------
+template<typename Pixel>
+__global__ void k_warpedGradients(Pixel* ix, Pixel* it, size_t stride,
+                                  uint32_t width, uint32_t height,
+                                  //uint32_t roi_x, uint32_t roi_y,
+                                  Texture2D i1_tex, Texture2D i2_tex, Texture2D u0_tex)
+{
+  const int x = blockIdx.x*blockDim.x + threadIdx.x /*+ roi_x*/;
+  const int y = blockIdx.y*blockDim.y + threadIdx.y /*+ roi_y*/;
+  const int c = y*stride+x;
+
+  if (x<width && y<height)
+  {
+    float u0 = tex2DFetch<float>(u0_tex, x,y);
+    float wx = x+u0;
+
+    float bd = .5f;
+    if ((wx < bd) || (x < bd) || (wx > width-bd-1) || (x > width-bd-1) ||
+        (y<bd) || (y>height-bd-1))
+    {
+      ix[c] =  0.0f;
+      it[c] =  0.0f;
+    }
+    else
+    {
+      Pixel i1_c, i2_w_c, i2_w_m, i2_w_p;
+
+      tex2DFetch(i1_c, i1_tex, x, y);
+
+      tex2DFetch(i2_w_c, i2_tex, wx, y);
+      tex2DFetch(i2_w_m, i2_tex, wx-.5f, y);
+      tex2DFetch(i2_w_p, i2_tex, wx+.5f, y);
+
+      // spatial gradient on warped image
+      ix[c] = i2_w_p - i2_w_m;
+      // temporal gradient between the warped moving image and the fixed image
+      it[c] = i2_w_c - i1_c;
+    }
+
+  }
+}
+
+//------------------------------------------------------------------------------
+template<typename Pixel>
+__global__ void k_warpedGradientsEpipolarConstraint(
+    Pixel* iw, Pixel* ix, Pixel* it, size_t stride,
+    uint32_t width, uint32_t height,
+    cu::PinholeCamera cam1, cu::PinholeCamera cam2,
+    const cu::Matrix3f F_mov_fix, const cu::SE3<float> T_mov_fix,
+    Texture2D i1_tex, Texture2D i2_tex, Texture2D u0_tex,
+    Texture2D depth_proposal_tex)
+{
+  const int x = blockIdx.x*blockDim.x + threadIdx.x /*+ roi_x*/;
+  const int y = blockIdx.y*blockDim.y + threadIdx.y /*+ roi_y*/;
+  const int c = y*stride+x;
+
+  if (x<width && y<height)
+  {
+    // compute epipolar geometry
+    float mu = tex2DFetch<float>(depth_proposal_tex, x, y);
+
+    //    float sigma = sqrtf(depth_proposal_sigma2_tex.fetch<float>(x,y));
+    float2 px_ref = make_float2((float)x, (float)y);
+    float2 px_mean = px_ref;
+
+    float3 px_ref_w = cam1.cam2world(px_ref);
+    float3 f_ref = ::normalize(px_ref_w);
+    if (std::fabs(mu) > 1e-3f)
+      px_mean = cam2.world2cam(T_mov_fix * (f_ref*mu));
+
+    //Vec32fC2 px_p3s = cam2.world2cam(T_mov_fix * (f_ref*(mu + 3.f*sigma)));
+    //float3 px_mean_h = make_float3(px_mean.x, px_mean.y, 1.0f);
+    float3 px_ref_h = make_float3(px_ref.x, px_ref.y, 1.f);
+
+    //    float3 epi_line = F_ref_cur*px_ref_h;
+    //    float3 epi_line = F_ref_cur*px_mean_h;
+    //float2 epi_line_slope = make_float2(epi_line.y, -epi_line.x);
+    //float2 epi_vec = ::normalize(epi_line_slope);
+
+    float3 epi_line = F_mov_fix*px_ref_h;
+    // from line equation: ax+by+c=0 -> y=(-c-ax)/b -> k=-a/b
+    float2 epi_line_slope = make_float2(1.0f, -epi_line.x/epi_line.y);
+    float2 epi_vec = ::normalize(epi_line_slope);
+
+#if 0
+    if(x==20 && y==20)
+    {
+      printf("mu: %f\n", mu);
+      printf("cam: %f, %f; %f, %f\n", cam1.fx(), cam1.fy(), cam1.cx(), cam1.cy());
+      printf("F: %e, %e, %e; %e, %e, %e; %e, %e, %e\n",
+             F_mov_fix(0,0), F_mov_fix(0,1), F_mov_fix(0,2),
+             F_mov_fix(1,0), F_mov_fix(1,1), F_mov_fix(1,2),
+             F_mov_fix(2,0), F_mov_fix(2,1), F_mov_fix(2,2));
+
+      printf("px_ref_w: %f, %f, %f\n", px_ref_w.x, px_ref_w.y, px_ref_w.z);
+
+      printf("f_ref: %f, %f %f\n", f_ref.x, f_ref.y, f_ref.z);
+      float3 bla = T_mov_fix * (f_ref*mu);
+      printf("bla: %f, %f %f\n", bla.x, bla.y, bla.z);
+
+      printf("px_ref: %f, %f\n", px_ref.x, px_ref.y);
+      printf("px_mean: %f, %f\n", px_mean.x, px_mean.y);
+
+      printf("epi_line: %f, %f %f\n", epi_line.x, epi_line.y, epi_line.z);
+      printf("epi_line_slope: %f, %f (length: %f)\n", epi_line_slope.x, epi_line_slope.y, ::length(epi_line_slope));
+      printf("epi_vec: %f, %f (length: %f)\n\n", epi_vec.x, epi_vec.y, ::length(epi_vec));
+    }
+#endif
+
+    float u0 = tex2DFetch<float>(u0_tex, x,y);
+    float2 px_u0 = px_mean + epi_vec*u0; // assuming that epi_vec is the unit vec
+    float2 px_u0_p = px_u0 + 0.5f*epi_vec;
+    float2 px_u0_m = px_u0 - 0.5f*epi_vec;
+
+    float bd = .5f;
+    // check if current mean projects in image /*and mark if not*/
+    // and if warped point is within a certain image area
+    if (
+        (px_mean.x > width-bd-1) || (px_mean.y > height-bd-1) || (px_mean.x < bd) || (px_mean.y < bd) ||
+        (px_u0.x < bd) || (x < bd) || (px_u0.y > width-bd-1) || (x > width-bd-1) ||
+        (px_u0.y < bd) || (px_u0.y > height-bd-1) || (y < bd) || (y > height-bd-1))
+    {
+      ix[c] = 0.0f;
+      it[c] = 0.0f;
+      iw[c] = 0.0f;
+    }
+    else
+    {
+      Pixel i1_c, i2_w_c, i2_w_m, i2_w_p;
+
+
+      tex2DFetch(i1_c, i1_tex, x, y);
+
+      tex2DFetch(i2_w_c, i2_tex, px_u0.x, px_u0.y-.5f*epi_vec.y);
+      tex2DFetch(i2_w_m, i2_tex, px_u0.x-.5f*epi_vec.x, px_u0.y-.5f*epi_vec.y);
+      tex2DFetch(i2_w_p, i2_tex, px_u0.x+.5f*epi_vec.x, px_u0.y+.5f*epi_vec.y);
+
+#if 0
+      i2_tex.fetch(i2_w_c, px_ref.x+epi_vec.x*u0, px_ref.y);
+      i2_tex.fetch(i2_w_m, px_ref.x+epi_vec.x*u0-epi_vec.x*0.5f, px_ref.y-0.5f*epi_vec.y);
+      i2_tex.fetch(i2_w_p, px_ref.x+epi_vec.x*u0+epi_vec.x*0.5f, px_ref.y+0.5f*epi_vec.y);
+#endif
+#if 0
+      float wx = x+u0;
+      i2_tex.fetch(i2_w_c, wx, y);
+      i2_tex.fetch(i2_w_m, wx-epi_vec.x*0.5f, y-epi_vec.y*0.5f);
+      i2_tex.fetch(i2_w_p, wx+epi_vec.x*0.5f, y+epi_vec.y*0.5f);
+#endif
+#if 0
+      float wx = x+u0;
+      i2_tex.fetch(i2_w_c, wx, y);
+      i2_tex.fetch(i2_w_m, wx-0.5f, y);
+      i2_tex.fetch(i2_w_p, wx+0.5f, y);
+#endif
+
+
+      // spatial gradient on warped image
+      ix[c] = i2_w_p - i2_w_m;
+      // temporal gradient between the warped moving image and the fixed image
+      it[c] = i2_w_c - i1_c;
+      // warped image
+      iw[c] = i2_w_c;
+    }
+  }
+}
+
+
+} // namespace cu
+} // namespace ze
+
+
+
+#endif // IMP_CU_K_WARPED_GRADIENTS_CUH
+
diff --git a/RWR/src/ze_oss/imp_cu_correspondence/test/test_dense_variational_epipolar_stereo.cpp b/RWR/src/ze_oss/imp_cu_correspondence/test/test_dense_variational_epipolar_stereo.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..249969ba2c6f36f1e6d33afbc7c9719738bc17ee
--- /dev/null
+++ b/RWR/src/ze_oss/imp_cu_correspondence/test/test_dense_variational_epipolar_stereo.cpp
@@ -0,0 +1,186 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#include <assert.h>
+#include <cstdint>
+#include <iostream>
+#include <memory>
+#include <tuple>
+
+#include <opencv2/core/core.hpp>
+#include <opencv2/highgui/highgui.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
+
+#include <imp/bridge/opencv/cv_bridge.hpp>
+#include <imp/bridge/opencv/cu_cv_bridge.hpp>
+#include <imp/bridge/opencv/image_cv.hpp>
+#include <imp/core/image_raw.hpp>
+#include <imp/core/roi.hpp>
+#include <imp/cu_core/cu_image_gpu.cuh>
+#include <imp/cu_core/cu_math.cuh>
+#include <imp/cu_core/cu_matrix.cuh>
+#include <imp/cu_core/cu_pinhole_camera.cuh>
+#include <imp/cu_core/cu_se3.cuh>
+#include <imp/cu_correspondence/variational_epipolar_stereo.hpp>
+#include <ze/cameras/camera.hpp>
+#include <ze/cameras/camera_rig.hpp>
+#include <ze/common/test_entrypoint.hpp>
+#include <ze/common/test_utils.hpp>
+#include <ze/common/logging.hpp>
+#include <ze/geometry/epipolar_geometry.hpp>
+
+DEFINE_bool(visualize, false, "Show input images and results");
+
+//-----------------------------------------------------------------------------
+// parameters: solver, scale_factor, expected error
+class DenseEpipolarStereoTests
+    : public ::testing::TestWithParam<std::tuple<ze::cu::StereoPDSolver, double, double>>
+{ };
+
+//-----------------------------------------------------------------------------
+TEST_P(DenseEpipolarStereoTests, EpipolarStereoAlgorithms)
+{
+  using namespace ze::cu;
+  using Stereo = ze::cu::VariationalEpipolarStereo;
+  using StereoParameters = Stereo::Parameters;
+
+  // Load two images:
+  std::string data_path = ze::getTestDataDir("synthetic_room_pinhole");
+  ImageGpu32fC1::Ptr cuimg_ref, cuimg_cur;
+  cvBridgeLoad(cuimg_ref, data_path + "/img/1.png", ze::PixelOrder::gray);
+  cvBridgeLoad(cuimg_cur, data_path + "/img/3.png", ze::PixelOrder::gray);
+
+  // Load poses:
+  std::map<int64_t, ze::Transformation> T_W_C_vec =
+      ze::loadIndexedPosesFromCsv(data_path + "/traj_gt.csv");
+  ze::Transformation T_ref_cur = T_W_C_vec[1].inverse() * T_W_C_vec[3];
+
+  VLOG(10) << "load camera";
+  ze::Camera::Ptr cam = ze::cameraFromYaml(data_path + "/calib.yaml");
+
+  const ze::VectorX projection_parameteres = cam->projectionParameters();
+  ze::cu::PinholeCamera cu_cam(projection_parameteres(0), projection_parameteres(1),
+                               projection_parameteres(2), projection_parameteres(3));
+
+  ze::ImageRaw32fC1 depth_ref(cam->width(), cam->height());
+  CHECK_EQ(depth_ref.width(), depth_ref.stride());
+  ze::loadDepthmapFromFile(data_path + "/depth/1.depth",
+                           depth_ref.numel(),
+                           reinterpret_cast<float*>(depth_ref.data()));
+
+  VLOG(10) << "compute fundamental matrix";
+
+  ze::Matrix3 F_ref_cur = ze::fundamentalMatrix(T_ref_cur, projection_parameteres,
+                                                projection_parameteres);
+  ze::cu::Matrix3f cu_F_cur_ref(F_ref_cur.transpose());
+
+  //! @todo (mwe) this also needs to get simpler from cpu transformation to gpu transformation...
+  ze::Quaternion q_cur_ref = T_ref_cur.inverse().getRotation();
+  ze::Vector3 t_cur_ref = T_ref_cur.inverse().getPosition();
+
+  ze::cu::SE3<float> cu_T_cur_ref(
+        static_cast<float>(q_cur_ref.w()), static_cast<float>(q_cur_ref.x()),
+        static_cast<float>(q_cur_ref.y()), static_cast<float>(q_cur_ref.z()),
+        static_cast<float>(t_cur_ref.x()), static_cast<float>(t_cur_ref.y()),
+        static_cast<float>(t_cur_ref.z()));
+
+  VLOG(300) << "(gpu) T_cur_ref: " << cu_T_cur_ref;
+
+  // compute dense stereo
+  StereoParameters::Ptr stereo_params = std::make_shared<StereoParameters>();
+  stereo_params->solver = std::get<0>(GetParam());
+  stereo_params->ctf.scale_factor = std::get<1>(GetParam());
+  stereo_params->ctf.iters = 100;
+  stereo_params->ctf.warps  = 10;
+  stereo_params->ctf.apply_median_filter = true;
+  //stereo->parameters()->lambda = 20;
+
+  std::unique_ptr<Stereo> stereo(new Stereo(stereo_params));
+
+  stereo->addImage(cuimg_ref);
+  stereo->addImage(cuimg_cur);
+
+  stereo->setFundamentalMatrix(cu_F_cur_ref);
+  stereo->setIntrinsics({cu_cam, cu_cam});
+  stereo->setExtrinsics(cu_T_cur_ref);
+
+  stereo->solve();
+
+  ImageGpu32fC1::Ptr cudisp = stereo->getDisparities();
+  CHECK_NOTNULL(cudisp.get());
+  ImageGpu32fC1::Ptr cuocc = stereo->getOcclusion();
+
+  {
+    ze::Pixel32fC1 min_val,max_val;
+    minMax(*cudisp, min_val, max_val);
+    VLOG(2) << "disp: min: " << min_val.x << " max: " << max_val.x;
+  }
+
+
+  if (FLAGS_visualize)
+  {
+    std::stringstream ss_window_name;
+    ss_window_name << "Disparities: ";
+    switch (stereo_params->solver)
+    {
+    case StereoPDSolver::EpipolarPrecondHuberL1:
+      ss_window_name << "Epipolar HL1 (precond.)";
+      break;
+    default:
+      ss_window_name << "unknown solver";
+      break;
+    }
+    ss_window_name << ", " << stereo_params->ctf.scale_factor;
+
+    cvBridgeShow("Reference Image", *cuimg_ref);
+    cvBridgeShow("Current Image", *cuimg_cur);
+    cvBridgeShow(ss_window_name.str(), *cudisp, true);
+    if (cuocc)
+    {
+      cvBridgeShow("Occlusions", *cuocc);
+    }
+    cv::waitKey(0);
+  }
+}
+
+//-----------------------------------------------------------------------------
+//! @todo (MWE) add desired error to make tests more useful.
+using Solver = ze::cu::StereoPDSolver;
+std::tuple<ze::cu::StereoPDSolver, double, double>
+const EpipolarStereoTestsParametrizationTable[] = {
+//  //              solver                           scale_factor  error
+//  std::make_tuple(Solver::EpipolarPrecondHuberL1,  0.5,          0.0),
+  //              solver                           scale_factor  error
+  std::make_tuple(Solver::EpipolarPrecondHuberL1,  0.8,          0.0),
+  //              solver                           scale_factor  error
+  std::make_tuple(Solver::EpipolarPrecondHuberL1,  0.95,         0.0),
+};
+
+//-----------------------------------------------------------------------------
+INSTANTIATE_TEST_CASE_P(
+  DenseEpipolarStereoTests, DenseEpipolarStereoTests,
+    ::testing::ValuesIn(EpipolarStereoTestsParametrizationTable));
+
+
+ZE_UNITTEST_ENTRYPOINT
diff --git a/RWR/src/ze_oss/imp_cu_correspondence/test/test_dense_variational_stereo.cpp b/RWR/src/ze_oss/imp_cu_correspondence/test/test_dense_variational_stereo.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..624084dc72909cb8b8640f5a5a6f70915d139075
--- /dev/null
+++ b/RWR/src/ze_oss/imp_cu_correspondence/test/test_dense_variational_stereo.cpp
@@ -0,0 +1,229 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#include <assert.h>
+#include <cstdint>
+#include <iostream>
+#include <memory>
+#include <tuple>
+
+#include <ze/common/test_entrypoint.hpp>
+#include <ze/common/test_utils.hpp>
+
+#include <opencv2/core/core.hpp>
+#include <opencv2/highgui/highgui.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
+
+#include <ze/common/test_entrypoint.hpp>
+#include <ze/common/test_utils.hpp>
+
+#include <imp/core/roi.hpp>
+#include <imp/core/image_raw.hpp>
+#include <imp/bridge/opencv/image_cv.hpp>
+#include <imp/cu_core/cu_image_gpu.cuh>
+#include <imp/cu_core/cu_math.cuh>
+#include <imp/bridge/opencv/cv_bridge.hpp>
+#include <imp/bridge/opencv/cu_cv_bridge.hpp>
+
+#include <imp/cu_correspondence/variational_stereo.hpp>
+
+DEFINE_bool(visualize, false, "Show input images and results.");
+DEFINE_bool(writePFM, true, "Write disparity output as PFM file.");
+
+//-----------------------------------------------------------------------------
+// check whether machine is little endian
+int littleendian()
+{
+  int intval = 1;
+  uchar *uval = (uchar*)&intval;
+  return uval[0] == 1;
+}
+
+//-----------------------------------------------------------------------------
+// write pfm image
+// 1-band PFM image, see http://netpbm.sourceforge.net/doc/pfm.html
+void writePFM(const ze::ImageRaw32fC1& disp, const std::string& filename,
+              float scalefactor=1.f/255.f)
+{
+  // Open the file
+  FILE *stream = fopen(filename.c_str(), "wb");
+  CHECK(stream != 0) << "writePFM could not open File " << filename;
+
+  // sign of scalefact indicates endianness, see pfms specs
+  if (littleendian())
+  {
+    scalefactor = -scalefactor;
+  }
+
+  // write the header: 3 lines: Pf, dimensions, scale factor (negative val == little endian)
+  fprintf(stream, "Pf\n%d %d\n%f\n", disp.width(), disp.height(), scalefactor);
+
+
+  for (uint32_t y = disp.height(); y-- > 0;)
+  {
+    for (uint32_t x = 0; x < disp.width(); ++x)
+    {
+      float disp_value = -static_cast<float>(disp(x,y));
+      if (disp_value < 0.f)
+      {
+        disp_value = INFINITY;
+      }
+      CHECK(1 == static_cast<uint32_t>(
+              fwrite(&disp_value, sizeof(float), 1, stream)));
+    }
+  }
+  fclose(stream);
+}
+
+//-----------------------------------------------------------------------------
+// parameters: solver, scale_factor, expected error
+class DenseStereoTests
+    : public ::testing::TestWithParam<std::tuple<ze::cu::StereoPDSolver, double, double>>
+{ };
+
+//-----------------------------------------------------------------------------
+TEST_P(DenseStereoTests, StereoAlgorithms)
+{
+  using namespace ze::cu;
+  using Stereo = ze::cu::VariationalStereo;
+  using StereoParameters = Stereo::Parameters;
+
+  // Load two images:
+  std::string data_path = ze::getTestDataDir("computer_vision_images");
+  ImageGpu32fC1::Ptr cuimg_ref, cuimg_cur;
+  cvBridgeLoad(cuimg_ref, data_path + "/middlebury/trainingQ/Teddy/im0.png", ze::PixelOrder::gray);
+  cvBridgeLoad(cuimg_cur, data_path + "/middlebury/trainingQ/Teddy/im1.png", ze::PixelOrder::gray);
+
+  // compute dense stereo
+  StereoParameters::Ptr stereo_params = std::make_shared<StereoParameters>();
+  stereo_params->solver = std::get<0>(GetParam());
+  stereo_params->ctf.scale_factor = std::get<1>(GetParam());;
+
+  std::unique_ptr<Stereo> stereo(new Stereo(stereo_params));
+
+  stereo->addImage(cuimg_ref);
+  stereo->addImage(cuimg_cur);
+
+  stereo->solve();
+
+  ImageGpu32fC1::Ptr cudisp = stereo->getDisparities();
+  CHECK_NOTNULL(cudisp.get());
+  ImageGpu32fC1::Ptr cuocc = stereo->getOcclusion();
+
+  {
+    ze::Pixel32fC1 min_val,max_val;
+    minMax(*cudisp, min_val, max_val);
+    VLOG(2) << "disp: min: " << min_val.x << " max: " << max_val.x;
+  }
+
+
+  if (FLAGS_writePFM)
+  {
+    std::stringstream ss_pfm_filename;
+    ss_pfm_filename << data_path << "/middlebury/trainingQ/Teddy/disp0";
+    std::string algorithm_short_string="?";
+    switch (stereo_params->solver)
+    {
+    case StereoPDSolver::HuberL1:
+      ss_pfm_filename << "HL1";
+      break;
+    case StereoPDSolver::PrecondHuberL1:
+      ss_pfm_filename << "HL1p";
+      break;
+    case StereoPDSolver::PrecondHuberL1Weighted:
+      ss_pfm_filename << "HL1p-w";
+      break;
+    default:
+      ss_pfm_filename << "unknown";
+      break;
+    }
+    ss_pfm_filename << "_sf";
+    ss_pfm_filename << stereo_params->ctf.scale_factor;
+    ss_pfm_filename << ".pfm";
+    std::string pfm_filename = ss_pfm_filename.str();
+
+    ze::ImageRaw32fC1::Ptr disp = std::make_shared<ze::ImageRaw32fC1>(*cudisp);
+    VLOG(1) << "Writing disparities to '" << pfm_filename << "'.";
+    writePFM(*disp, pfm_filename, 1.f/55.f);
+  }
+
+  if (FLAGS_visualize)
+  {
+    std::stringstream ss_window_name;
+    ss_window_name << "Disparities: ";
+    switch (stereo_params->solver)
+    {
+    case StereoPDSolver::HuberL1:
+      ss_window_name << "HL1";
+      break;
+    case StereoPDSolver::PrecondHuberL1:
+      ss_window_name << "HL1 (precond.)";
+      break;
+    case StereoPDSolver::PrecondHuberL1Weighted:
+      ss_window_name << "HL1-weighted (precond.)";
+      break;
+    default:
+      ss_window_name << "unknown solver";
+      break;
+    }
+    ss_window_name << ", " << stereo_params->ctf.scale_factor;
+
+    cvBridgeShow("Reference Image", *cuimg_ref);
+    cvBridgeShow("Current Image", *cuimg_cur);
+    cvBridgeShow(ss_window_name.str(), *cudisp, true);
+    if (cuocc)
+    {
+      cvBridgeShow("Occlusions", *cuocc);
+    }
+    cv::waitKey(0);
+  }
+}
+
+//-----------------------------------------------------------------------------
+//! @todo (MWE) add desired error to make tests more useful.
+using Solver = ze::cu::StereoPDSolver;
+std::tuple<ze::cu::StereoPDSolver, double, double> const
+StereoTestsParametrizationTable[] =
+{
+  //              solver                           scale_factor  error
+  std::make_tuple(Solver::HuberL1,                 0.5,          0.0),
+  std::make_tuple(Solver::PrecondHuberL1,          0.5,          0.0),
+  std::make_tuple(Solver::PrecondHuberL1Weighted,  0.5,          0.0),
+  //              solver                           scale_factor  error
+  std::make_tuple(Solver::HuberL1,                 0.8,          1.23),
+  std::make_tuple(Solver::PrecondHuberL1,          0.8,          1.23),
+  std::make_tuple(Solver::PrecondHuberL1Weighted,  0.8,          1.33),
+  //              solver                           scale_factor  error
+  std::make_tuple(Solver::HuberL1,                 0.95,         0.0),
+  std::make_tuple(Solver::PrecondHuberL1,          0.95,         0.0),
+  std::make_tuple(Solver::PrecondHuberL1Weighted,  0.95,         0.0),
+};
+
+//-----------------------------------------------------------------------------
+INSTANTIATE_TEST_CASE_P(
+    DenseStereoSolverTests, DenseStereoTests,
+    ::testing::ValuesIn(StereoTestsParametrizationTable));
+
+
+ZE_UNITTEST_ENTRYPOINT
diff --git a/RWR/src/ze_oss/imp_cu_imgproc/CATKIN_IGNORE b/RWR/src/ze_oss/imp_cu_imgproc/CATKIN_IGNORE
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/RWR/src/ze_oss/imp_cu_imgproc/CMakeLists.txt b/RWR/src/ze_oss/imp_cu_imgproc/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..a1c8d56afd0eac8adbe3b118fcc7c03f94586003
--- /dev/null
+++ b/RWR/src/ze_oss/imp_cu_imgproc/CMakeLists.txt
@@ -0,0 +1,80 @@
+project(imp_cu_imgproc)
+cmake_minimum_required(VERSION 2.8.0)
+
+if(${CMAKE_MAJOR_VERSION} VERSION_GREATER 3.0)
+  cmake_policy(SET CMP0054 OLD)
+endif(${CMAKE_MAJOR_VERSION} VERSION_GREATER 3.0)
+
+find_package(catkin_simple REQUIRED)
+catkin_simple(ALL_DEPS_REQUIRED)
+
+include(ze_setup)
+include(ze_macros_cuda)
+find_cuda()
+
+set(HEADERS
+  )
+
+set(SOURCES
+  )
+
+set(CU_HDRS
+  include/imp/cu_imgproc/image_pyramid.hpp
+  include/imp/cu_imgproc/cu_reduce.cuh
+  include/imp/cu_imgproc/cu_resample.cuh
+  include/imp/cu_imgproc/cu_image_filter.cuh
+  include/imp/cu_imgproc/edge_detectors.cuh
+  include/imp/cu_imgproc/cu_variational_denoising.cuh
+  include/imp/cu_imgproc/cu_rof_denoising.cuh
+  include/imp/cu_imgproc/cu_tvl1_denoising.cuh
+  include/imp/cu_imgproc/str_tex_decomposer.hpp
+  include/imp/cu_imgproc/cu_remap.cuh
+  include/imp/cu_imgproc/cu_undistortion.cuh
+  include/imp/cu_imgproc/cu_stereo_rectification.cuh
+  include/imp/cu_imgproc/cu_horizontal_stereo_pair_rectifier.cuh
+  )
+
+set(CU_SRCS
+  src/image_pyramid.cpp
+  src/cu_reduce.cu
+  src/cu_resample.cu
+  src/cu_median3x3_filter.cu
+  src/cu_gauss_filter.cu
+  src/cu_natural_edges.cu
+  src/cu_variational_denoising.cu
+  src/cu_rof_denoising.cu
+  src/cu_tvl1_denoising.cu
+  src/str_tex_decomposer.cpp
+  src/cu_undistortion.cu
+  src/cu_stereo_rectification.cu
+  src/cu_horizontal_stereo_pair_rectifier.cu
+  )
+
+cs_cuda_add_library(${PROJECT_NAME}
+   ${CU_SRCS} ${CU_HDRS}
+   ${SOURCES} ${HEADERS}
+   )
+
+##########
+# GTESTS #
+##########
+catkin_add_gtest(test_cu_image_pyramid test/test_cu_image_pyramid.cpp)
+target_link_libraries(test_cu_image_pyramid ${PROJECT_NAME})
+
+catkin_add_gtest(test_cu_rof test/test_cu_rof.cpp)
+target_link_libraries(test_cu_rof ${PROJECT_NAME})
+
+catkin_add_gtest(test_cu_str_tex_decomp test/test_cu_str_tex_decomp.cpp)
+target_link_libraries(test_cu_str_tex_decomp ${PROJECT_NAME})
+
+catkin_add_gtest(test_cu_undistortion test/test_cu_undistortion.cpp)
+target_link_libraries(test_cu_undistortion ${PROJECT_NAME})
+
+catkin_add_gtest(test_cu_stereo_rectifier test/test_cu_stereo_rectifier.cpp)
+target_link_libraries(test_cu_stereo_rectifier ${PROJECT_NAME})
+
+##########
+# EXPORT #
+##########
+cs_install()
+cs_export()
diff --git a/RWR/src/ze_oss/imp_cu_imgproc/include/imp/cu_imgproc/cu_horizontal_stereo_pair_rectifier.cuh b/RWR/src/ze_oss/imp_cu_imgproc/include/imp/cu_imgproc/cu_horizontal_stereo_pair_rectifier.cuh
new file mode 100644
index 0000000000000000000000000000000000000000..e9f5c13447c275c5fffc673ad972e63b86ecde8a
--- /dev/null
+++ b/RWR/src/ze_oss/imp_cu_imgproc/include/imp/cu_imgproc/cu_horizontal_stereo_pair_rectifier.cuh
@@ -0,0 +1,87 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#pragma once
+
+#include <imp/cu_imgproc/cu_stereo_rectification.cuh>
+#include <ze/geometry/epipolar_geometry.hpp>
+
+namespace ze {
+namespace cu {
+
+//! A HorizontalStereoPairRectifier is used to rectify images acquired by a fully calibrated
+//! camera pair in horizontal stereo setting.
+template<typename CameraModel,
+         typename DistortionModel,
+         typename Pixel>
+class HorizontalStereoPairRectifier
+{
+public:
+  //! \brief HorizontalStereoPairRectifier
+  //! \param transformed_cam0_params The output camera 0 parameters [fx, fy, cx, cy]^T
+  //! in the rectified reference frame.
+  //! \param transformed_cam1_params The output camera 1 parameters [fx, fy, cx, cy]^T
+  //! in the rectified reference frame.
+  //! \param horizontal_offset Output horizontal offset in the rectified reference system.
+  //! \param img_size The size of the images to rectify.
+  //! \param cam0_params The camera parameters [fx, fy, cx, cy]^T for the camera 0
+  //! \param cam0_dist_coeffs The distortion coefficients for the camera 0
+  //! \param cam1_params The camera parameters [fx, fy, cx, cy]^T for the camera 1
+  //! \param cam1_dist_coeffs The distortion coefficients for the camera 1.
+  //! \param T_cam1_cam0 transformation from cam0 to cam1 reference system.
+  HorizontalStereoPairRectifier(Vector4& transformed_cam0_params,
+                                Vector4& transformed_cam1_params,
+                                real_t& horizontal_offset,
+                                const Size2u& img_size,
+                                const Vector4& cam0_params,
+                                const Vector4& cam0_dist_coeffs,
+                                const Vector4& cam1_params,
+                                const Vector4& cam1_dist_coeffs,
+                                const Transformation& T_cam1_cam0);
+
+  ~HorizontalStereoPairRectifier() = default;
+
+  //! \brief Run rectification
+  //! \param cam0_dst Destination image to store the rectified camera 0 image in GPU memory.
+  //! \param cam1_dst Destination image to store the rectified camera 1 image in GPU memory.
+  //! \param cam0_src The source camera 0 image in GPU memory
+  //! \param cam1_src The source camera 1 image in GPU memory
+  void rectify(ImageGpu<Pixel>& cam0_dst,
+               ImageGpu<Pixel>& cam1_dst,
+               const ImageGpu<Pixel>& cam0_src,
+               const ImageGpu<Pixel>& cam1_src) const;
+
+  //! \brief Retrieves the computed undistortion-rectification maps
+  //! \param cam_idx Camera index in (0, 1)
+  const ImageGpu32fC2& getUndistortRectifyMap(int8_t cam_idx) const;
+
+private:
+  std::unique_ptr<StereoRectifier<CameraModel, DistortionModel, Pixel>> rectifiers_[2];
+};
+
+using HorizontalStereoPairRectifierEquidist32fC1 = HorizontalStereoPairRectifier<PinholeGeometry, EquidistantDistortion, Pixel32fC1>;
+using HorizontalStereoPairRectifierRadTan32fC1 = HorizontalStereoPairRectifier<PinholeGeometry, RadialTangentialDistortion, Pixel32fC1>;
+
+} // cu namespace
+} // ze namespace
diff --git a/RWR/src/ze_oss/imp_cu_imgproc/include/imp/cu_imgproc/cu_image_filter.cuh b/RWR/src/ze_oss/imp_cu_imgproc/include/imp/cu_imgproc/cu_image_filter.cuh
new file mode 100644
index 0000000000000000000000000000000000000000..4e465aade69522ed33738aa141e31cccc13ce203
--- /dev/null
+++ b/RWR/src/ze_oss/imp_cu_imgproc/include/imp/cu_imgproc/cu_image_filter.cuh
@@ -0,0 +1,65 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#pragma once
+
+#include <imp/core/types.hpp>
+#include <imp/core/pixel_enums.hpp>
+#include <imp/cu_core/cu_image_gpu.cuh>
+
+namespace ze {
+namespace cu {
+
+//-----------------------------------------------------------------------------
+/** filterMedian3x3 performs a median filter on a 3x3 window
+ *
+ */
+template<typename Pixel>
+void filterMedian3x3(ImageGpu<Pixel>& dst,
+                     const ImageGpu<Pixel>& src);
+
+//-----------------------------------------------------------------------------
+/** filterGauss performs a gaussian smoothing filter on the given input image \a src
+ * @param[out] dst Gauss filtered result image
+ * @pram[in] src Input image on the GPU (CUDA memory)
+ * @param[in] sigma Gaussian kernel standard deviation
+ * @param[in] kernel_size Gaussian filter kernel size. (if default (0) computed automatically)
+ * @param[inout] tmp_image optinal temp. image to avoid memory reallocation for multiple calls of the Gaussian filtering
+ */
+template<typename Pixel>
+void filterGauss(ImageGpu<Pixel>& dst,
+                 const ImageGpu<Pixel>& src,
+                 float sigma, int kernel_size=0,
+                 ImageGpuPtr<Pixel> tmp_img=nullptr);
+//                 cudaStream_t stream);
+
+template<typename Pixel>
+void filterGauss(ImageGpu<Pixel>& dst,
+                 const Texture2D& src_tex,
+                 float sigma, int kernel_size=0,
+                 ImageGpuPtr<Pixel> tmp_img=nullptr);
+//                 cudaStream_t stream);
+
+} // namespace cu
+} // namespace ze
diff --git a/RWR/src/ze_oss/imp_cu_imgproc/include/imp/cu_imgproc/cu_reduce.cuh b/RWR/src/ze_oss/imp_cu_imgproc/include/imp/cu_imgproc/cu_reduce.cuh
new file mode 100644
index 0000000000000000000000000000000000000000..27858ae1da7656a4f2ad9b6deec50b2bf31780f2
--- /dev/null
+++ b/RWR/src/ze_oss/imp_cu_imgproc/include/imp/cu_imgproc/cu_reduce.cuh
@@ -0,0 +1,55 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#pragma once
+
+#include <imp/core/types.hpp>
+#include <imp/core/pixel_enums.hpp>
+#include <imp/cu_core/cu_image_gpu.cuh>
+
+namespace ze {
+namespace cu {
+
+/**
+ * @brief Image reduction from \a src to \a dst image
+ */
+template<typename Pixel>
+void reduce(ImageGpu<Pixel>& dst,
+            const ImageGpu<Pixel>& src,
+            InterpolationMode interp = InterpolationMode::Linear,
+            bool gauss_prefilter=true);
+
+
+// /**
+//  * @brief Image reduction from \a src to \a dst image
+//  */
+// template<typename Pixel, imp::PixelType pixel_type>
+// void resample(ImageGpu<Pixel, pixel_type>& dst,
+//               const ImageGpu<Pixel, pixel_type>& src,
+//               InterpolationMode interp = InterpolationMode::Linear,
+//               bool gauss_prefilter=false);
+
+
+} // namespace cu
+} // namespace ze
diff --git a/RWR/src/ze_oss/imp_cu_imgproc/include/imp/cu_imgproc/cu_remap.cuh b/RWR/src/ze_oss/imp_cu_imgproc/include/imp/cu_imgproc/cu_remap.cuh
new file mode 100644
index 0000000000000000000000000000000000000000..c4dd5448d4853b0a936e034af917a00c9d001b5b
--- /dev/null
+++ b/RWR/src/ze_oss/imp_cu_imgproc/include/imp/cu_imgproc/cu_remap.cuh
@@ -0,0 +1,57 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#pragma once
+
+#include <imp/cu_core/cu_image_gpu.cuh>
+#include <imp/cu_core/cu_texture.cuh>
+
+namespace ze {
+namespace cu {
+
+template<typename T>
+__global__
+void k_remap(
+    Pixel1<T>* dst,
+    size_t dst_stride,
+    const Pixel32fC2* map,
+    size_t map_stride,
+    int32_t width,
+    int32_t height,
+    Texture2D src)
+{
+  const int x = blockIdx.x*blockDim.x + threadIdx.x;
+  const int y = blockIdx.y*blockDim.y + threadIdx.y;
+
+  if (x < width && y < height)
+  {
+    Pixel1<T> val;
+    const Pixel32fC2& px = map[y*map_stride+x];
+    tex2DFetch(val, src, px[0], px[1]);
+    dst[y*dst_stride+x] = val;
+  }
+}
+
+} // cu namespace
+} // ze namespace
diff --git a/RWR/src/ze_oss/imp_cu_imgproc/include/imp/cu_imgproc/cu_resample.cuh b/RWR/src/ze_oss/imp_cu_imgproc/include/imp/cu_imgproc/cu_resample.cuh
new file mode 100644
index 0000000000000000000000000000000000000000..56f9e641ee695b6d9864ca9758ea12b028433ef6
--- /dev/null
+++ b/RWR/src/ze_oss/imp_cu_imgproc/include/imp/cu_imgproc/cu_resample.cuh
@@ -0,0 +1,45 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#pragma once
+
+#include <imp/core/types.hpp>
+#include <imp/core/pixel_enums.hpp>
+#include <imp/cu_core/cu_image_gpu.cuh>
+
+namespace ze {
+namespace cu {
+
+/**
+ * @brief Image resampling from \a src to \a dst image
+ */
+template<typename Pixel>
+void resample(ImageGpu<Pixel>& dst,
+              const ImageGpu<Pixel>& src,
+              InterpolationMode interp = InterpolationMode::Linear,
+              bool gauss_prefilter=false);
+
+
+} // namespace cu
+} // namespace ze
diff --git a/RWR/src/ze_oss/imp_cu_imgproc/include/imp/cu_imgproc/cu_rof_denoising.cuh b/RWR/src/ze_oss/imp_cu_imgproc/include/imp/cu_imgproc/cu_rof_denoising.cuh
new file mode 100644
index 0000000000000000000000000000000000000000..caecb209480a90b80a36fbe4499e93531565f9b7
--- /dev/null
+++ b/RWR/src/ze_oss/imp_cu_imgproc/include/imp/cu_imgproc/cu_rof_denoising.cuh
@@ -0,0 +1,79 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#pragma once
+
+#include <imp/cu_imgproc/cu_variational_denoising.cuh>
+
+#include <memory>
+#include <cuda_runtime_api.h>
+
+#include <imp/cu_core/cu_image_gpu.cuh>
+#include <imp/cu_core/cu_utils.hpp>
+#include <ze/common/macros.hpp>
+
+namespace ze {
+namespace cu {
+
+template<typename Pixel>
+class RofDenoising  : public ze::cu::VariationalDenoising
+{
+public:
+  ZE_POINTER_TYPEDEFS(RofDenoising);
+  using Base = VariationalDenoising;
+  using ImageGpu = ze::cu::ImageGpu<Pixel>;
+
+public:
+  RofDenoising() = default;
+  virtual ~RofDenoising() = default;
+  using Base::Base;
+
+  virtual void denoise(const ze::ImageBase::Ptr& dst,
+                       const ze::ImageBase::Ptr& src) override;
+
+  void primalDualEnergy(double& primal_energy, double& dual_energy);
+
+protected:
+  virtual void init(const Size2u& size) override;
+  virtual void print(std::ostream &os) const override;
+
+private:
+  typename ImageGpu::Ptr f_;
+
+  // pixel-wise primal and dual energies to avoid allocation of memory for every check
+  std::unique_ptr<ImageGpu32fC1> primal_energies_;
+  std::unique_ptr<ImageGpu32fC1> dual_energies_;
+};
+
+//-----------------------------------------------------------------------------
+// convenience typedefs
+// (sync with explicit template class instantiations at the end of the cpp file)
+typedef RofDenoising<ze::Pixel8uC1> RofDenoising8uC1;
+typedef RofDenoising<ze::Pixel32fC1> RofDenoising32fC1;
+
+template <typename Pixel>
+using RofDenoisingPtr = typename std::shared_ptr<RofDenoising<Pixel>>;
+
+} // namespace cu
+} // namespace ze
diff --git a/RWR/src/ze_oss/imp_cu_imgproc/include/imp/cu_imgproc/cu_stereo_rectification.cuh b/RWR/src/ze_oss/imp_cu_imgproc/include/imp/cu_imgproc/cu_stereo_rectification.cuh
new file mode 100644
index 0000000000000000000000000000000000000000..46d688d0edb40334c0d6026c44418628509c7dd9
--- /dev/null
+++ b/RWR/src/ze_oss/imp_cu_imgproc/include/imp/cu_imgproc/cu_stereo_rectification.cuh
@@ -0,0 +1,74 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#pragma once
+
+#include <imp/cu_core/cu_image_gpu.cuh>
+#include <imp/cu_core/cu_utils.hpp>
+#include <ze/cameras/camera_models.hpp>
+
+namespace ze {
+namespace cu {
+
+//! \brief A StereoRectifier can rectify images using the GPU
+template<typename CameraModel,
+         typename DistortionModel,
+         typename Pixel>
+class StereoRectifier
+{
+public:
+  //! \brief StereoRectifier
+  //! \param img_size The size of the images to rectify.
+  //! \param camera_params The camera parameters [fx, fy, cx, cy]^T.
+  //! \param transformed_camera_params The camera parameters
+  //! [fx, fy, cx, cy]^T in the rectified reference frame.
+  //! \param dist_coeffs Camera distortion coefficients.
+  //! \param inv_H Inverse of the rectifying homography.
+  StereoRectifier(const Size2u& img_size,
+                  const Vector4& camera_params,
+                  const Vector4& transformed_camera_params,
+                  const Vector4& dist_coeffs,
+                  const Matrix3& inv_H);
+
+  ~StereoRectifier() = default;
+
+  //! \brief Rectify
+  //! \param dst Destination image to store rectification result in GPU memory.
+  //! \param src The image to rectify in GPU memory
+  void rectify(ImageGpu<Pixel>& dst,
+               const ImageGpu<Pixel>& src) const;
+
+  //! \brief Retrieves the computed undistortion-rectification map
+  const ImageGpu32fC2& getUndistortRectifyMap() const;
+
+private:
+  ImageGpu32fC2 undistort_rectify_map_;   //!< The (2-channels) undistortion-rectification map
+  Fragmentation<16, 16> fragm_;           //!< @todo (MPI) opptimize CUDA thread grid config
+};
+
+using EquidistStereoRectifier32fC1 = StereoRectifier<PinholeGeometry, EquidistantDistortion, Pixel32fC1>;
+using RadTanStereoRectifier32fC1 = StereoRectifier<PinholeGeometry, RadialTangentialDistortion, Pixel32fC1>;
+
+} // cu namespace
+} // ze namespace
diff --git a/RWR/src/ze_oss/imp_cu_imgproc/include/imp/cu_imgproc/cu_tvl1_denoising.cuh b/RWR/src/ze_oss/imp_cu_imgproc/include/imp/cu_imgproc/cu_tvl1_denoising.cuh
new file mode 100644
index 0000000000000000000000000000000000000000..2075e559ebc83f300fe0a15d456afa790ecbf704
--- /dev/null
+++ b/RWR/src/ze_oss/imp_cu_imgproc/include/imp/cu_imgproc/cu_tvl1_denoising.cuh
@@ -0,0 +1,73 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#pragma once
+
+#include <imp/cu_imgproc/cu_variational_denoising.cuh>
+
+#include <memory>
+#include <cuda_runtime_api.h>
+
+#include <imp/cu_core/cu_image_gpu.cuh>
+#include <imp/cu_core/cu_utils.hpp>
+
+namespace ze {
+namespace cu {
+
+template<typename Pixel>
+class TvL1Denoising  : public ze::cu::VariationalDenoising
+{
+public:
+  using Ptr = std::shared_ptr<TvL1Denoising<Pixel>>;
+  using Base = VariationalDenoising;
+  using ImageGpu = ze::cu::ImageGpu<Pixel>;
+
+public:
+  TvL1Denoising() = default;
+  virtual ~TvL1Denoising() = default;
+  using Base::Base;
+
+  virtual void __host__ denoise(const ImageBase::Ptr& dst,
+                                const ImageBase::Ptr& src) override;
+
+protected:
+  virtual void init(const Size2u& size) override;
+  virtual void print(std::ostream &os) const override;
+
+private:
+  typename ImageGpu::Ptr f_;
+
+};
+
+//-----------------------------------------------------------------------------
+// convenience typedefs
+// (sync with explicit template class instantiations at the end of the cpp file)
+typedef TvL1Denoising<ze::Pixel8uC1> TvL1Denoising8uC1;
+typedef TvL1Denoising<ze::Pixel32fC1> TvL1Denoising32fC1;
+
+template <typename Pixel>
+using TvL1DenoisingPtr = typename std::shared_ptr<TvL1Denoising<Pixel>>;
+
+} // namespace cu
+} // namespace ze
diff --git a/RWR/src/ze_oss/imp_cu_imgproc/include/imp/cu_imgproc/cu_undistortion.cuh b/RWR/src/ze_oss/imp_cu_imgproc/include/imp/cu_imgproc/cu_undistortion.cuh
new file mode 100644
index 0000000000000000000000000000000000000000..ab503fa96b8e7c50f6df5931b87163a9bf73b275
--- /dev/null
+++ b/RWR/src/ze_oss/imp_cu_imgproc/include/imp/cu_imgproc/cu_undistortion.cuh
@@ -0,0 +1,62 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#pragma once
+
+#include <imp/cu_core/cu_image_gpu.cuh>
+#include <imp/cu_core/cu_utils.hpp>
+#include <ze/cameras/camera_models.hpp>
+
+namespace ze {
+namespace cu {
+
+template<typename CameraModel,
+         typename DistortionModel,
+         typename Pixel>
+class ImageUndistorter
+{
+public:
+  ImageUndistorter(
+      const Size2u& img_size,
+      const VectorX& camera_params,
+      const VectorX& dist_coeffs);
+
+  ~ImageUndistorter() = default;
+
+  void undistort(
+      ImageGpu<Pixel>& dst,
+      const ImageGpu<Pixel>& src) const;
+
+  const ImageGpu32fC2& getUndistortionMap() const;
+
+private:
+  ImageGpu32fC2 undistortion_map_;
+  Fragmentation<16, 16> fragm_;
+};
+
+using EquidistUndistort32fC1 = cu::ImageUndistorter<PinholeGeometry, EquidistantDistortion, Pixel32fC1>;
+using RadTanUndistort32fC1 = cu::ImageUndistorter<PinholeGeometry, RadialTangentialDistortion, Pixel32fC1>;
+
+} // cu namespace
+} // ze namespace
diff --git a/RWR/src/ze_oss/imp_cu_imgproc/include/imp/cu_imgproc/cu_variational_denoising.cuh b/RWR/src/ze_oss/imp_cu_imgproc/include/imp/cu_imgproc/cu_variational_denoising.cuh
new file mode 100644
index 0000000000000000000000000000000000000000..755ab86478be56ac3029c87e63ad503bab7e45c9
--- /dev/null
+++ b/RWR/src/ze_oss/imp_cu_imgproc/include/imp/cu_imgproc/cu_variational_denoising.cuh
@@ -0,0 +1,112 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#pragma once
+
+#include <memory>
+#include <cuda_runtime_api.h>
+#include <imp/core/image_base.hpp>
+#include <imp/cu_core/cu_image_gpu.cuh>
+#include <imp/cu_core/cu_utils.hpp>
+#include <ze/common/macros.hpp>
+
+namespace ze {
+namespace cu {
+
+// forward declarations
+class Texture2D;
+
+struct VariationalDenoisingParams
+{
+  float lambda = 10.f;
+  std::uint16_t max_iter = 100;
+  std::uint16_t primal_dual_energy_check_iter = 0;
+  double primal_dual_gap_tolerance = 0.0;
+};
+
+/**
+ * @brief The VariationalDenoising class
+ */
+class VariationalDenoising
+{
+public:
+  ZE_POINTER_TYPEDEFS(VariationalDenoising);
+  typedef ze::cu::Fragmentation<16> Fragmentation;
+  typedef std::shared_ptr<Fragmentation> FragmentationPtr;
+
+public:
+  VariationalDenoising();
+  virtual ~VariationalDenoising();
+
+  virtual void  __host__  denoise(const ze::ImageBase::Ptr& dst,
+                                  const ze::ImageBase::Ptr& src) = 0;
+
+  inline dim3 dimGrid() {return fragmentation_->dimGrid;}
+  inline dim3 dimBlock() {return fragmentation_->dimBlock;}
+  virtual inline VariationalDenoisingParams& params() { return params_; }
+
+
+  friend std::ostream& operator<<(std::ostream& os,
+                                  const VariationalDenoising& rhs);
+
+protected:
+  virtual void init(const Size2u& size);
+  inline virtual void print(std::ostream& os) const
+  {
+    //os << "  size: " << this->size_ << std::endl
+    os << "  lambda: " << this->params_.lambda << std::endl
+       << "  max_iter: " << this->params_.max_iter << std::endl
+       << "  primal_dual_energy_check_iter: " << this->params_.primal_dual_energy_check_iter << std::endl
+       << "  primal_dual_gap_tolerance: " << this->params_.primal_dual_gap_tolerance << std::endl
+       << std::endl;
+  }
+
+
+  ze::cu::ImageGpu32fC1::Ptr u_;
+  ze::cu::ImageGpu32fC1::Ptr u_prev_;
+  ze::cu::ImageGpu32fC2::Ptr p_;
+
+  // cuda textures
+  std::shared_ptr<ze::cu::Texture2D> f_tex_;
+  std::shared_ptr<ze::cu::Texture2D> u_tex_;
+  std::shared_ptr<ze::cu::Texture2D> u_prev_tex_;
+  std::shared_ptr<ze::cu::Texture2D> p_tex_;
+
+  Size2u size_;
+  FragmentationPtr fragmentation_;
+
+  // algorithm parameters
+  VariationalDenoisingParams params_;
+};
+
+inline std::ostream& operator<<(std::ostream& os,
+                                const VariationalDenoising& rhs)
+{
+  rhs.print(os);
+  return os;
+}
+
+
+} // namespace cu
+} // namespace ze
diff --git a/RWR/src/ze_oss/imp_cu_imgproc/include/imp/cu_imgproc/edge_detectors.cuh b/RWR/src/ze_oss/imp_cu_imgproc/include/imp/cu_imgproc/edge_detectors.cuh
new file mode 100644
index 0000000000000000000000000000000000000000..58a94020c378af9638dd4de216369723cf778caa
--- /dev/null
+++ b/RWR/src/ze_oss/imp_cu_imgproc/include/imp/cu_imgproc/edge_detectors.cuh
@@ -0,0 +1,50 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#pragma once
+
+#include <imp/core/types.hpp>
+#include <imp/core/pixel_enums.hpp>
+#include <imp/cu_core/cu_image_gpu.cuh>
+
+
+namespace ze {
+namespace cu {
+
+/** Compute 'natural' image edges as a function like g = exp(-alpha*(norm)^q)
+ * Dependending on the dimension of the edge image the function computes the edge's
+ * magnitude (EdgePixel == Pixel1), its direction (EdgePixel == Pixel2) or a
+ * 3-dimensional tensor (EdgePixel == Pixel3 or Pixel4).
+ *
+ * @todo (MWE) add a tmp image as input param so we don't have to allocate memory
+ *             for the internal denoising all the time.
+ */
+template<typename Pixel>
+void naturalEdges(ImageGpu<Pixel>& dst,
+                  const ImageGpu<Pixel>& src,
+                  float sigma=1.0f, float alpha=10.f, float q=.5f,
+                  ImageGpuPtr<Pixel> tmp_denoised=nullptr);
+
+} // namespace cu
+} // namespace ze
diff --git a/RWR/src/ze_oss/imp_cu_imgproc/include/imp/cu_imgproc/image_pyramid.hpp b/RWR/src/ze_oss/imp_cu_imgproc/include/imp/cu_imgproc/image_pyramid.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..3a04b1e3a73f363577d52bbba015a261db6ea4b3
--- /dev/null
+++ b/RWR/src/ze_oss/imp_cu_imgproc/include/imp/cu_imgproc/image_pyramid.hpp
@@ -0,0 +1,200 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#pragma once
+
+#include <imp/cu_core/cu_image_gpu.cuh>
+#include <imp/cu_imgproc/cu_reduce.cuh>
+
+namespace ze {
+
+/**
+ * @brief The ImagePyramid class holds an image scale pyramid
+ *
+ * @todo (MWE) no roi support yet (e.g. propagated automatically from finest to coarser level)
+ */
+template<typename Pixel>
+class ImagePyramid
+{
+public:
+  ZE_POINTER_TYPEDEFS(ImagePyramid);
+
+  // typedefs for convenience
+  using Image = typename ze::Image<Pixel>;
+  using ImagePtr = typename ze::ImagePtr<Pixel>;
+  using ImageLevels = std::vector<ImagePtr>;
+
+public:
+  ImagePyramid() = delete;
+  virtual ~ImagePyramid() = default;
+
+  /**
+   * @brief ImagePyramid constructs an empy image pyramid
+   * @param size Image size of level 0
+   * @param scale_factor multiplicative level-to-level scale factor
+   * @param size_bound_ minimum size of the shorter side on coarsest level
+   * @param max_num_levels maximum number of levels
+   */
+  ImagePyramid(Size2u size, float scale_factor=0.5f, uint32_t size_bound=8,
+               uint32_t max_num_levels=UINT32_MAX);
+
+  /** Clearing image pyramid, not resetting parameters though. */
+  void clear() noexcept;
+
+  /** Setting up levels. */
+  void init(const ze::Size2u& size);
+
+  /*
+   * Getters / Setters
+   */
+
+  /** Returns the image pyramid (all levels) */
+  inline ImageLevels& levels() {return levels_;}
+  inline const ImageLevels& levels() const {return levels_;}
+
+  /** Returns the actual number of levels saved in the pyramid. */
+  inline size_t numLevels() const {return num_levels_;}
+
+  /** Returns a reference to the \a i-th image of the pyramid level. */
+  inline Image& operator[] (size_t i) {return *levels_[i];}
+  inline const Image& operator[] (size_t i) const {return *levels_[i];}
+
+  /** Returns a shared pointer to the \a i-th image of the pyramid level. */
+  inline ImagePtr atShared(size_t i) {return levels_.at(i);}
+  inline const ImagePtr atShared(size_t i) const {return levels_.at(i);}
+
+  /** Returns a reference to i-th image of the pyramid level. */
+  inline Image& at(size_t i) { return *levels_.at(i); }
+  inline const Image& at(size_t i) const { return *levels_.at(i); }
+
+  /** Returns the size of the i-th image. */
+  inline Size2u size(size_t i) const {return this->at(i).size();}
+
+  /** Sets the multiplicative level-to-level scale factor
+   *  (most likely in the interval [0.5,1.0[)
+   */
+  inline void setScaleFactor(const float& scale_factor) {scale_factor_ = scale_factor;}
+  /** Returns the multiplicative level-to-level scale factor. */
+  inline float scaleFactor() const {return scale_factor_;}
+
+  /** Returns the multiplicative scale-factor from \a i-th level to 0-level. */
+  inline float scaleFactor(const size_t i) const
+  {
+    CHECK_LT(i, scale_factors_.size());
+    return scale_factors_[i];
+  }
+
+  /** Sets the user defined maximum number of pyramid levels. */
+  inline void setMaxNumLevels(const size_t max_num_levels)
+  {
+    max_num_levels_ = max_num_levels;
+  }
+  /** Returns the user defined maximum number of pyramid levels. */
+  inline size_t maxNumLevels() const {return max_num_levels_;}
+
+
+  /** Sets the user defined size bound for the coarsest level (short side). */
+  inline void sizeBound(const uint32_t size_bound) {size_bound_ = size_bound;}
+  /** Returns the user defined size bound for the coarsest level (short side). */
+  inline uint32_t sizeBound() const {return size_bound_;}
+
+  /** Factory function: Add image */
+  inline void push_back(const ImagePtr& img) { levels_.push_back(img); }
+
+  /** Perfect forwarding of the initialization. Avoids copying */
+  template<typename... Args>
+  void emplace_back(Args&&... args) { levels_.emplace_back(std::forward<Args>(args)...); }
+
+
+private:
+
+
+private:
+  ImageLevels levels_; //!< Image pyramid levels holding shared_ptrs to images.
+  std::vector<float> scale_factors_; //!< Scale factors (multiplicative) towards the 0-level.
+  float scale_factor_ = 0.5f; //!< Scale factor between pyramid levels
+  uint32_t size_bound_ = 8; //!< User defined minimum size of coarsest level (short side).
+  size_t max_num_levels_ = UINT32_MAX; //!< User defined maximum number of pyramid levels.
+  size_t num_levels_ = UINT32_MAX; //!< actual number of levels dependent on the current setting.
+};
+
+//-----------------------------------------------------------------------------
+// convenience typedefs
+// (sync with explicit template class instantiations at the end of the cpp file)
+typedef ImagePyramid<ze::Pixel8uC1> ImagePyramid8uC1;
+typedef ImagePyramid<ze::Pixel8uC2> ImagePyramid8uC2;
+typedef ImagePyramid<ze::Pixel8uC3> ImagePyramid8uC3;
+typedef ImagePyramid<ze::Pixel8uC4> ImagePyramid8uC4;
+
+typedef ImagePyramid<ze::Pixel16uC1> ImagePyramid16uC1;
+typedef ImagePyramid<ze::Pixel16uC2> ImagePyramid16uC2;
+typedef ImagePyramid<ze::Pixel16uC3> ImagePyramid16uC3;
+typedef ImagePyramid<ze::Pixel16uC4> ImagePyramid16uC4;
+
+typedef ImagePyramid<ze::Pixel32sC1> ImagePyramid32sC1;
+typedef ImagePyramid<ze::Pixel32sC2> ImagePyramid32sC2;
+typedef ImagePyramid<ze::Pixel32sC3> ImagePyramid32sC3;
+typedef ImagePyramid<ze::Pixel32sC4> ImagePyramid32sC4;
+
+typedef ImagePyramid<ze::Pixel32fC1> ImagePyramid32fC1;
+typedef ImagePyramid<ze::Pixel32fC2> ImagePyramid32fC2;
+typedef ImagePyramid<ze::Pixel32fC3> ImagePyramid32fC3;
+typedef ImagePyramid<ze::Pixel32fC4> ImagePyramid32fC4;
+
+//------------------------------------------------------------------------------
+namespace cu {
+
+//! Image Pyramid Factory:
+template<typename Pixel>
+std::shared_ptr<ImagePyramid<Pixel>>
+createImagePyramidGpu(
+    typename Image<Pixel>::Ptr img_level0, real_t scale_factor=0.5,
+    uint32_t max_num_levels=UINT32_MAX, uint32_t size_bound=8u)
+{
+  // Sanity checks.
+  CHECK(img_level0->isGpuMemory());
+  using Pyr = ImagePyramid<Pixel>;
+  auto pyr = std::make_shared<Pyr>(
+        img_level0->size(), scale_factor, size_bound, max_num_levels);
+
+  pyr->push_back(img_level0);
+  Size2u sz0 =  img_level0->size();
+
+  for (size_t i=1; i<pyr->numLevels(); ++i)
+  {
+    Size2u sz(static_cast<uint32_t>(sz0.width() * pyr->scaleFactor(i) + 0.5f),
+              static_cast<uint32_t>(sz0.height() * pyr->scaleFactor(i) + 0.5f));
+
+    // init level memory with either ImageGpu or ImageRaw
+    using ImageGpu = typename ze::cu::ImageGpu<Pixel>;
+    pyr->emplace_back(std::make_shared<ImageGpu>(sz));
+    typename ImageGpu::Ptr prev = std::dynamic_pointer_cast<ImageGpu>(pyr->atShared(i-1));
+    typename ImageGpu::Ptr img = std::dynamic_pointer_cast<ImageGpu>(pyr->atShared(i));
+    VLOG(300) << "Creating GPU ImagePyramid Level " << i << " of size " << sz;
+    ze::cu::reduce(*img, *prev, InterpolationMode::Linear, true);
+  }
+  return pyr;
+}
+} // namespace cu
+} // namespace ze
diff --git a/RWR/src/ze_oss/imp_cu_imgproc/include/imp/cu_imgproc/str_tex_decomposer.hpp b/RWR/src/ze_oss/imp_cu_imgproc/include/imp/cu_imgproc/str_tex_decomposer.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..692004f6febb3bdf14fd6e2b1f557b5c64e8405b
--- /dev/null
+++ b/RWR/src/ze_oss/imp_cu_imgproc/include/imp/cu_imgproc/str_tex_decomposer.hpp
@@ -0,0 +1,75 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#pragma once
+
+#include <imp/cu_imgproc/cu_variational_denoising.cuh>
+
+#include <memory>
+#include <cuda_runtime_api.h>
+
+#include <imp/cu_core/cu_image_gpu.cuh>
+#include <imp/cu_core/cu_utils.hpp>
+#include <ze/common/macros.hpp>
+
+namespace ze {
+namespace cu {
+
+/** Decomposes the input image into its structure (cartoon) and texture part. */
+template<typename Pixel>
+class StrTexDecomposer
+{
+public:
+  ZE_POINTER_TYPEDEFS(StrTexDecomposer);
+  using Base = VariationalDenoising;
+  using ImageGpu = ze::cu::ImageGpu<Pixel>;
+
+public:
+  StrTexDecomposer();
+  ~StrTexDecomposer() = default;
+
+  void solve(const ze::cu::ImageGpuPtr<Pixel>& tex_image,
+             const ze::cu::ImageGpuPtr<Pixel>& src,
+             const ze::cu::ImageGpuPtr<Pixel>& structure_image = NULL);
+
+private:
+  ImageGpuPtr<Pixel> src_;
+  ImageGpuPtr<Pixel> denoised_;
+  ImageGpuPtr<Pixel> str_;
+  ImageGpuPtr<Pixel> tex_;
+  ze::cu::VariationalDenoising::Ptr denoiser_;
+  float weight_ = 0.8; //!< weighting of denoised towards original image during decomposition
+};
+
+//-----------------------------------------------------------------------------
+// convenience typedefs
+// (sync with explicit template class instantiations at the end of the cpp file)
+typedef StrTexDecomposer<ze::Pixel8uC1> StrTexDecomposer8uC1;
+typedef StrTexDecomposer<ze::Pixel32fC1> StrTexDecomposer32fC1;
+
+template <typename Pixel>
+using StrTexDecomposerPtr = typename std::shared_ptr<StrTexDecomposer<Pixel>>;
+
+} // namespace cu
+} // namespace ze
diff --git a/RWR/src/ze_oss/imp_cu_imgproc/package.xml b/RWR/src/ze_oss/imp_cu_imgproc/package.xml
new file mode 100644
index 0000000000000000000000000000000000000000..2645c77ad1ce11e90724f4c63502581a74a56ecd
--- /dev/null
+++ b/RWR/src/ze_oss/imp_cu_imgproc/package.xml
@@ -0,0 +1,23 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>imp_cu_imgproc</name>
+  <description>
+    IMP CUDA image processing module
+  </description>
+  <version>0.1.4</version>
+  <license>ZE</license>
+
+  <maintainer email="code@werlberger.org">Manuel Werlberger</maintainer>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <buildtool_depend>catkin_simple</buildtool_depend>
+
+  <depend>ze_cmake</depend>
+  <depend>ze_cameras</depend>
+  <depend>ze_geometry</depend>
+  <depend>imp_core</depend>
+  <depend>imp_cu_core</depend>
+  <depend>imp_3rdparty_cuda_toolkit</depend>
+  
+  <test_depend>gtest</test_depend>
+</package>
diff --git a/RWR/src/ze_oss/imp_cu_imgproc/src/cu_bilateral_filter.cu b/RWR/src/ze_oss/imp_cu_imgproc/src/cu_bilateral_filter.cu
new file mode 100644
index 0000000000000000000000000000000000000000..6e4ca8ae5dd8de77836bf5bef1ecc4c676aa0181
--- /dev/null
+++ b/RWR/src/ze_oss/imp_cu_imgproc/src/cu_bilateral_filter.cu
@@ -0,0 +1,251 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#ifndef IMP_CU_BILATERAL_IMPL_CU
+#define IMP_CU_BILATERAL_IMPL_CU
+
+#include <imp/cu_imgproc/cu_image_filter.cuh>
+
+#include <cstdint>
+#include <cuda_runtime.h>
+
+#include <imp/core/types.hpp>
+#include <imp/core/roi.hpp>
+#include <imp/cu_core/cu_image_gpu.cuh>
+#include <imp/cu_core/cu_utils.hpp>
+#include <imp/cu_core/cu_texture.cuh>
+
+
+
+namespace imp {
+namespace cu {
+
+// ----------------------------------------------------------------------------
+// kernel: bilateral filter kernel C1
+__global__ void cuFilterBilateralKernel_32f_C1(const float* src, float* dst,
+                                               const float* prior,
+                                               const float sigma_spatial, const float sigma_range,
+                                               const int radius, const size_t stride,
+                                               const int xoff, const int yoff,
+                                               const int width, const int height)
+{
+  int x = blockIdx.x*blockDim.x + threadIdx.x + xoff;
+  int y = blockIdx.y*blockDim.y + threadIdx.y + yoff;
+
+  int c = y*stride+x;
+  float p = prior[c];
+
+  if(x<width && y<height)
+  {
+    float sum_g = 0.0f;
+    float sum_val = 0.0f;
+
+    for (int l=-radius; l<=radius; ++l)
+    {
+      for (int k=-radius; k<=radius; ++k)
+      {
+        int xx=x+k, yy=y+l;
+        if(xx>=0 && yy>=0 && xx<width && yy<height)
+        {
+          int cc = yy*stride+xx;
+          float g = expf(-((iu::sqr(x-xx)+iu::sqr(y-yy))/(2.0f*iu::sqr(sigma_spatial)))
+                         -(iu::sqr(p-prior[cc])/(2.0f*iu::sqr(sigma_range))));
+          sum_g += g;
+          sum_val += g*src[cc];
+        }
+      }
+    }
+
+    dst[c] = sum_val / IUMAX(1e-6f, sum_g);
+  }
+
+}
+
+// ----------------------------------------------------------------------------
+// kernel: bilateral filter kernel C1 with C4 prior
+__global__ void cuFilterBilateralKernel_32f_C1C4(const float* src, float* dst,
+                                                 const float4* prior,
+                                                 const float sigma_spatial, const float sigma_range,
+                                                 const int radius,
+                                                 const size_t stride1, const size_t stride4,
+                                                 const int xoff, const int yoff,
+                                                 const int width, const int height)
+{
+  int x = blockIdx.x*blockDim.x + threadIdx.x + xoff;
+  int y = blockIdx.y*blockDim.y + threadIdx.y + yoff;
+
+  float4 p = prior[y*stride4+x];
+
+  if(x<width && y<height)
+  {
+    float sum_g = 0.0f;
+    float sum_val = 0.0f;
+
+    for (int l=-radius; l<=radius; ++l)
+    {
+      for (int k=-radius; k<=radius; ++k)
+      {
+        int xx=x+k, yy=y+l;
+        if(xx>=0 && yy>=0 && xx<width && yy<height)
+        {
+          float4 diff = p-prior[yy*stride4+xx];
+          float g = expf(-((iu::sqr(x-xx)+iu::sqr(y-yy))/(2*iu::sqr(sigma_spatial)))
+                         -(dot(diff,diff)/(2*iu::sqr(sigma_range))));
+          sum_g += g;
+          sum_val += g*src[y*stride1+x];
+        }
+      }
+    }
+
+    dst[y*stride1+x] = sum_val / IUMAX(1e-6f, sum_g);
+  }
+}
+
+// ----------------------------------------------------------------------------
+// kernel: bilateral filter kernel C4
+__global__ void cuFilterBilateralKernel_32f_C4(const float4* src, float4* dst,
+                                               const float4* prior,
+                                               float sigma_spatial, const float sigma_range,
+                                               const int radius, const size_t stride,
+                                               const int xoff, const int yoff,
+                                               const int width, const int height)
+{
+  int x = blockIdx.x*blockDim.x + threadIdx.x + xoff;
+  int y = blockIdx.y*blockDim.y + threadIdx.y + yoff;
+
+  int c = y*stride+x;
+  float4 p = prior[c];
+
+  if(x<width && y<height)
+  {
+    float sum_g = 0.0f;
+    float4 sum_val = make_float4(0.0f);
+
+    for (int l=-radius; l<=radius; ++l)
+    {
+      for (int k=-radius; k<=radius; ++k)
+      {
+        int xx=x+k, yy=y+l;
+        if(xx>=0 && yy>=0 && xx<width && yy<height)
+        {
+          int cc = yy*stride+xx;
+          float4 diff = p-prior[cc];
+          float g = expf(-((iu::sqr(x-xx)+iu::sqr(y-yy))/(2*iu::sqr(sigma_spatial)))
+                         -(dot(diff,diff)/(2*iu::sqr(sigma_range))));
+          sum_g += g;
+          sum_val += g*src[cc];
+        }
+      }
+    }
+
+    dst[c] = sum_val / IUMAX(1e-6f, sum_g);
+  }
+
+}
+
+
+// ----------------------------------------------------------------------------
+// wrapper: bilateral filter, C1
+void cuFilterBilateral(const iu::ImageGpu_32f_C1* src, iu::ImageGpu_32f_C1* dst, const IuRect& roi,
+                       const iu::ImageGpu_32f_C1* prior, const int iters,
+                       const float sigma_spatial, const float sigma_range,
+                       const int radius)
+{
+  float min,max;
+  iu::minMax(src, src->roi(), min, max);
+  printf("src min/max=%f/%f\n", min, max);
+  iu::minMax(prior, src->roi(), min, max);
+  printf("prior min/max=%f/%f\n", min, max);
+
+  // fragmentation
+  unsigned int block_size = 16;
+  dim3 dimBlock(block_size, block_size);
+  dim3 dimGrid(iu::divUp(roi.width, dimBlock.x), iu::divUp(roi.height, dimBlock.y));
+
+  // filter iterations
+  for (int iter=0; iter<iters; ++iter)
+  {
+    cuFilterBilateralKernel_32f_C1
+        <<< dimGrid, dimBlock >>> (src->data(), dst->data(), prior->data(),
+                                   sigma_spatial, sigma_range, radius,
+                                   src->stride(), roi.x, roi.y, roi.width, roi.height);
+  }
+
+  IU_CUDA_CHECK();
+}
+
+// wrapper: bilateral filter, C1 and C4 prior
+void cuFilterBilateral(const iu::ImageGpu_32f_C1* src, iu::ImageGpu_32f_C1* dst, const IuRect& roi,
+                       const iu::ImageGpu_32f_C4* prior, const int iters,
+                       const float sigma_spatial, const float sigma_range,
+                       const int radius)
+{
+  // fragmentation
+  unsigned int block_size = 16;
+  dim3 dimBlock(block_size, block_size);
+  dim3 dimGrid(iu::divUp(roi.width, dimBlock.x), iu::divUp(roi.height, dimBlock.y));
+
+  // filter iterations
+  for (int iter=0; iter<iters; ++iter)
+  {
+    cuFilterBilateralKernel_32f_C1C4
+        <<< dimGrid, dimBlock >>> (src->data(), dst->data(), prior->data(),
+                                   sigma_spatial, sigma_range, radius,
+                                   src->stride(), prior->stride(),
+                                   roi.x, roi.y, roi.width, roi.height);
+  }
+
+  IU_CUDA_CHECK();
+}
+
+// wrapper: bilateral filter, C4
+void cuFilterBilateral(const iu::ImageGpu_32f_C4* src, iu::ImageGpu_32f_C4* dst, const IuRect& roi,
+                       const iu::ImageGpu_32f_C4* prior, const int iters,
+                       const float sigma_spatial, const float sigma_range,
+                       const int radius)
+{
+  // fragmentation
+  unsigned int block_size = 16;
+  dim3 dimBlock(block_size, block_size);
+  dim3 dimGrid(iu::divUp(roi.width, dimBlock.x), iu::divUp(roi.height, dimBlock.y));
+
+  // filter iterations
+  for (int iter=0; iter<iters; ++iter)
+  {
+    cuFilterBilateralKernel_32f_C4
+        <<< dimGrid, dimBlock >>> (src->data(), dst->data(), prior->data(),
+                                   sigma_spatial, sigma_range, radius,
+                                   src->stride(), roi.x, roi.y, roi.width, roi.height);
+  }
+
+  IU_CUDA_CHECK();
+}
+
+
+} // namespace cu
+} // namespace imp
+
+
+
+#endif IMP_CU_BILATERAL_IMPL_CU
diff --git a/RWR/src/ze_oss/imp_cu_imgproc/src/cu_gauss_filter.cu b/RWR/src/ze_oss/imp_cu_imgproc/src/cu_gauss_filter.cu
new file mode 100644
index 0000000000000000000000000000000000000000..fe9cf4162462fe972102e047eae7502ef22a8944
--- /dev/null
+++ b/RWR/src/ze_oss/imp_cu_imgproc/src/cu_gauss_filter.cu
@@ -0,0 +1,221 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#ifndef IMP_CU_GAUSS_IMPL_CU
+#define IMP_CU_GAUSS_IMPL_CU
+
+#include <imp/cu_imgproc/cu_image_filter.cuh>
+
+#include <cstdint>
+#include <cuda_runtime.h>
+
+#include <imp/core/types.hpp>
+#include <imp/core/roi.hpp>
+#include <imp/cu_core/cu_image_gpu.cuh>
+#include <imp/cu_core/cu_utils.hpp>
+#include <imp/cu_core/cu_texture.cuh>
+
+namespace ze {
+namespace cu {
+
+//-----------------------------------------------------------------------------
+/** Perform a convolution with an gaussian smoothing kernel
+ * @param dst          pointer to output image (linear memory)
+ * @param stride       length of image row [pixels]
+ * @param xoff         x-coordinate offset where to start the region [pixels]
+ * @param yoff         y-coordinate offset where to start the region [pixels]
+ * @param width        width of region [pixels]
+ * @param height       height of region [pixels]
+ * @param kernel_size  lenght of the smoothing kernel [pixels]
+ * @param horizontal   defines the direction of convolution
+ */
+template<typename Pixel>
+__global__ void k_gauss(Pixel* dst, const size_t stride,
+                        const int xoff, const int yoff,
+                        const int width, const int height,
+                        Texture2D src_tex, int kernel_size, float c0,
+                        float c1, bool horizontal=true)
+{
+  int x = blockIdx.x*blockDim.x + threadIdx.x;
+  int y = blockIdx.y*blockDim.y + threadIdx.y;
+  const size_t out_idx = y*stride+x;
+
+  if(x>=0 && y>= 0 && x<width && y<height)
+  {
+    x += xoff;
+    y += yoff;
+
+    float sum = 0.0f;
+    int half_kernel_elements = (kernel_size - 1) / 2;
+
+    Pixel texel_c, texel;
+    tex2DFetch(texel_c, src_tex, x, y);
+
+    if (horizontal)
+    {
+      // convolve horizontally
+      float g2 = c1 * c1;
+      sum = c0 * texel_c;
+      float sum_coeff = c0;
+
+      for (int i = 1; i <= half_kernel_elements; i++)
+      {
+        c0 *= c1;
+        c1 *= g2;
+        int cur_x = max(0, min(width-1, x+i));
+        tex2DFetch(texel, src_tex, cur_x, y);
+        sum += c0 * texel;
+        cur_x = max(0, min(width-1, x-i));
+        tex2DFetch(texel, src_tex, cur_x, y);
+        sum += c0 * texel;
+        sum_coeff += 2.0f*c0;
+      }
+      dst[out_idx] = sum/sum_coeff;
+    }
+    else
+    {
+      // convolve vertically
+      float g2 = c1 * c1;
+      sum = c0 * texel_c;
+      float sum_coeff = c0;
+
+      for (int j = 1; j <= half_kernel_elements; j++)
+      {
+        c0 *= c1;
+        c1 *= g2;
+        float cur_y = max(0, min(height-1, y+j));
+        tex2DFetch(texel, src_tex, x, cur_y);
+        sum += c0 * texel;
+        cur_y = max(0, min(height-1, y-j));
+        tex2DFetch(texel, src_tex, x, cur_y);
+        sum += c0 *  texel;
+        sum_coeff += 2.0f*c0;
+      }
+      dst[out_idx] = sum/sum_coeff;
+    }
+  }
+}
+
+//-----------------------------------------------------------------------------
+template<typename Pixel>
+void filterGauss(ImageGpu<Pixel>& dst,
+                 const Texture2D& src_tex,
+                 float sigma, int kernel_size,
+                 ImageGpuPtr<Pixel> tmp_img)
+//                 cudaStream_t stream);
+{
+  Roi2u roi = dst.roi();
+
+  if (kernel_size == 0)
+    kernel_size = max(5, static_cast<int>(std::ceil(sigma*3)*2 + 1));
+  if (kernel_size % 2 == 0)
+    ++kernel_size;
+
+  // temporary variable for filtering (separabel kernel!)
+  if (!tmp_img || dst.roi().size() != tmp_img->size());
+  {
+    tmp_img.reset(new ImageGpu<Pixel>(roi.size()));
+  }
+
+  // fragmentation
+  Fragmentation<> frag(roi);
+
+  float c0 = 1.0f / (std::sqrt(2.0f * M_PI)*sigma);
+  float c1 = std::exp(-0.5f / (sigma * sigma));
+
+  // Convolve horizontally
+  k_gauss
+      <<<
+        frag.dimGrid, frag.dimBlock//, 0, stream
+      >>> (tmp_img->data(), tmp_img->stride(),
+           roi.x(), roi.y(), tmp_img->width(), tmp_img->height(),
+           src_tex, /*sigma, */kernel_size, c0, c1, false);
+
+  IMP_CUDA_CHECK();
+  std::shared_ptr<Texture2D> tmp_tex =
+      tmp_img->genTexture(false,(tmp_img->bitDepth()<32) ? cudaFilterModePoint
+                                                         : cudaFilterModeLinear);
+  IMP_CUDA_CHECK();
+
+  // Convolve vertically
+  k_gauss
+      <<<
+        frag.dimGrid, frag.dimBlock//, 0, stream
+      >>> (dst.data(), dst.stride(),
+           roi.x(), roi.y(), roi.width(), roi.height(),
+           *tmp_tex, /*sigma, */kernel_size, c0, c1, true);
+
+  IMP_CUDA_CHECK();
+}
+
+
+//-----------------------------------------------------------------------------
+template<typename Pixel>
+void filterGauss(ImageGpu<Pixel>& dst,
+                 const ImageGpu<Pixel>& src,
+                 float sigma, int kernel_size,
+                 ImageGpuPtr<Pixel> tmp_img)
+//                 cudaStream_t stream);
+{
+  std::shared_ptr<Texture2D> src_tex =
+      src.genTexture(false,(src.bitDepth()<32) ? cudaFilterModePoint
+                                               : cudaFilterModeLinear);
+  ze::Roi2u roi = src.roi();
+  if (dst.roi().size() != roi.size())
+     dst.setRoi(roi);
+
+  ze::cu::filterGauss(dst, *src_tex, sigma, kernel_size, tmp_img);
+
+  IMP_CUDA_CHECK();
+}
+
+
+//==============================================================================
+//
+// template instantiations for all our ima ge types
+//
+
+template void filterGauss(ImageGpu8uC1& dst, const ImageGpu8uC1& src, float sigma, int kernel_size, std::shared_ptr<ImageGpu8uC1> tmp_imp);
+template void filterGauss(ImageGpu8uC2& dst, const ImageGpu8uC2& src, float sigma, int kernel_size, std::shared_ptr<ImageGpu8uC2> tmp_imp);
+template void filterGauss(ImageGpu8uC4& dst, const ImageGpu8uC4& src, float sigma, int kernel_size, std::shared_ptr<ImageGpu8uC4> tmp_imp);
+
+template void filterGauss(ImageGpu16uC1& dst, const ImageGpu16uC1& src, float sigma, int kernel_size, std::shared_ptr<ImageGpu16uC1> tmp_imp);
+template void filterGauss(ImageGpu16uC2& dst, const ImageGpu16uC2& src, float sigma, int kernel_size, std::shared_ptr<ImageGpu16uC2> tmp_imp);
+template void filterGauss(ImageGpu16uC4& dst, const ImageGpu16uC4& src, float sigma, int kernel_size, std::shared_ptr<ImageGpu16uC4> tmp_imp);
+
+template void filterGauss(ImageGpu32sC1& dst, const ImageGpu32sC1& src, float sigma, int kernel_size, std::shared_ptr<ImageGpu32sC1> tmp_imp);
+template void filterGauss(ImageGpu32sC2& dst, const ImageGpu32sC2& src, float sigma, int kernel_size, std::shared_ptr<ImageGpu32sC2> tmp_imp);
+template void filterGauss(ImageGpu32sC4& dst, const ImageGpu32sC4& src, float sigma, int kernel_size, std::shared_ptr<ImageGpu32sC4> tmp_imp);
+
+template void filterGauss(ImageGpu32fC1& dst, const ImageGpu32fC1& src, float sigma, int kernel_size, std::shared_ptr<ImageGpu32fC1> tmp_imp);
+template void filterGauss(ImageGpu32fC2& dst, const ImageGpu32fC2& src, float sigma, int kernel_size, std::shared_ptr<ImageGpu32fC2> tmp_imp);
+template void filterGauss(ImageGpu32fC4& dst, const ImageGpu32fC4& src, float sigma, int kernel_size, std::shared_ptr<ImageGpu32fC4> tmp_imp);
+
+
+} // namespace cu
+} // namespace ze
+
+
+
+#endif // IMP_CU_GAUSS_IMPL_CU
diff --git a/RWR/src/ze_oss/imp_cu_imgproc/src/cu_horizontal_stereo_pair_rectifier.cu b/RWR/src/ze_oss/imp_cu_imgproc/src/cu_horizontal_stereo_pair_rectifier.cu
new file mode 100644
index 0000000000000000000000000000000000000000..3ae18e434012ca896e4192f533dd5943751d6cd2
--- /dev/null
+++ b/RWR/src/ze_oss/imp_cu_imgproc/src/cu_horizontal_stereo_pair_rectifier.cu
@@ -0,0 +1,99 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#include <imp/cu_imgproc/cu_horizontal_stereo_pair_rectifier.cuh>
+
+namespace ze {
+namespace cu {
+
+template <typename CameraModel,
+          typename DistortionModel,
+          typename Pixel>
+HorizontalStereoPairRectifier<CameraModel, DistortionModel, Pixel>::HorizontalStereoPairRectifier(
+    Vector4& transformed_cam0_params,
+    Vector4& transformed_cam1_params,
+    real_t& horizontal_offset,
+    const Size2u& img_size,
+    const Vector4& cam0_params,
+    const Vector4& cam0_dist_coeffs,
+    const Vector4& cam1_params,
+    const Vector4& cam1_dist_coeffs,
+    const Transformation& T_cam1_cam0)
+{
+  Matrix3 cam0_H;
+  Matrix3 cam1_H;
+
+  std::tie(cam0_H, cam1_H,
+           transformed_cam0_params,
+           transformed_cam1_params, horizontal_offset) =
+      computeHorizontalStereoParameters <CameraModel, DistortionModel>(img_size,
+                                                                       cam0_params,
+                                                                       cam0_dist_coeffs,
+                                                                       cam1_params,
+                                                                       cam1_dist_coeffs,
+                                                                       T_cam1_cam0);
+
+  //! Allocate rectifiers
+  const Matrix3 inv_cam0_H = cam0_H.inverse();
+  const Matrix3 inv_cam1_H = cam1_H.inverse();
+  rectifiers_[0].reset(
+        new StereoRectifier<CameraModel, DistortionModel, Pixel>(
+          img_size, cam0_params, transformed_cam0_params,
+          cam0_dist_coeffs, inv_cam0_H));
+  rectifiers_[1].reset(
+        new StereoRectifier<CameraModel, DistortionModel, Pixel>(
+          img_size, cam1_params, transformed_cam1_params,
+          cam1_dist_coeffs, inv_cam1_H));
+}
+
+template <typename CameraModel,
+          typename DistortionModel,
+          typename Pixel>
+void HorizontalStereoPairRectifier<CameraModel, DistortionModel, Pixel>::rectify(
+    ImageGpu<Pixel>& cam0_dst,
+    ImageGpu<Pixel>& cam1_dst,
+    const ImageGpu<Pixel>& cam0_src,
+    const ImageGpu<Pixel>& cam1_src) const
+{
+  rectifiers_[0]->rectify(cam0_dst, cam0_src);
+  rectifiers_[1]->rectify(cam1_dst, cam1_src);
+}
+
+template <typename CameraModel,
+          typename DistortionModel,
+          typename Pixel>
+const ImageGpu32fC2& HorizontalStereoPairRectifier<CameraModel, DistortionModel, Pixel>::getUndistortRectifyMap(
+    int8_t cam_idx) const
+{
+  CHECK_GE(cam_idx, 0);
+  CHECK_LE(cam_idx, 1);
+  return rectifiers_[cam_idx]->getUndistortRectifyMap();
+}
+
+// Explicit template instantiations
+template class HorizontalStereoPairRectifier<PinholeGeometry, EquidistantDistortion, Pixel32fC1>;
+template class HorizontalStereoPairRectifier<PinholeGeometry, RadialTangentialDistortion, Pixel32fC1>;
+
+} // cu namespace
+} // ze namespace
diff --git a/RWR/src/ze_oss/imp_cu_imgproc/src/cu_median3x3_filter.cu b/RWR/src/ze_oss/imp_cu_imgproc/src/cu_median3x3_filter.cu
new file mode 100644
index 0000000000000000000000000000000000000000..68f423bfa4c91aa5d15d3c7a5ff28592b5672bcf
--- /dev/null
+++ b/RWR/src/ze_oss/imp_cu_imgproc/src/cu_median3x3_filter.cu
@@ -0,0 +1,335 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#ifndef IMP_CU_MEDIAN3X3_IMPL_CU
+#define IMP_CU_MEDIAN3X3_IMPL_CU
+
+#include <imp/cu_imgproc/cu_image_filter.cuh>
+
+#include <cstdint>
+#include <cfloat>
+#include <cuda_runtime.h>
+
+#include <imp/core/types.hpp>
+#include <imp/core/pixel.hpp>
+#include <imp/core/roi.hpp>
+#include <imp/cu_core/cu_image_gpu.cuh>
+#include <imp/cu_core/cu_utils.hpp>
+#include <imp/cu_core/cu_texture.cuh>
+
+namespace ze {
+namespace cu {
+
+//-----------------------------------------------------------------------------
+template<typename Pixel>
+__global__ void  k_median3x3(Pixel* dst, const size_t stride,
+                             const uint32_t xoff, const uint32_t yoff,
+                             const uint32_t width, const uint32_t height,
+                             Texture2D src_tex)
+{
+  int x = blockIdx.x*blockDim.x + threadIdx.x;
+  int y = blockIdx.y*blockDim.y + threadIdx.y;
+
+  if(x>=0 && y>= 0 && x<width && y<height)
+  {
+    x += xoff;
+    y += yoff;
+
+    // shared mem coords
+    const int tx = threadIdx.x+1;
+    const int ty = threadIdx.y+1;
+    // we have a 3x3 kernel, so our width of the shared memory (shp) is blockDim.x + 2!
+    const int shp = blockDim.x + 2;
+    const int shc = ty*shp + tx;
+    extern __shared__ float sh_in[];
+
+    // Load input 3x3 block into shared memory
+    // Note: the FLT_MAX prevents us from overemphasizing the border pixels if they are outliers!
+    {
+      // for each thread: copy the data of the current input position to shared mem
+      Pixel texel;
+      tex2DFetch(texel, src_tex, x, y);
+      sh_in[shc] = texel;
+
+      /////////////////////////////////////////////////////////////////////////////
+      // boundary conditions
+      /////////////////////////////////////////////////////////////////////////////
+      if (x == 0) // at left image border
+      {
+        if (y == 0)
+          sh_in[shc-shp-1] = FLT_MAX; // left-upper corner (image)
+        else if (ty == 1)
+        {
+          // left-upper corner (block)
+          tex2DFetch(texel, src_tex, x, y-1.f);
+          sh_in[shc-shp-1] = texel;
+        }
+
+        sh_in[shc-1] = sh_in[shc];     // left border (image)
+
+        if (y == height-1)
+          sh_in[shc+shp-1] = FLT_MAX; // left-lower corner (image)
+        else if (ty == blockDim.y)
+        {
+          tex2DFetch(texel, src_tex, x, y+1);
+          sh_in[shc+shp-1] = texel; // left-lower corner (block)
+        }
+      }
+      else if (tx == 1) // at left block border (inside image w.r.t x)
+      {
+        if (y == 0)
+        {
+          tex2DFetch(texel, src_tex, x-1, y);
+          sh_in[shc-shp-1] = texel; // left-upper corner (block, outside)
+        }
+        else if (ty == 1)
+        {
+          tex2DFetch(texel, src_tex, x-1, y-1);
+          sh_in[shc-shp-1] = texel; // left-upper corner (block, inside)
+        }
+
+        tex2DFetch(texel, src_tex, x-1, y);
+        sh_in[shc-1] = texel; // left border (block)
+
+        if (y == height-1)
+        {
+          tex2DFetch(texel, src_tex, x-1, y);
+          sh_in[shc+shp-1] = texel; // left-lower corner (block, outside)
+        }
+        else if (ty == blockDim.y)
+        {
+          tex2DFetch(texel, src_tex, x-1, y+1);
+          sh_in[shc+shp-1] = texel; // left-lower corner (block, inside)
+        }
+      }
+
+
+      if (x == width-1) // at right image border
+      {
+        if (y == 0)
+          sh_in[shc-shp+1] = FLT_MAX; // right-upper corner (image)
+        else if (ty == 1)
+        {
+          tex2DFetch(texel, src_tex, x, y-1);
+          sh_in[shc-shp+1] = texel; // right-upper corner (block)
+        }
+
+        sh_in[shc+1] = sh_in[shc]; // right border (image)
+
+        if (y == height-1)
+          sh_in[shc+shp+1] = FLT_MAX; // right-lower corner (image)
+        else if (ty == blockDim.y)
+        {
+          tex2DFetch(texel, src_tex, x, y+1);
+          sh_in[shc+shp+1] = texel; // right-lower corner (block)
+        }
+      }
+      else if (tx == blockDim.x) // at right block border (inside image w.r.t x)
+      {
+        if (y == 0)
+        {
+          tex2DFetch(texel, src_tex, x+1, y);
+          sh_in[shc-shp+1] = texel; // right-upper corner (block, outside)
+        }
+        else if (ty == 1)
+        {
+          tex2DFetch(texel, src_tex, x+1, y-1);
+          sh_in[shc-shp+1] = texel; // right-upper corner (block, inside)
+        }
+
+        tex2DFetch(texel, src_tex, x+1, y);
+        sh_in[shc+1] = texel; // right border (block)
+
+        if (y == height-1)
+        {
+          tex2DFetch(texel, src_tex, x+1, y);
+          sh_in[shc+shp+1] = texel; // right-lower corner (block, outside)
+        }
+        else if (ty == blockDim.y)
+        {
+          tex2DFetch(texel, src_tex, x+1, y+1);
+          sh_in[shc+shp+1] = texel; // right-lower corner (block, inside)
+        }
+      }
+
+      if (y == 0)
+        sh_in[shc-shp] = sh_in[shc]; // upper border (image)
+      else if (ty == 1)
+      {
+        tex2DFetch(texel, src_tex, x, y-1);
+        sh_in[shc-shp] = texel; // upper border (block)
+      }
+
+      if (y == height-1)
+        sh_in[shc+shp] = sh_in[shc]; // lower border (image)
+      else if (ty == blockDim.y)
+      {
+        tex2DFetch(texel, src_tex, x, y+1);
+        sh_in[shc+shp] = texel; // lower border (block)
+      }
+
+      __syncthreads();
+    }
+
+    // in a sequence of nine elements, we have to remove four times the maximum from the sequence and need
+    // a fifth calculated maximum which is the median!
+
+    float maximum;
+    {
+      float vals[8];
+
+      // first 'loop'
+      vals[0] = fmin(sh_in[shc-shp-1], sh_in[shc-shp]);
+      maximum = fmax(sh_in[shc-shp-1], sh_in[shc-shp]);
+      vals[1] = fmin(maximum, sh_in[shc-shp+1]);
+      maximum = fmax(maximum, sh_in[shc-shp+1]);
+      vals[2] = fmin(maximum, sh_in[shc-1]);
+      maximum = fmax(maximum, sh_in[shc-1]);
+      vals[3] = fmin(maximum, sh_in[shc]);
+      maximum = fmax(maximum, sh_in[shc]);
+      vals[4] = fmin(maximum, sh_in[shc+1]);
+      maximum = fmax(maximum, sh_in[shc+1]);
+      vals[5] = fmin(maximum, sh_in[shc+shp-1]);
+      maximum = fmax(maximum, sh_in[shc+shp-1]);
+      vals[6] = fmin(maximum, sh_in[shc+shp]);
+      maximum = fmax(maximum, sh_in[shc+shp]);
+      vals[7] = fmin(maximum, sh_in[shc+shp+1]);
+      maximum = fmax(maximum, sh_in[shc+shp+1]);
+
+      // second 'loop'
+      maximum = fmax(vals[0], vals[1]);
+      vals[0] = fmin(vals[0], vals[1]);
+      vals[1] = maximum;
+      maximum = fmax(vals[1], vals[2]);
+      vals[1] = fmin(vals[1], vals[2]);
+      vals[2] = maximum;
+      maximum = fmax(vals[2], vals[3]);
+      vals[2] = fmin(vals[2], vals[3]);
+      vals[3] = maximum;
+      maximum = fmax(vals[3], vals[4]);
+      vals[3] = fmin(vals[3], vals[4]);
+      vals[4] = maximum;
+      maximum = fmax(vals[4], vals[5]);
+      vals[4] = fmin(vals[4], vals[5]);
+      vals[5] = maximum;
+      maximum = fmax(vals[5], vals[6]);
+      vals[5] = fmin(vals[5], vals[6]);
+      vals[6] = fmin(maximum, vals[7]);
+
+      // third 'loop'
+      maximum = fmax(vals[0], vals[1]);
+      vals[0] = fmin(vals[0], vals[1]);
+      vals[1] = maximum;
+      maximum = fmax(vals[1], vals[2]);
+      vals[1] = fmin(vals[1], vals[2]);
+      vals[2] = maximum;
+      maximum = fmax(vals[2], vals[3]);
+      vals[2] = fmin(vals[2], vals[3]);
+      vals[3] = maximum;
+      maximum = fmax(vals[3], vals[4]);
+      vals[3] = fmin(vals[3], vals[4]);
+      vals[4] = maximum;
+      maximum = fmax(vals[4], vals[5]);
+      vals[4] = fmin(vals[4], vals[5]);
+      vals[5] = fmin(maximum, vals[6]);
+
+      // 4th 'loop'
+      maximum = fmax(vals[0], vals[1]);
+      vals[0] = fmin(vals[0], vals[1]);
+      vals[1] = maximum;
+      maximum = fmax(vals[1], vals[2]);
+      vals[1] = fmin(vals[1], vals[2]);
+      vals[2] = maximum;
+      maximum = fmax(vals[2], vals[3]);
+      vals[2] = fmin(vals[2], vals[3]);
+      vals[3] = maximum;
+      maximum = fmax(vals[3], vals[4]);
+      vals[3] = fmin(vals[3], vals[4]);
+      vals[4] = fmin(maximum, vals[5]);
+
+      // 5th 'loop'
+      maximum = fmax(vals[0], vals[1]);
+      maximum = fmax(maximum, vals[2]);
+      maximum = fmax(maximum, vals[3]);
+      maximum = fmax(maximum, vals[4]);
+    }
+
+    dst[y*stride+x] = maximum;
+  }
+}
+
+//-----------------------------------------------------------------------------
+template<typename Pixel>
+void filterMedian3x3(ImageGpu<Pixel>& dst,
+                     const ImageGpu<Pixel>& src)
+{
+  std::shared_ptr<Texture2D> src_tex =
+      src.genTexture(false, (src.bitDepth()<32) ? cudaFilterModePoint
+                                                : cudaFilterModeLinear);
+
+  constexpr std::uint16_t block_size = 16;
+  Fragmentation<block_size, block_size> frag(src.roi());
+  size_t shared_size = (block_size+2)*(block_size+2)*sizeof(float);
+
+  Roi2u roi = src.roi();
+  dst.setRoi(roi);
+
+  k_median3x3
+      <<<
+        frag.dimGrid, frag.dimBlock, shared_size
+      >>> (
+          dst.data(), dst.stride(),
+          roi.x(), roi.y(), roi.width(), roi.height(), *src_tex);
+
+  IMP_CUDA_CHECK();
+}
+
+//==============================================================================
+//
+// template instantiations for all our image types
+//
+
+template void filterMedian3x3(ImageGpu8uC1& dst, const ImageGpu8uC1& src);
+template void filterMedian3x3(ImageGpu8uC2& dst, const ImageGpu8uC2& src);
+template void filterMedian3x3(ImageGpu8uC4& dst, const ImageGpu8uC4& src);
+
+template void filterMedian3x3(ImageGpu16uC1& dst, const ImageGpu16uC1& src);
+template void filterMedian3x3(ImageGpu16uC2& dst, const ImageGpu16uC2& src);
+template void filterMedian3x3(ImageGpu16uC4& dst, const ImageGpu16uC4& src);
+
+template void filterMedian3x3(ImageGpu32sC1& dst, const ImageGpu32sC1& src);
+template void filterMedian3x3(ImageGpu32sC2& dst, const ImageGpu32sC2& src);
+template void filterMedian3x3(ImageGpu32sC4& dst, const ImageGpu32sC4& src);
+
+template void filterMedian3x3(ImageGpu32fC1& dst, const ImageGpu32fC1& src);
+template void filterMedian3x3(ImageGpu32fC2& dst, const ImageGpu32fC2& src);
+template void filterMedian3x3(ImageGpu32fC4& dst, const ImageGpu32fC4& src);
+
+
+} // namespace cu
+} // namespace ze
+
+
+
+#endif // IMP_CU_MEDIAN3X3_IMPL_CU
diff --git a/RWR/src/ze_oss/imp_cu_imgproc/src/cu_natural_edges.cu b/RWR/src/ze_oss/imp_cu_imgproc/src/cu_natural_edges.cu
new file mode 100644
index 0000000000000000000000000000000000000000..a6e0643fa24e6d5f396d4de0ef150ab236c57ea2
--- /dev/null
+++ b/RWR/src/ze_oss/imp_cu_imgproc/src/cu_natural_edges.cu
@@ -0,0 +1,174 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#ifndef IM_CU_EDGE_DETECTORS_NATURAL_EDGES_IMPL_CUH
+#define IM_CU_EDGE_DETECTORS_NATURAL_EDGES_IMPL_CUH
+
+#include <imp/cu_imgproc/edge_detectors.cuh>
+
+#include <cstdint>
+#include <cuda_runtime.h>
+
+#include <imp/core/types.hpp>
+#include <imp/core/pixel.hpp>
+#include <imp/core/roi.hpp>
+#include <imp/cu_core/cu_image_gpu.cuh>
+#include <imp/cu_core/cu_utils.hpp>
+#include <imp/cu_core/cu_texture.cuh>
+#include <imp/cu_imgproc/cu_image_filter.cuh>
+
+namespace ze {
+namespace cu {
+
+//------------------------------------------------------------------------------
+template<typename Pixel, typename T>
+__device__ __forceinline__ void d_calcNaturalEdge(Pixel1<T>& g,
+    const Pixel& dx, const Pixel& dy, const float alpha, const float q)
+{
+  float norm = sqrtf(sqr(dx) + sqr(dy));
+  g = max(1e-3f, exp(-alpha*pow(norm,q)));
+}
+
+//------------------------------------------------------------------------------
+template<typename Pixel, typename T>
+__device__ __forceinline__ void d_calcNaturalEdge(Pixel2<T>& g,
+    const Pixel& dx, const Pixel& dy, const float alpha, const float q)
+{
+  g.x = max(1e-3f, exp(-alpha*pow(dx,q)));
+  g.y = max(1e-3f, exp(-alpha*pow(dy,q)));
+}
+
+//------------------------------------------------------------------------------
+template<typename Pixel, typename T>
+__device__ __forceinline__ void d_calcNaturalEdge(Pixel3<T>& g,
+    const Pixel& dx, const Pixel& dy, const float alpha, const float q)
+{
+  float norm = sqrtf(sqr(dx) + sqr(dy));
+  float n1 = 1.0f, n2 = 0.0f;
+  float dt = 1e-6;
+  if (norm > dt)
+  {
+    n1 = dx/norm;
+    n2 = dy/norm;
+  }
+  // orthogonal to the normal
+  float n1_ = n2;
+  float n2_ = -n1;
+
+  float w = max(1e-3f, exp(-alpha*pow(norm,q)));
+  g.x = n1_*n1_ + w*n1*n1;
+  g.y = n2_*n2_ + w*n2*n2;
+  g.z = n1_*n2_ + w*n1*n2;
+}
+
+//------------------------------------------------------------------------------
+template<typename Pixel, typename EdgePixel>
+__global__ void k_naturalEdges(EdgePixel *g, const size_t stride,
+                               const float alpha, const float q,
+                               const uint32_t xoff, const uint32_t yoff,
+                               const uint32_t width, const uint32_t height,
+                               Texture2D src_tex)
+{
+  const int x = blockIdx.x*blockDim.x + threadIdx.x;
+  const int y = blockIdx.y*blockDim.y + threadIdx.y;
+
+  if (x<width && y<height)
+  {
+    Pixel ic, ixp, iyp;
+    tex2DFetch(ic, src_tex, x, y);
+    tex2DFetch(ixp, src_tex, x+1.f, y);
+    tex2DFetch(iyp, src_tex, x, y+1.f);
+
+    // calculate finite derivatives
+    Pixel dx = ixp - ic;
+    if (x >= width-1)
+      dx = 0.0f;
+    Pixel dy = iyp - ic;
+    if (y >= height-1)
+      dy = 0.0f;
+
+    d_calcNaturalEdge(g[y*stride+x], dx, dy, alpha, q);
+  }
+}
+
+//------------------------------------------------------------------------------
+template<typename Pixel>
+void naturalEdges(ImageGpu<Pixel>& dst,
+                  const ImageGpu<Pixel>& src,
+                  float sigma, float alpha, float q,
+                  ImageGpuPtr<Pixel> tmp_denoised)
+{
+  Roi2u roi = src.roi();
+  dst.setRoi(roi);
+
+  // temporary variable for filtering (separabel kernel!)
+  if (!tmp_denoised || src.size() != tmp_denoised->size())
+  {
+    tmp_denoised.reset(new ImageGpu<Pixel>(roi.size()));
+  }
+  else
+  {
+    tmp_denoised->setRoi(roi);
+  }
+
+  ze::cu::filterGauss(*tmp_denoised, src, sigma);
+
+  std::shared_ptr<Texture2D> src_tex =
+      tmp_denoised->genTexture(
+        false, (tmp_denoised->bitDepth()<32) ? cudaFilterModePoint
+                                             : cudaFilterModeLinear);
+  IMP_CUDA_CHECK();
+
+  Fragmentation<> frag(roi);
+
+  k_naturalEdges<Pixel, Pixel>
+      <<<
+        frag.dimGrid, frag.dimBlock
+      >>> (dst.data(), dst.stride(),
+           alpha, q,
+           roi.x(), roi.y(), roi.width(), roi.height(), *src_tex);
+
+  IMP_CUDA_CHECK();
+}
+
+//==============================================================================
+//
+// template instantiations for all our image types
+//
+
+template void naturalEdges(ImageGpu8uC1& dst, const ImageGpu8uC1& src, float sigma, float alpha, float q, ImageGpu8uC1::Ptr tmp_denoised);
+//template void naturalEdges(ImageGpu8uC2* dst, ImageGpu8uC1* src, float sigma, float alpha, float q);
+//template void naturalEdges(ImageGpu8uC3* dst, ImageGpu8uC1* src, float sigma, float alpha, float q);
+//template void naturalEdges(ImageGpu8uC4* dst, ImageGpu8uC1* src, float sigma, float alpha, float q);
+
+template void naturalEdges(ImageGpu32fC1& dst, const ImageGpu32fC1& src, float sigma, float alpha, float q, std::shared_ptr<ImageGpu32fC1> tmp_denoised);
+//template void naturalEdges(ImageGpu32fC2* dst, ImageGpu32fC1* src, float sigma, float alpha, float q);
+//template void naturalEdges(ImageGpu32fC3* dst, ImageGpu32fC1* src, float sigma, float alpha, float q);
+//template void naturalEdges(ImageGpu32fC4* dst, ImageGpu32fC1* src, float sigma, float alpha, float q);
+
+
+} // namespace cu
+} // namespace ze
+
+#endif // IM_CU_EDGE_DETECTORS_NATURAL_EDGES_IMPL_CUH
diff --git a/RWR/src/ze_oss/imp_cu_imgproc/src/cu_reduce.cu b/RWR/src/ze_oss/imp_cu_imgproc/src/cu_reduce.cu
new file mode 100644
index 0000000000000000000000000000000000000000..526dc0345ab1e13640388bc3a2415766344192b9
--- /dev/null
+++ b/RWR/src/ze_oss/imp_cu_imgproc/src/cu_reduce.cu
@@ -0,0 +1,151 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#include <imp/cu_imgproc/cu_reduce.cuh>
+
+#include <memory>
+#include <cstdint>
+#include <cmath>
+
+#include <cuda_runtime.h>
+
+#include <imp/core/types.hpp>
+#include <imp/core/roi.hpp>
+#include <imp/cu_core/cu_image_gpu.cuh>
+#include <imp/cu_core/cu_utils.hpp>
+#include <imp/cu_core/cu_texture.cuh>
+#include <imp/cu_imgproc/cu_image_filter.cuh>
+
+namespace ze {
+namespace cu {
+
+//-----------------------------------------------------------------------------
+template<typename Pixel>
+__global__ void k_reduce(Pixel* d_dst, size_t stride,
+                         uint32_t dst_width, uint32_t dst_height,
+                         uint32_t roi_x, uint32_t roi_y,
+                         float sf_x, float sf_y, Texture2D src_tex)
+{
+  const int x = blockIdx.x*blockDim.x + threadIdx.x + roi_x;
+  const int y = blockIdx.y*blockDim.y + threadIdx.y + roi_y;
+  if (x<dst_width && y<dst_height)
+  {
+    Pixel val;
+    tex2DFetch(val, src_tex, x, y, sf_x, sf_y);
+    d_dst[y*stride+x] = val;
+  }
+}
+
+//-----------------------------------------------------------------------------
+template<typename Pixel>
+void reduce(ImageGpu<Pixel>& dst,
+            const ImageGpu<Pixel>& src,
+            ze::InterpolationMode interp, bool gauss_prefilter)
+{
+  ze::Roi2u src_roi = src.roi();
+  ze::Roi2u dst_roi = dst.roi();
+
+  // scale factor for x/y > 0 && < 1 (for multiplication with dst coords in the kernel!)
+  float sf_x = static_cast<float>(src_roi.width()) / static_cast<float>(dst_roi.width());
+  float sf_y = static_cast<float>(src_roi.height()) / static_cast<float>(dst_roi.height());
+
+  std::unique_ptr<ImageGpu<Pixel>> filtered;
+  if (gauss_prefilter)
+  {
+    float sf = .5f*(sf_x+sf_y);
+
+    filtered.reset(new ImageGpu<Pixel>(src.size()));
+    float sigma = 1/(3*sf) ;  // empirical magic
+    std::uint16_t kernel_size = std::ceil(6.0f*sigma);
+    if (kernel_size % 2 == 0)
+      kernel_size++;
+
+    ze::cu::filterGauss(*filtered, src, sigma, kernel_size);
+  }
+
+  cudaTextureFilterMode tex_filter_mode = (interp == InterpolationMode::Linear) ?
+        cudaFilterModeLinear : cudaFilterModePoint;
+  if (src.bitDepth() < 32)
+    tex_filter_mode = cudaFilterModePoint;
+
+  std::shared_ptr<Texture2D> src_tex;
+  if (filtered)
+    src_tex = filtered->genTexture(false, tex_filter_mode);
+  else
+    src_tex = src.genTexture(false, tex_filter_mode);
+
+
+  Fragmentation<> dst_frag(dst_roi.size());
+
+  switch(interp)
+  {
+  case InterpolationMode::Point:
+  case InterpolationMode::Linear:
+    // fallthrough intended
+    k_reduce
+        <<<
+          dst_frag.dimGrid, dst_frag.dimBlock/*, 0, stream*/
+        >>> (dst.data(), dst.stride(), dst.width(), dst.height(),
+             dst_roi.x(), dst_roi.y(), sf_x , sf_y, *src_tex);
+    break;
+    //  case InterpolationMode::Cubic:
+    //    cuTransformCubicKernel_32f_C1
+    //        <<< dimGridOut, dimBlock, 0, stream >>> (dst.data(), dst.stride(), dst.width(), dst.height(),
+    //                                      sf_x , sf_y);
+    //    break;
+    //  case InterpolationMode::CubicSpline:
+    //    cuTransformCubicSplineKernel_32f_C1
+    //        <<< dimGridOut, dimBlock, 0, stream >>> (dst.data(), dst.stride(), dst.width(), dst.height(),
+    //                                      sf_x , sf_y);
+    //    break;
+  default:
+    CHECK(false) << "unsupported interpolation type";
+  }
+
+  IMP_CUDA_CHECK();
+}
+
+//==============================================================================
+//
+// template instantiations for all our image types
+//
+template void reduce(ImageGpu8uC1& dst, const ImageGpu8uC1& src, InterpolationMode interp, bool gauss_prefilter);
+template void reduce(ImageGpu8uC2& dst, const ImageGpu8uC2& src, InterpolationMode interp, bool gauss_prefilter);
+template void reduce(ImageGpu8uC4& dst, const ImageGpu8uC4& src, InterpolationMode interp, bool gauss_prefilter);
+
+template void reduce(ImageGpu16uC1& dst, const ImageGpu16uC1& src, InterpolationMode interp, bool gauss_prefilter);
+template void reduce(ImageGpu16uC2& dst, const ImageGpu16uC2& src, InterpolationMode interp, bool gauss_prefilter);
+template void reduce(ImageGpu16uC4& dst, const ImageGpu16uC4& src, InterpolationMode interp, bool gauss_prefilter);
+
+template void reduce(ImageGpu32sC1& dst, const ImageGpu32sC1& src, InterpolationMode interp, bool gauss_prefilter);
+template void reduce(ImageGpu32sC2& dst, const ImageGpu32sC2& src, InterpolationMode interp, bool gauss_prefilter);
+template void reduce(ImageGpu32sC4& dst, const ImageGpu32sC4& src, InterpolationMode interp, bool gauss_prefilter);
+
+template void reduce(ImageGpu32fC1& dst, const ImageGpu32fC1& src, InterpolationMode interp, bool gauss_prefilter);
+template void reduce(ImageGpu32fC2& dst, const ImageGpu32fC2& src, InterpolationMode interp, bool gauss_prefilter);
+template void reduce(ImageGpu32fC4& dst, const ImageGpu32fC4& src, InterpolationMode interp, bool gauss_prefilter);
+
+} // namespace cu
+} // namespace ze
+
diff --git a/RWR/src/ze_oss/imp_cu_imgproc/src/cu_resample.cu b/RWR/src/ze_oss/imp_cu_imgproc/src/cu_resample.cu
new file mode 100644
index 0000000000000000000000000000000000000000..84e6f754212402a04ad002911241a209dde2f83d
--- /dev/null
+++ b/RWR/src/ze_oss/imp_cu_imgproc/src/cu_resample.cu
@@ -0,0 +1,152 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#include <imp/cu_imgproc/cu_resample.cuh>
+
+#include <memory>
+#include <cstdint>
+#include <cmath>
+
+#include <cuda_runtime.h>
+
+#include <imp/core/types.hpp>
+#include <imp/core/roi.hpp>
+#include <imp/cu_core/cu_image_gpu.cuh>
+#include <imp/cu_core/cu_utils.hpp>
+#include <imp/cu_core/cu_texture.cuh>
+#include <imp/cu_imgproc/cu_image_filter.cuh>
+
+namespace ze {
+namespace cu {
+
+//-----------------------------------------------------------------------------
+template<typename Pixel>
+__global__ void k_resample(Pixel* d_dst, size_t stride,
+                           uint32_t dst_width, uint32_t dst_height,
+                           uint32_t roi_x, uint32_t roi_y,
+                           float sf_x, float sf_y, Texture2D src_tex)
+{
+  const int x = blockIdx.x*blockDim.x + threadIdx.x + roi_x;
+  const int y = blockIdx.y*blockDim.y + threadIdx.y + roi_y;
+  if (x<dst_width && y<dst_height)
+  {
+    Pixel val;
+    tex2DFetch(val, src_tex, x, y, sf_x, sf_y);
+    d_dst[y*stride+x] = val;
+  }
+}
+
+//-----------------------------------------------------------------------------
+template<typename Pixel>
+void resample(ImageGpu<Pixel>& dst,
+              const ImageGpu<Pixel>& src,
+              ze::InterpolationMode interp, bool gauss_prefilter)
+{
+  ze::Roi2u src_roi = src.roi();
+  ze::Roi2u dst_roi = dst.roi();
+
+  // scale factor for x/y > 0 && < 1 (for multiplication with dst coords in the kernel!)
+  float sf_x = static_cast<float>(src_roi.width()) / static_cast<float>(dst_roi.width());
+  float sf_y = static_cast<float>(src_roi.height()) / static_cast<float>(dst_roi.height());
+
+  cudaTextureFilterMode tex_filter_mode =
+      (interp == InterpolationMode::Linear) ? cudaFilterModeLinear
+                                            : cudaFilterModePoint;
+  if (src.bitDepth() < 32)
+    tex_filter_mode = cudaFilterModePoint;
+
+  std::shared_ptr<Texture2D> src_tex;
+  std::unique_ptr<ImageGpu<Pixel>> filtered;
+  if (gauss_prefilter)
+  {
+    float sf = .5f*(sf_x+sf_y);
+
+    filtered.reset(new ImageGpu<Pixel>(src.size()));
+    float sigma = 1/(3*sf) ;  // empirical magic
+    std::uint16_t kernel_size = std::ceil(6.0f*sigma);
+    if (kernel_size % 2 == 0)
+      kernel_size++;
+
+    ze::cu::filterGauss(*filtered, src, sigma, kernel_size);
+    src_tex = filtered->genTexture(false, tex_filter_mode);
+  }
+  else
+  {
+    src_tex = src.genTexture(false, tex_filter_mode);
+  }
+
+  Fragmentation<> dst_frag(dst_roi.size());
+
+  switch(interp)
+  {
+  case InterpolationMode::Point:
+  case InterpolationMode::Linear:
+    // fallthrough intended
+    k_resample
+        <<<
+          dst_frag.dimGrid, dst_frag.dimBlock/*, 0, stream*/
+        >>> (dst.data(), dst.stride(), dst.width(), dst.height(),
+             dst_roi.x(), dst_roi.y(), sf_x , sf_y, *src_tex);
+    break;
+    //  case InterpolationMode::Cubic:
+    //    cuTransformCubicKernel_32f_C1
+    //        <<< dimGridOut, dimBlock, 0, stream >>> (dst.data(), dst.stride(), dst.width(), dst.height(),
+    //                                      sf_x , sf_y);
+    //    break;
+    //  case InterpolationMode::CubicSpline:
+    //    cuTransformCubicSplineKernel_32f_C1
+    //        <<< dimGridOut, dimBlock, 0, stream >>> (dst.data(), dst.stride(), dst.width(), dst.height(),
+    //                                      sf_x , sf_y);
+    //    break;
+  default:
+    CHECK(false) << "unsupported interpolation type";
+  }
+
+  IMP_CUDA_CHECK();
+}
+
+//==============================================================================
+//
+// template instantiations for all our image types
+//
+
+template void resample(ImageGpu8uC1& dst, const ImageGpu8uC1& src, InterpolationMode interp, bool gauss_prefilter);
+template void resample(ImageGpu8uC2& dst, const ImageGpu8uC2& src, InterpolationMode interp, bool gauss_prefilter);
+template void resample(ImageGpu8uC4& dst, const ImageGpu8uC4& src, InterpolationMode interp, bool gauss_prefilter);
+
+template void resample(ImageGpu16uC1& dst, const ImageGpu16uC1& src, InterpolationMode interp, bool gauss_prefilter);
+template void resample(ImageGpu16uC2& dst, const ImageGpu16uC2& src, InterpolationMode interp, bool gauss_prefilter);
+template void resample(ImageGpu16uC4& dst, const ImageGpu16uC4& src, InterpolationMode interp, bool gauss_prefilter);
+
+template void resample(ImageGpu32sC1& dst, const ImageGpu32sC1& src, InterpolationMode interp, bool gauss_prefilter);
+template void resample(ImageGpu32sC2& dst, const ImageGpu32sC2& src, InterpolationMode interp, bool gauss_prefilter);
+template void resample(ImageGpu32sC4& dst, const ImageGpu32sC4& src, InterpolationMode interp, bool gauss_prefilter);
+
+template void resample(ImageGpu32fC1& dst, const ImageGpu32fC1& src, InterpolationMode interp, bool gauss_prefilter);
+template void resample(ImageGpu32fC2& dst, const ImageGpu32fC2& src, InterpolationMode interp, bool gauss_prefilter);
+template void resample(ImageGpu32fC4& dst, const ImageGpu32fC4& src, InterpolationMode interp, bool gauss_prefilter);
+
+
+} // namespace cu
+} // namespace ze
diff --git a/RWR/src/ze_oss/imp_cu_imgproc/src/cu_rof_denoising.cu b/RWR/src/ze_oss/imp_cu_imgproc/src/cu_rof_denoising.cu
new file mode 100644
index 0000000000000000000000000000000000000000..3baa8f6fdae531b070e70f51e55cb53125aec323
--- /dev/null
+++ b/RWR/src/ze_oss/imp_cu_imgproc/src/cu_rof_denoising.cu
@@ -0,0 +1,327 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#include <imp/cu_imgproc/cu_rof_denoising.cuh>
+
+#include <iostream>
+
+#include <cuda_runtime.h>
+
+#include <imp/cuda_toolkit/helper_math.hpp>
+#include <imp/core/pixel.hpp>
+#include <imp/cu_core/cu_texture.cuh>
+#include <imp/cu_core/cu_k_derivative.cuh>
+#include <imp/cu_core/cu_math.cuh>
+#include <ze/common/logging.hpp>
+
+
+namespace ze {
+namespace cu {
+
+//-----------------------------------------------------------------------------
+__global__ void k_initRofSolver(Pixel32fC1* d_u, Pixel32fC1* d_u_prev,
+                                size_t stride_u,
+                                Pixel32fC2* d_p, size_t stride_p,
+                                ze::cu::Texture2D f_tex,
+                                size_t width, size_t height)
+{
+  int x = blockIdx.x*blockDim.x + threadIdx.x;
+  int y = blockIdx.y*blockDim.y + threadIdx.y;
+
+  if (x<width && y<height)
+  {
+    float val = tex2DFetch<float>(f_tex, x, y);
+    d_u[y*stride_u + x] = val;
+    d_u_prev[y*stride_u + x] = val;
+    d_p[y*stride_p + x] = Pixel32fC2(0.0f, 0.0f);
+  }
+}
+
+//-----------------------------------------------------------------------------
+__global__ void k_rofPrimalUpdate(
+    Pixel32fC1* d_u, Pixel32fC1* d_u_prev, size_t stride_u,
+    Texture2D f_tex, Texture2D u_tex, Texture2D p_tex,
+    float lambda, float tau, float theta, size_t width, size_t height)
+{
+  int x = blockIdx.x*blockDim.x + threadIdx.x;
+  int y = blockIdx.y*blockDim.y + threadIdx.y;
+
+  if (x<width && y<height)
+  {
+    float f = tex2DFetch<float>(f_tex, x, y);
+    float u = tex2DFetch<float>(u_tex, x, y);
+    float u_prev = u;
+    float div = dpAd(p_tex, x, y, width, height);
+
+    u = (u + tau*(div + lambda*f)) / (1.0f + tau*lambda);
+
+    d_u[y*stride_u + x] = u;
+    d_u_prev[y*stride_u + x] = u + theta*(u-u_prev);
+  }
+}
+
+//-----------------------------------------------------------------------------
+__global__ void k_rofDualUpdate(
+    Pixel32fC2* d_p, size_t stride_p, Texture2D p_tex, Texture2D u_prev_tex,
+    float sigma, size_t width, size_t height)
+{
+  int x = blockIdx.x*blockDim.x + threadIdx.x;
+  int y = blockIdx.y*blockDim.y + threadIdx.y;
+
+  if (x<width && y<height)
+  {
+    float2 p = tex2DFetch<float2>(p_tex, x, y);
+    float2 dp_u = dp(u_prev_tex, x, y);
+
+    p = p + sigma*dp_u;
+    p = p / max(1.0f, length(p));
+    d_p[y*stride_p + x] = {p.x, p.y};
+  }
+}
+
+//-----------------------------------------------------------------------------
+__global__ void k_convertResult8uC1(Pixel8uC1* d_u, size_t stride_u,
+                                    ze::cu::Texture2D u_tex,
+                                    size_t width, size_t height)
+{
+  int x = blockIdx.x*blockDim.x + threadIdx.x;
+  int y = blockIdx.y*blockDim.y + threadIdx.y;
+
+  if (x<width && y<height)
+  {
+    d_u[y*stride_u + x] = static_cast<std::uint8_t>(
+          255.0f * tex2DFetch<float>(u_tex, x, y));
+  }
+}
+
+//-----------------------------------------------------------------------------
+__global__ void k_rofPrimalEnergy(Pixel32fC1* d_ep,  size_t stride,
+                                  uint32_t width, uint32_t height,
+                                  float lambda, Texture2D f_tex, Texture2D u_tex)
+{
+  int x = blockIdx.x*blockDim.x + threadIdx.x;
+  int y = blockIdx.y*blockDim.y + threadIdx.y;
+
+  if (x<width && y<height)
+  {
+    float2 dp_u = dp(u_tex, x, y);
+    float f = tex2DFetch<float>(f_tex, x, y);
+    float u = tex2DFetch<float>(u_tex, x, y);
+    d_ep[y*stride + x] = length(dp_u) + lambda/2.0f * ze::cu::sqr(u-f);
+  }
+}
+
+//-----------------------------------------------------------------------------
+__global__ void k_rofDualEnergy(Pixel32fC1* d_ed,  size_t stride,
+                                uint32_t width, uint32_t height,
+                                float lambda, Texture2D f_tex, Texture2D p_tex)
+{
+  int x = blockIdx.x*blockDim.x + threadIdx.x;
+  int y = blockIdx.y*blockDim.y + threadIdx.y;
+
+  if (x<width && y<height)
+  {
+    float f = tex2DFetch<float>(f_tex, x, y);
+    float div = dpAd(p_tex, x, y, width, height);
+    d_ed[y*stride + x] = -ze::cu::sqr(div)/(2.0f*lambda) - div*f;
+  }
+}
+
+
+//#############################################################################
+
+//-----------------------------------------------------------------------------
+template<typename Pixel>
+void RofDenoising<Pixel>::init(const Size2u& size)
+{
+  Base::init(size);
+  IMP_CUDA_CHECK();
+
+  // setup textures
+  // (except input image which is set when f_ is set from the input image)
+  u_tex_ = u_->genTexture(false, cudaFilterModeLinear, cudaAddressModeClamp,
+                          cudaReadModeElementType);
+  u_prev_tex_ = u_prev_->genTexture(false, cudaFilterModeLinear,
+                                    cudaAddressModeClamp, cudaReadModeElementType);
+  p_tex_ = p_->genTexture(false, cudaFilterModeLinear, cudaAddressModeClamp,
+                          cudaReadModeElementType);
+  IMP_CUDA_CHECK();
+
+  // init internal vars
+  k_initRofSolver
+      <<<
+        dimGrid(), dimBlock()
+      >>> (u_->data(), u_prev_->data(), u_->stride(),
+           p_->data(), p_->stride(),
+           *f_tex_, size_.width(), size_.height());
+  IMP_CUDA_CHECK();
+}
+
+//-----------------------------------------------------------------------------
+template<typename Pixel>
+void RofDenoising<Pixel>::denoise(const std::shared_ptr<ImageBase>& dst,
+                                  const std::shared_ptr<ImageBase>& src)
+{
+  VLOG(100) << "[Solver @gpu] RofDenoising::denoise:";
+  CHECK(src);
+  CHECK(dst);
+  CHECK_EQ(src->size(), dst->size());
+
+  f_ = src->as<ImageGpu>();
+  CHECK(f_);
+  f_tex_ = f_->genTexture(false, cudaFilterModeLinear, cudaAddressModeClamp,
+                          (f_->bitDepth()==8) ? cudaReadModeNormalizedFloat :
+                                                cudaReadModeElementType);
+
+  if (size_ != f_->size())
+  {
+    this->init(f_->size());
+  }
+
+  // internal algorithm params
+  float L = sqrtf(8.f);
+  float tau = 1.f/L;
+  float sigma = 1.f/L;
+  float theta = 1.f;
+
+  for(int iter = 0; iter < this->params_.max_iter; ++iter)
+  {
+    if (sigma < 1000.0f)
+    {
+      theta = 1.f/sqrtf(1.0f + 0.7f * this->params_.lambda * tau);
+    }
+    else
+    {
+      theta = 1.0f;
+    }
+
+    VLOG(101) << "(rof solver) iter: " << iter << "; tau: " << tau
+              << "; sigma: " << sigma << "; theta: " << theta;
+
+    if (params_.primal_dual_energy_check_iter > 0
+        && iter % params_.primal_dual_energy_check_iter
+        && params_.primal_dual_gap_tolerance > 0.f)
+    {
+      double primal_energy = 0.0, dual_energy = 0.0;
+      this->primalDualEnergy(primal_energy, dual_energy);
+      VLOG(102) << "ENERGIES: primal: " << primal_energy <<
+                   "; dual: " << dual_energy;
+    }
+
+    k_rofDualUpdate
+        <<<
+          dimGrid(), dimBlock()
+        >>> (p_->data(), p_->stride(), *p_tex_, *u_prev_tex_,
+             sigma, size_.width(), size_.height());
+
+    k_rofPrimalUpdate
+        <<<
+          dimGrid(), dimBlock()
+        >>> (u_->data(), u_prev_->data(), u_->stride(),
+             *f_tex_, *u_tex_, *p_tex_, params_.lambda, tau, theta,
+             size_.width(), size_.height());
+
+    sigma /= theta;
+    tau *= theta;
+  }
+  IMP_CUDA_CHECK();
+
+  switch (dst->pixelType())
+  {
+  case PixelType::i8uC1:
+  {
+    std::shared_ptr<ImageGpu8uC1> u(std::dynamic_pointer_cast<ImageGpu8uC1>(dst));
+    k_convertResult8uC1
+        <<<
+          dimGrid(), dimBlock()
+        >>> (u->data(), u->stride(),
+             *u_tex_, size_.width(), size_.height());
+  }
+    break;
+  case PixelType::i32fC1:
+  {
+    std::shared_ptr<ImageGpu32fC1> u(std::dynamic_pointer_cast<ImageGpu32fC1>(dst));
+    u_->copyTo(*u);
+  }
+    break;
+  default:
+    LOG(FATAL) << "Unsupported pixel type.";
+    break;
+  }
+  IMP_CUDA_CHECK();
+}
+
+//-----------------------------------------------------------------------------
+template<typename Pixel>
+void RofDenoising<Pixel>::primalDualEnergy(
+    double& primal_energy, double& dual_energy)
+{
+  if (!primal_energies_ || !dual_energies_)
+  {
+    primal_energies_.reset(new ImageGpu32fC1(size_));
+    dual_energies_.reset(new ImageGpu32fC1(size_));
+  }
+
+  Pixel32fC1 ep_min, ep_max, ed_min, ed_max;
+
+  k_rofPrimalEnergy
+      <<<
+        this->dimGrid(), this->dimBlock()
+      >>> (primal_energies_->data(), primal_energies_->stride(),
+           size_.width(), size_.height(), params_.lambda,
+           *f_tex_, *u_tex_);
+
+  // TODO sum
+  primal_energy = 10.0;
+  ze::cu::minMax(*primal_energies_, ep_min, ep_max);
+  IMP_CUDA_CHECK();
+
+  k_rofDualEnergy
+      <<<
+        this->dimGrid(), this->dimBlock()
+      >>> (dual_energies_->data(), dual_energies_->stride(),
+           size_.width(), size_.height(), params_.lambda,
+           *f_tex_, *p_tex_);
+  dual_energy = 20.0;
+  ze::cu::minMax(*dual_energies_, ed_min, ed_max);
+
+  IMP_CUDA_CHECK();
+}
+
+//-----------------------------------------------------------------------------
+template<typename Pixel>
+void RofDenoising<Pixel>::print(std::ostream& os) const
+{
+  os << "ROF Denoising:" << std::endl;
+  this->Base::print(os);
+}
+
+//=============================================================================
+// Explicitely instantiate the desired classes
+// (sync with typedefs at the end of the hpp file)
+template class RofDenoising<ze::Pixel8uC1>;
+template class RofDenoising<ze::Pixel32fC1>;
+
+} // namespace cu
+} // namespace ze
diff --git a/RWR/src/ze_oss/imp_cu_imgproc/src/cu_stereo_rectification.cu b/RWR/src/ze_oss/imp_cu_imgproc/src/cu_stereo_rectification.cu
new file mode 100644
index 0000000000000000000000000000000000000000..27e3f4c4f87b1a8eff97011ea4a2eb17caab6310
--- /dev/null
+++ b/RWR/src/ze_oss/imp_cu_imgproc/src/cu_stereo_rectification.cu
@@ -0,0 +1,160 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#include <imp/cu_core/cu_linearmemory.cuh>
+#include <imp/cu_core/cu_texture.cuh>
+#include <imp/cu_imgproc/cu_remap.cuh>
+#include <imp/cu_imgproc/cu_stereo_rectification.cuh>
+#include <ze/geometry/epipolar_geometry.hpp>
+
+namespace ze {
+namespace cu {
+
+//! @todo (MPI) test constant memory fo camera/dist parameters
+//! maybe also for the rectifying homography
+template<typename CameraModel,
+         typename DistortionModel>
+__global__
+void k_computeUndistortRectifyMap(
+    Pixel32fC2* dst,
+    size_t dst_stride,
+    std::uint32_t width,
+    std::uint32_t height,
+    const float* d_cam_params,
+    const float* d_transformed_cam_params,
+    const float* d_dist_coeffs,
+    const float* d_inv_H)
+{
+  const int u = blockIdx.x*blockDim.x + threadIdx.x;
+  const int v = blockIdx.y*blockDim.y + threadIdx.y;
+
+  if (u < width && v < height)
+  {
+    float px[2]{static_cast<float>(u), static_cast<float>(v)};
+    CameraModel::backProject(d_transformed_cam_params, px);
+    const float x = d_inv_H[0]*px[0]+d_inv_H[3]*px[1]+d_inv_H[6];
+    const float y = d_inv_H[1]*px[0]+d_inv_H[4]*px[1]+d_inv_H[7];
+    const float w = d_inv_H[2]*px[0]+d_inv_H[5]*px[1]+d_inv_H[8];
+    px[0] = x / w;
+    px[1] = y / w;
+    DistortionModel::distort(d_dist_coeffs, px);
+    CameraModel::project(d_cam_params, px);
+    dst[v*dst_stride + u][0] = px[0];
+    dst[v*dst_stride + u][1] = px[1];
+  }
+}
+
+template <typename CameraModel,
+          typename DistortionModel,
+          typename Pixel>
+StereoRectifier<CameraModel, DistortionModel, Pixel>::StereoRectifier(
+    const Size2u& img_size,
+    const Vector4& camera_params,
+    const Vector4& transformed_camera_params,
+    const Vector4& dist_coeffs,
+    const Matrix3& inv_H)
+  : undistort_rectify_map_(img_size)
+  , fragm_(img_size)
+{
+  //! Convert to float and upload to GPU
+  Eigen::Vector4f cp_flt = camera_params.cast<float>();
+  Eigen::Vector4f tcp_flt = transformed_camera_params.cast<float>();
+  Eigen::Vector4f dist_flt = dist_coeffs.cast<float>();
+  Eigen::Matrix3f inv_H_flt = inv_H.cast<float>();
+  //! Copy to host LinearMemory
+  ze::LinearMemory32fC1 h_cam_params(
+        reinterpret_cast<Pixel32fC1*>(cp_flt.data()),
+        4, true);
+  ze::LinearMemory32fC1 h_transformed_cam_params(
+        reinterpret_cast<Pixel32fC1*>(tcp_flt.data()),
+        4, true);
+  ze::LinearMemory32fC1 h_dist_coeffs(
+        reinterpret_cast<Pixel32fC1*>(dist_flt.data()),
+        4, true);
+  ze::LinearMemory32fC1 h_inv_H(
+        reinterpret_cast<Pixel32fC1*>(inv_H_flt.data()),
+        9, true);
+  //! Copy to device LinearMemory
+  cu::LinearMemory32fC1 d_cam_params(h_cam_params);
+  cu::LinearMemory32fC1 d_transformed_cam_params(h_transformed_cam_params);
+  cu::LinearMemory32fC1 d_dist_coeffs(h_dist_coeffs);
+  cu::LinearMemory32fC1 d_inv_H(h_inv_H);
+
+  //! Compute map
+  k_computeUndistortRectifyMap<CameraModel, DistortionModel>
+      <<<
+        fragm_.dimGrid, fragm_.dimBlock
+      >>> (undistort_rectify_map_.data(),
+           undistort_rectify_map_.stride(),
+           undistort_rectify_map_.width(),
+           undistort_rectify_map_.height(),
+           d_cam_params.cuData(),
+           d_transformed_cam_params.cuData(),
+           d_dist_coeffs.cuData(),
+           d_inv_H.cuData());
+}
+
+template <typename CameraModel,
+          typename DistortionModel,
+          typename Pixel>
+void StereoRectifier<CameraModel, DistortionModel, Pixel>::rectify(
+    ImageGpu<Pixel>& dst,
+    const ImageGpu<Pixel>& src) const
+{
+  CHECK_EQ(src.size(), dst.size());
+  CHECK_EQ(src.size(), undistort_rectify_map_.size());
+
+  // Attach texture
+  std::shared_ptr<Texture2D> src_tex =
+      src.genTexture(false, cudaFilterModeLinear);
+  IMP_CUDA_CHECK();
+
+  //! Execute remapping
+  k_remap
+      <<<
+        fragm_.dimGrid, fragm_.dimBlock
+      >>> (dst.data(),
+           dst.stride(),
+           undistort_rectify_map_.data(),
+           undistort_rectify_map_.stride(),
+           dst.width(),
+           dst.height(),
+           *src_tex);
+  IMP_CUDA_CHECK();
+}
+
+template <typename CameraModel,
+          typename DistortionModel,
+          typename Pixel>
+const ImageGpu32fC2& StereoRectifier<CameraModel, DistortionModel, Pixel>::getUndistortRectifyMap() const
+{
+  return undistort_rectify_map_;
+}
+
+// Explicit template instantiations
+template class StereoRectifier<PinholeGeometry, EquidistantDistortion, Pixel32fC1>;
+template class StereoRectifier<PinholeGeometry, RadialTangentialDistortion, Pixel32fC1>;
+
+} // cu namespace
+} // ze namespace
diff --git a/RWR/src/ze_oss/imp_cu_imgproc/src/cu_tvl1_denoising.cu b/RWR/src/ze_oss/imp_cu_imgproc/src/cu_tvl1_denoising.cu
new file mode 100644
index 0000000000000000000000000000000000000000..b30a2503de2f665ecbd612ed9c44b6131a063196
--- /dev/null
+++ b/RWR/src/ze_oss/imp_cu_imgproc/src/cu_tvl1_denoising.cu
@@ -0,0 +1,245 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#include <imp/cu_imgproc/cu_tvl1_denoising.cuh>
+
+#include <iostream>
+#include <cuda_runtime.h>
+#include <ze/common/logging.hpp>
+#include <imp/core/pixel.hpp>
+#include <imp/cu_core/cu_math.cuh>
+#include <imp/cu_core/cu_texture.cuh>
+#include <imp/cuda_toolkit/helper_math.hpp>
+// kernels
+#include <imp/cu_core/cu_k_derivative.cuh>
+
+
+namespace ze {
+namespace cu {
+
+//-----------------------------------------------------------------------------
+__global__ void k_initTvL1Solver(Pixel32fC1* d_u, Pixel32fC1* d_u_prev, size_t stride_u,
+                                 Pixel32fC2* d_p, size_t stride_p,
+                                 ze::cu::Texture2D f_tex,
+                                 size_t width, size_t height)
+{
+  int x = blockIdx.x*blockDim.x + threadIdx.x;
+  int y = blockIdx.y*blockDim.y + threadIdx.y;
+
+  if (x<width && y<height)
+  {
+    float val = tex2D<float>(f_tex, x+.5f, y+.5f);
+    d_u[y*stride_u + x] = val;
+    d_u_prev[y*stride_u + x] = val;
+    d_p[y*stride_p + x] = Pixel32fC2(0.0f, 0.0f);
+  }
+}
+
+//-----------------------------------------------------------------------------
+__global__ void k_tvL1PrimalUpdate(
+    Pixel32fC1* d_u, Pixel32fC1* d_u_prev, size_t stride_u,
+    Texture2D f_tex, Texture2D u_tex, Texture2D p_tex,
+    float lambda, float tau, float theta, size_t width, size_t height)
+{
+  int x = blockIdx.x*blockDim.x + threadIdx.x;
+  int y = blockIdx.y*blockDim.y + threadIdx.y;
+
+  if (x<width && y<height)
+  {
+    float f = tex2D<float>(f_tex, x+.5f, y+.5f);
+    float u = tex2D<float>(u_tex, x+.5f, y+.5f);
+    float u_prev = u;
+    float div = dpAd(p_tex, x, y, width, height);
+    u += tau*div;
+
+    float tau_lambda = tau*lambda;
+    float residual = u - f;
+    if (residual < -tau_lambda)
+    {
+      u += tau_lambda;
+    }
+    else if (residual > tau_lambda)
+    {
+      u -= tau_lambda;
+    }
+    else
+    {
+      u = f;
+    }
+
+    d_u[y*stride_u + x] = u;
+    d_u_prev[y*stride_u + x] = u + theta*(u-u_prev);
+  }
+}
+
+//-----------------------------------------------------------------------------
+__global__ void k_tvL1DualUpdate(
+    Pixel32fC2* d_p, size_t stride_p, Texture2D p_tex, Texture2D u_prev_tex,
+    float sigma, size_t width, size_t height)
+{
+  int x = blockIdx.x*blockDim.x + threadIdx.x;
+  int y = blockIdx.y*blockDim.y + threadIdx.y;
+
+  if (x<width && y<height)
+  {
+    float2 p = tex2D<float2>(p_tex, x+.5f, y+.5f);
+    float2 dp_u = dp(u_prev_tex, x, y);
+
+    p = p + sigma*dp_u;
+    p = p / max(1.0f, length(p));
+    d_p[y*stride_p + x] = {p.x, p.y};
+  }
+}
+
+//-----------------------------------------------------------------------------
+//! @todo (MWE) move to a common place (also needed for other algorithms!)
+__global__ void k_tvL1convertResult8uC1(Pixel8uC1* d_u, size_t stride_u,
+                                        ze::cu::Texture2D u_tex,
+                                        size_t width, size_t height)
+{
+  int x = blockIdx.x*blockDim.x + threadIdx.x;
+  int y = blockIdx.y*blockDim.y + threadIdx.y;
+
+  if (x<width && y<height)
+  {
+    d_u[y*stride_u + x] = static_cast<std::uint8_t>(
+          255.0f * tex2D<float>(u_tex, x+.5f, y+.5f));
+  }
+}
+
+//#############################################################################
+
+//-----------------------------------------------------------------------------
+template<typename Pixel>
+void TvL1Denoising<Pixel>::init(const Size2u& size)
+{
+  Base::init(size);
+  IMP_CUDA_CHECK();
+
+  // setup textures
+  f_tex_ = f_->genTexture(false, cudaFilterModeLinear, cudaAddressModeClamp,
+                          (f_->bitDepth()==8) ? cudaReadModeNormalizedFloat :
+                                                cudaReadModeElementType);
+  u_tex_ = u_->genTexture(false, cudaFilterModeLinear, cudaAddressModeClamp,
+                          cudaReadModeElementType);
+  u_prev_tex_ = u_prev_->genTexture(false, cudaFilterModeLinear,
+                                    cudaAddressModeClamp, cudaReadModeElementType);
+  p_tex_ = p_->genTexture(false, cudaFilterModeLinear, cudaAddressModeClamp,
+                          cudaReadModeElementType);
+  IMP_CUDA_CHECK();
+
+  // init internal vars
+  k_initTvL1Solver
+      <<< dimGrid(), dimBlock() >>> (u_->data(), u_prev_->data(), u_->stride(),
+                                     p_->data(), p_->stride(),
+                                     *f_tex_, size_.width(), size_.height());
+  IMP_CUDA_CHECK();
+}
+
+//-----------------------------------------------------------------------------
+template<typename Pixel>
+void TvL1Denoising<Pixel>::denoise(const ImageBase::Ptr& dst,
+                                   const ImageBase::Ptr& src)
+{
+  VLOG(100) << "[Solver @gpu] TvL1Denoising::denoise:";
+  CHECK_EQ(src->size(), dst->size());
+
+  f_ = std::dynamic_pointer_cast<ImageGpu>(src);
+  //! @todo (MWE) we could use dst for u_ if pixel_type is consistent
+
+  if (size_ != f_->size())
+  {
+    this->init(f_->size());
+  }
+
+  // internal params
+  float theta = 1.0f;
+  //float L = sqrtf(8.0f);
+  float sigma = 1.f/sqrtf(8.0f);
+  float tau = 1.f/8.f;
+
+  for(int iter = 0; iter < this->params_.max_iter; ++iter)
+  {
+    if (sigma < 1000.0f)
+      theta = 1.f/sqrtf(1.0f+0.7f*this->params_.lambda*tau);
+    else
+      theta = 1.0f;
+
+    VLOG(101) << "(TvL1 solver) iter: " << iter << "; tau: " << tau
+              << "; sigma: " << sigma << "; theta: " << theta;
+
+    k_tvL1DualUpdate
+        <<< dimGrid(), dimBlock() >>> (p_->data(), p_->stride(),
+                                       *p_tex_, *u_prev_tex_,
+                                       sigma, size_.width(), size_.height());
+
+    k_tvL1PrimalUpdate
+        <<< dimGrid(), dimBlock() >>> (u_->data(), u_prev_->data(), u_->stride(),
+                                       *f_tex_, *u_tex_, *p_tex_,
+                                       params_.lambda, tau, theta,
+                                       size_.width(), size_.height());
+
+    sigma /= theta;
+    tau *= theta;
+  }
+  IMP_CUDA_CHECK();
+
+  switch (dst->pixelType())
+  {
+  case PixelType::i8uC1:
+  {
+    std::shared_ptr<ImageGpu8uC1> u(std::dynamic_pointer_cast<ImageGpu8uC1>(dst));
+    k_tvL1convertResult8uC1
+        <<< dimGrid(), dimBlock() >>> (u->data(), u->stride(),
+                                       *u_tex_, size_.width(), size_.height());
+  }
+    break;
+  case PixelType::i32fC1:
+  {
+    std::shared_ptr<ImageGpu32fC1> u(std::dynamic_pointer_cast<ImageGpu32fC1>(dst));
+    u_->copyTo(*u);
+  }
+    break;
+  default:
+    CHECK(false) << "Unsupported PixelType";
+  }
+  IMP_CUDA_CHECK();
+}
+
+//-----------------------------------------------------------------------------
+template<typename Pixel>
+void TvL1Denoising<Pixel>::print(std::ostream& os) const
+{
+  os << "TvL1 Denoising:" << std::endl;
+  this->Base::print(os);
+}
+
+//=============================================================================
+// Explicitely instantiate the desired classes
+// (sync with typedefs at the end of the hpp file)
+template class TvL1Denoising<ze::Pixel8uC1>;
+template class TvL1Denoising<ze::Pixel32fC1>;
+
+} // namespace cu
+} // namespace ze
diff --git a/RWR/src/ze_oss/imp_cu_imgproc/src/cu_undistortion.cu b/RWR/src/ze_oss/imp_cu_imgproc/src/cu_undistortion.cu
new file mode 100644
index 0000000000000000000000000000000000000000..ab7bf54888a531cace586bb7c0726ff32fb8388e
--- /dev/null
+++ b/RWR/src/ze_oss/imp_cu_imgproc/src/cu_undistortion.cu
@@ -0,0 +1,130 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#include <imp/cu_core/cu_linearmemory.cuh>
+#include <imp/cu_core/cu_texture.cuh>
+#include <imp/cu_imgproc/cu_remap.cuh>
+#include <imp/cu_imgproc/cu_undistortion.cuh>
+
+namespace ze {
+namespace cu {
+
+template<typename CameraModel,
+         typename DistortionModel>
+__global__
+void k_computeUndistortionMap(
+    Pixel32fC2* dst,
+    size_t dst_stride,
+    uint32_t width,
+    uint32_t height,
+    const float* d_cam_params,
+    const float* d_dist_coeffs)
+{
+  const int x = blockIdx.x*blockDim.x + threadIdx.x;
+  const int y = blockIdx.y*blockDim.y + threadIdx.y;
+
+  if (x < width && y < height)
+  {
+    float px[2]{static_cast<float>(x), static_cast<float>(y)};
+    CameraModel::backProject(d_cam_params, px);
+    DistortionModel::distort(d_dist_coeffs, px);
+    CameraModel::project(d_cam_params, px);
+    dst[y*dst_stride + x][0] = px[0];
+    dst[y*dst_stride + x][1] = px[1];
+  }
+}
+
+template <typename CameraModel,
+          typename DistortionModel,
+          typename Pixel>
+ImageUndistorter<CameraModel, DistortionModel, Pixel>::ImageUndistorter(
+    const Size2u& img_size,
+    const VectorX& camera_params,
+    const VectorX& dist_coeffs)
+  : undistortion_map_(img_size),
+    fragm_(img_size)
+{
+  // Cast to float and allocate host linear memory
+  Eigen::VectorXf cp_flt = camera_params.cast<float>();
+  Eigen::VectorXf dist_flt = dist_coeffs.cast<float>();
+  ze::LinearMemory32fC1 h_cam_params(
+        reinterpret_cast<Pixel32fC1*>(cp_flt.data()),
+        camera_params.rows(), true);
+  ze::LinearMemory32fC1 h_dist_coeffs(
+        reinterpret_cast<Pixel32fC1*>(dist_flt.data()),
+        dist_coeffs.rows(), true);
+  // Copy to device linear memory
+  cu::LinearMemory32fC1 d_cam_params(h_cam_params);
+  cu::LinearMemory32fC1 d_dist_coeffs(h_dist_coeffs);
+
+  k_computeUndistortionMap<CameraModel, DistortionModel>
+      <<<
+        fragm_.dimGrid, fragm_.dimBlock
+      >>> (undistortion_map_.data(),
+           undistortion_map_.stride(),
+           undistortion_map_.width(),
+           undistortion_map_.height(),
+           d_cam_params.cuData(),
+           d_dist_coeffs.cuData());
+}
+
+template <typename CameraModel,
+          typename DistortionModel,
+          typename Pixel>
+void ImageUndistorter<CameraModel, DistortionModel, Pixel>::undistort(
+    ImageGpu<Pixel>& dst,
+    const ImageGpu<Pixel>& src) const
+{
+  CHECK_EQ(src.size(), dst.size());
+  CHECK_EQ(src.size(), undistortion_map_.size());
+  std::shared_ptr<Texture2D> src_tex =
+      src.genTexture(false, cudaFilterModeLinear);
+  IMP_CUDA_CHECK();
+  k_remap
+      <<<
+        fragm_.dimGrid, fragm_.dimBlock
+      >>> (dst.data(),
+           dst.stride(),
+           undistortion_map_.data(),
+           undistortion_map_.stride(),
+           dst.width(),
+           dst.height(),
+           *src_tex);
+  IMP_CUDA_CHECK();
+}
+
+template <typename CameraModel,
+          typename DistortionModel,
+          typename Pixel>
+const ImageGpu32fC2& ImageUndistorter<CameraModel, DistortionModel, Pixel>::getUndistortionMap() const
+{
+  return undistortion_map_;
+}
+
+// Explicit template instantiations
+template class ImageUndistorter<PinholeGeometry, EquidistantDistortion, Pixel32fC1>;
+template class ImageUndistorter<PinholeGeometry, RadialTangentialDistortion, Pixel32fC1>;
+
+} // cu namespace
+} // ze namespace
diff --git a/RWR/src/ze_oss/imp_cu_imgproc/src/cu_variational_denoising.cu b/RWR/src/ze_oss/imp_cu_imgproc/src/cu_variational_denoising.cu
new file mode 100644
index 0000000000000000000000000000000000000000..12aa058e66ea0fb47779368e32a3363160a56507
--- /dev/null
+++ b/RWR/src/ze_oss/imp_cu_imgproc/src/cu_variational_denoising.cu
@@ -0,0 +1,60 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#include <imp/cu_imgproc/cu_variational_denoising.cuh>
+#include <imp/cu_core/cu_texture.cuh>
+
+namespace ze {
+namespace cu {
+
+//-----------------------------------------------------------------------------
+VariationalDenoising::VariationalDenoising()
+  : f_tex_(nullptr)
+  , u_tex_(nullptr)
+  , u_prev_tex_(nullptr)
+  , p_tex_(nullptr)
+{
+}
+
+//-----------------------------------------------------------------------------
+VariationalDenoising::~VariationalDenoising()
+{
+}
+
+//-----------------------------------------------------------------------------
+__host__ void VariationalDenoising::init(const Size2u& size)
+{
+  size_ = size;
+  fragmentation_.reset(new Fragmentation(size));
+
+  // setup internal memory
+  this->u_.reset(new ImageGpu32fC1(size));
+  this->u_prev_.reset(new ImageGpu32fC1(size));
+  this->p_.reset(new ImageGpu32fC2(size));
+}
+
+
+} // namespace cu
+} // namespace ze
+
diff --git a/RWR/src/ze_oss/imp_cu_imgproc/src/image_pyramid.cpp b/RWR/src/ze_oss/imp_cu_imgproc/src/image_pyramid.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..3184e748acece0da43365ebe0ba0c53ce0799d5a
--- /dev/null
+++ b/RWR/src/ze_oss/imp_cu_imgproc/src/image_pyramid.cpp
@@ -0,0 +1,103 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#include <imp/cu_imgproc/image_pyramid.hpp>
+
+#include <glog/logging.h>
+#include <imp/core/pixel_enums.hpp>
+
+namespace ze {
+
+//------------------------------------------------------------------------------
+template<typename Pixel>
+ImagePyramid<Pixel>::ImagePyramid(
+    Size2u size, float scale_factor, uint32_t size_bound, uint32_t max_num_levels)
+  : scale_factor_(scale_factor)
+  , size_bound_(size_bound)
+  , max_num_levels_(max_num_levels)
+{
+  this->init(size);
+}
+
+//------------------------------------------------------------------------------
+template<typename Pixel>
+void ImagePyramid<Pixel>::clear() noexcept
+{
+  levels_.clear();
+  scale_factors_.clear();
+}
+
+//------------------------------------------------------------------------------
+template<typename Pixel>
+void ImagePyramid<Pixel>::init(const ze::Size2u& size)
+{
+  CHECK_GT(scale_factor_, 0.0f);
+  CHECK_LT(scale_factor_, 1.0f);
+
+  if (!levels_.empty() || scale_factors_.empty())
+  {
+    this->clear();
+  }
+
+  uint32_t shorter_side = std::min(size.width(), size.height());
+
+  // calculate the maximum number of levels
+  float ratio = static_cast<float>(shorter_side)/static_cast<float>(size_bound_);
+  // +1 because the original size is level 0
+  size_t possible_num_levels =
+      static_cast<int>(-std::log(ratio)/std::log(scale_factor_)) + 1;
+  num_levels_ = std::min(max_num_levels_, possible_num_levels);
+
+  // init rate for each level
+  for (size_t i = 0; i<num_levels_; ++i)
+  {
+    scale_factors_.push_back(std::pow(scale_factor_, static_cast<float>(i)));
+  }
+}
+
+//=============================================================================
+// Explicitely instantiate the desired classes
+// (sync with typedefs at the end of the hpp file)
+template class ImagePyramid<ze::Pixel8uC1>;
+template class ImagePyramid<ze::Pixel8uC2>;
+//template class ImagePyramid<imp::Pixel8uC3>;
+template class ImagePyramid<ze::Pixel8uC4>;
+
+template class ImagePyramid<ze::Pixel16uC1>;
+template class ImagePyramid<ze::Pixel16uC2>;
+//template class ImagePyramid<imp::Pixel16uC3>;
+template class ImagePyramid<ze::Pixel16uC4>;
+
+template class ImagePyramid<ze::Pixel32sC1>;
+template class ImagePyramid<ze::Pixel32sC2>;
+//template class ImagePyramid<imp::Pixel32sC3>;
+template class ImagePyramid<ze::Pixel32sC4>;
+
+template class ImagePyramid<ze::Pixel32fC1>;
+template class ImagePyramid<ze::Pixel32fC2>;
+//template class ImagePyramid<imp::Pixel32fC3>;
+template class ImagePyramid<ze::Pixel32fC4>;
+
+} // namespace ze
+
diff --git a/RWR/src/ze_oss/imp_cu_imgproc/src/str_tex_decomposer.cpp b/RWR/src/ze_oss/imp_cu_imgproc/src/str_tex_decomposer.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..7354d605a704aa7433ece07db403bee2532f4eba
--- /dev/null
+++ b/RWR/src/ze_oss/imp_cu_imgproc/src/str_tex_decomposer.cpp
@@ -0,0 +1,76 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#include <imp/cu_imgproc/str_tex_decomposer.hpp>
+
+#include <imp/cu_core/cu_math.cuh>
+#include <imp/cu_imgproc/cu_rof_denoising.cuh>
+#include <ze/common/logging.hpp>
+
+namespace ze {
+namespace cu {
+//-----------------------------------------------------------------------------
+template<typename Pixel>
+StrTexDecomposer<Pixel>::StrTexDecomposer()
+  : denoiser_(new RofDenoising<Pixel>())
+{
+
+}
+
+//-----------------------------------------------------------------------------
+template<typename Pixel>
+void StrTexDecomposer<Pixel>::solve(
+    const ze::cu::ImageGpuPtr<Pixel>& tex_image,
+    const ze::cu::ImageGpuPtr<Pixel>& src,
+    const ze::cu::ImageGpuPtr<Pixel>& structure_image)
+{
+  CHECK(src);
+  CHECK(tex_image);
+
+  CHECK_EQ(src->size(), tex_image->size());
+  if (structure_image)
+  {
+    CHECK_EQ(src->size(), structure_image->size());
+  }
+
+  src_ = src;
+  tex_ = tex_image;
+  // in-place denoising if no structure image desired for an output
+  str_ = structure_image ? structure_image : tex_image;
+
+  denoiser_->params().lambda = 1.0f;
+  denoiser_->params().max_iter = 100;
+  denoiser_->denoise(str_, src_);
+  ze::cu::weightedSum(*tex_, *src_, weight_, *str_, 1.f-2.f*weight_);
+}
+
+//=============================================================================
+// Explicitely instantiate the desired classes
+// (sync with typedefs at the end of the hpp file)
+template class StrTexDecomposer<ze::Pixel8uC1>;
+template class StrTexDecomposer<ze::Pixel32fC1>;
+
+
+} // namespace cu
+} // namespace ze
diff --git a/RWR/src/ze_oss/imp_cu_imgproc/test/test_cu_image_pyramid.cpp b/RWR/src/ze_oss/imp_cu_imgproc/test/test_cu_image_pyramid.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..dcec7628db3d6141ad66f4b0349849aa9af3fbc6
--- /dev/null
+++ b/RWR/src/ze_oss/imp_cu_imgproc/test/test_cu_image_pyramid.cpp
@@ -0,0 +1,65 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#include <sstream>
+#include <string>
+
+#include <ze/common/test_entrypoint.hpp>
+#include <ze/common/test_utils.hpp>
+#include <ze/common/benchmark.hpp>
+#include <ze/common/types.hpp>
+
+#include <imp/core/image.hpp>
+#include <imp/core/image_raw.hpp>
+#include <imp/bridge/opencv/cu_cv_bridge.hpp>
+#include <imp/cu_imgproc/image_pyramid.hpp>
+
+DEFINE_bool(visualize, false, "Show input images and results");
+
+TEST(ImagePyramidTest, testImagePyramidGPU)
+{
+  using namespace ze::cu;
+
+  std::string data_path = ze::getTestDataDir("computer_vision_images");
+  ImageGpu32fC1::Ptr cuimg;
+  cvBridgeLoad(cuimg, data_path + "/lena_grayscale_511x512.png", ze::PixelOrder::gray);
+
+  auto pyr = createImagePyramidGpu<ze::Pixel32fC1>(cuimg, 0.8);
+
+  VLOG(200) << "IMAGE PYRAMID: num levels: " << pyr->numLevels();
+
+  if(FLAGS_visualize)
+  {
+    for (size_t level=0; level < pyr->numLevels(); ++level)
+    {
+      std::stringstream windowname;
+      VLOG(200) << "showing pyramid level " << level;
+      windowname << "praymid level " << level;
+      cvBridgeShow(windowname.str(), dynamic_cast<ImageGpu32fC1&>(pyr->at(level)));
+    }
+    cv::waitKey(0);
+  }
+}
+
+ZE_UNITTEST_ENTRYPOINT
diff --git a/RWR/src/ze_oss/imp_cu_imgproc/test/test_cu_rof.cpp b/RWR/src/ze_oss/imp_cu_imgproc/test/test_cu_rof.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..127ad2523d6436e1b8f002f10b3eebf695eb7818
--- /dev/null
+++ b/RWR/src/ze_oss/imp_cu_imgproc/test/test_cu_rof.cpp
@@ -0,0 +1,98 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#include <sstream>
+#include <string>
+
+#include <ze/common/test_entrypoint.hpp>
+#include <ze/common/test_utils.hpp>
+#include <ze/common/benchmark.hpp>
+#include <ze/common/types.hpp>
+
+#include <imp/cu_core/cu_image_gpu.cuh>
+#include <imp/bridge/opencv/cu_cv_bridge.hpp>
+#include <imp/cu_imgproc/cu_rof_denoising.cuh>
+
+DEFINE_bool(visualize, false, "Show input images and results");
+
+template <typename Pixel>
+class CuRofDenoiseTestFixture : public ::testing::Test
+{
+protected:
+  CuRofDenoiseTestFixture()
+    : data_path_(ze::getTestDataDir("computer_vision_images"))
+  {
+  }
+
+  void loadLenaGrayscale()
+  {
+    ze::cu::cvBridgeLoad(
+          in_, data_path_ + "/lena_grayscale_511x512.png", ze::PixelOrder::gray);
+  }
+
+  void denoise()
+  {
+    denoised_ = std::make_shared<ze::cu::ImageGpu<Pixel>>(in_->size());
+    rof_ = std::make_shared<ze::cu::RofDenoising<Pixel>>();
+
+    rof_->params().primal_dual_energy_check_iter = 10;
+    rof_->params().primal_dual_gap_tolerance = 1e-3;
+
+    rof_->denoise(denoised_, in_);
+  }
+
+protected:
+  std::string data_path_;
+  ze::cu::RofDenoisingPtr<Pixel> rof_;
+  ze::cu::ImageGpuPtr<Pixel> in_;
+  ze::cu::ImageGpuPtr<Pixel> denoised_;
+};
+
+
+// The list of types we want to test.
+typedef testing::Types<
+ze::Pixel8uC1, ze::Pixel32fC1
+> PixelTypes;
+
+TYPED_TEST_CASE(CuRofDenoiseTestFixture, PixelTypes);
+
+TYPED_TEST(CuRofDenoiseTestFixture, VisualTestRofDenoising)
+{
+  using namespace ze::cu;
+
+  this->loadLenaGrayscale();
+  this->denoise();
+
+  if(FLAGS_visualize)
+  {
+    cvBridgeShow("Lena", *this->in_);
+    std::stringstream windowname;
+    windowname << "ROF Lena ("
+               << static_cast<int>(this->denoised_->bitDepth()) << "-bit)";
+    cvBridgeShow(windowname.str(), *this->denoised_);
+    cv::waitKey();
+  }
+}
+
+ZE_UNITTEST_ENTRYPOINT
diff --git a/RWR/src/ze_oss/imp_cu_imgproc/test/test_cu_stereo_rectifier.cpp b/RWR/src/ze_oss/imp_cu_imgproc/test/test_cu_stereo_rectifier.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..3e46b9d8b61bfc81fde5ed70bac7c7d97f22f30a
--- /dev/null
+++ b/RWR/src/ze_oss/imp_cu_imgproc/test/test_cu_stereo_rectifier.cpp
@@ -0,0 +1,212 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#include <imp/bridge/opencv/cv_bridge.hpp>
+#include <imp/cu_imgproc/cu_stereo_rectification.cuh>
+#include <imp/cu_imgproc/cu_horizontal_stereo_pair_rectifier.cuh>
+#include <ze/cameras/camera_rig.hpp>
+#include <ze/common/benchmark.hpp>
+#include <ze/common/file_utils.hpp>
+#include <ze/common/test_entrypoint.hpp>
+#include <ze/common/test_utils.hpp>
+
+namespace ze {
+
+void testRectificationMapAgainstFile(
+    const ImageCv32fC2& map,
+    const std::string& map_x_file_path,
+    const std::string& map_y_file_path,
+    float tolerance)
+{
+  CHECK(fileExists(map_x_file_path));
+  CHECK(fileExists(map_y_file_path));
+
+  std::ifstream map_x_file(map_x_file_path, std::ofstream::binary);
+  std::ifstream map_y_file(map_y_file_path, std::ofstream::binary);
+  CHECK(map_x_file.is_open());
+  CHECK(map_y_file.is_open());
+
+  const size_t map_width = map.width();
+  const size_t map_height = map.height();
+  const size_t map_n_elems = map_width * map_height;
+
+  std::unique_ptr<float[]> gt_map_x(new float[map_n_elems]);
+  std::unique_ptr<float[]> gt_map_y(new float[map_n_elems]);
+
+  map_x_file.read(
+        reinterpret_cast<char*>(gt_map_x.get()), map_n_elems*sizeof(float));
+  map_y_file.read(
+        reinterpret_cast<char*>(gt_map_y.get()), map_n_elems*sizeof(float));
+
+  // Compare computed map with ground-truth map
+  for (uint32_t y = 0; y < map_height; ++y)
+  {
+    for (uint32_t x = 0; x < map_width; ++x)
+    {
+      EXPECT_NEAR(
+            gt_map_x.get()[y*map_width+x], map(x, y)[0], tolerance);
+      EXPECT_NEAR(
+            gt_map_y.get()[y*map_width+x], map(x, y)[1], tolerance);
+    }
+  }
+}
+
+} // ze namespace
+
+using namespace ze;
+
+TEST(impCuStereoRectifierTexture, radTan32fC1)
+{
+  constexpr float c_map_tolearance{0.001};
+  const std::string test_folder =
+      ze::joinPath(ze::getTestDataDir("imp_cu_imgproc"), "stereo_rectifier");
+  const std::string calib_file =
+      ze::joinPath(test_folder, "stereo_parameters.yaml");
+
+  ze::CameraRig::Ptr rig = ze::cameraRigFromYaml(calib_file);
+  VLOG(2) << "loaded camera rig from yaml file " << calib_file;
+
+  // use 3x3 submatrix returned in left_P, right_P
+  Vector4 left_intrinsics;
+  left_intrinsics << 972.2670826175212, 972.2670826175212, 654.0978851318359, 482.1916770935059;
+
+  Vector4 original_left_intrinsics =
+      rig->at(0).projectionParameters();
+  Vector4 left_distortion =
+      rig->at(0).distortionParameters();
+
+  Matrix3 left_H;
+  left_H << 0.9999716948470486, 0.002684020348481424, -0.007028907417937792,
+      -0.002697713727894102, 0.9999944805267602, -0.001939395951741348,
+      0.007023663243873155, 0.00195830303687577, 0.9999734162485783;
+
+  const std::string left_img_path = ze::joinPath(test_folder, "left01.png");
+  ImageCv32fC1::Ptr cv_left_img;
+  cvBridgeLoad(cv_left_img, left_img_path, PixelOrder::gray);
+  VLOG(2) << "loaded image " << left_img_path
+          << ", size " << cv_left_img->size();
+
+  Matrix3 left_H_inv = left_H.inverse();
+
+  // Allocate rectifier
+  cu::RadTanStereoRectifier32fC1 left_rectifier(
+        cv_left_img->size(), original_left_intrinsics, left_intrinsics,
+        left_distortion, left_H_inv);
+
+  ImageCv32fC2 left_map(left_rectifier.getUndistortRectifyMap());
+  CHECK_EQ(cv_left_img->size(), left_map.size());
+
+  // Test against ground-truth maps
+  const std::string gt_left_map_x_path =
+      joinPath(test_folder, "map_x_left.bin");
+  const std::string gt_left_map_y_path =
+      joinPath(test_folder, "map_y_left.bin");
+  testRectificationMapAgainstFile(left_map,
+                                  gt_left_map_x_path,
+                                  gt_left_map_y_path,
+                                  c_map_tolearance);
+}
+
+TEST(impCuStereoRectifierTexture, horizontalStereoPairRadTan32fC1)
+{
+  constexpr float c_map_tolearance{1.0f};
+  const std::string test_folder =
+      ze::joinPath(ze::getTestDataDir("imp_cu_imgproc"), "stereo_rectifier");
+  const std::string calib_file =
+      ze::joinPath(test_folder, "stereo_parameters.yaml");
+
+  ze::CameraRig::Ptr rig = ze::cameraRigFromYaml(calib_file);
+  VLOG(2) << "loaded camera rig from yaml file " << calib_file;
+
+  Vector4 left_cam_params =
+      rig->at(0).projectionParameters();
+  Vector4 left_distortion =
+      rig->at(0).distortionParameters();
+
+  const std::string left_img_path = ze::joinPath(test_folder, "left01.png");
+  ImageCv32fC1::Ptr cv_left_img;
+  cvBridgeLoad(cv_left_img, left_img_path, PixelOrder::gray);
+  VLOG(2) << "loaded image " << left_img_path
+          << ", size " << cv_left_img->size();
+
+  Vector4 right_cam_params =
+      rig->at(1).projectionParameters();
+  Vector4 right_distortion =
+      rig->at(1).distortionParameters();
+
+  const std::string right_img_path = ze::joinPath(test_folder, "right01.png");
+  ImageCv32fC1::Ptr cv_right_img;
+  cvBridgeLoad(cv_right_img, right_img_path, PixelOrder::gray);
+  VLOG(2) << "loaded image " << right_img_path
+          << ", size " << cv_right_img->size();
+
+  ze::Transformation T_C0_B = rig->T_C_B(0);
+  ze::Transformation T_C1_B = rig->T_C_B(1);
+  ze::Transformation T_C0_C1 = T_C0_B * T_C1_B.inverse();
+
+  VLOG(2) << "Stereo extrinsics (T_C0_C1):\n" << T_C0_C1;
+  VLOG(2) << "Stereo extrinsics (T_C1_C0):\n" << T_C0_C1.inverse();
+
+  // Allocate rectifier
+  Vector4 transformed_left_cam_params;
+  Vector4 transformed_right_cam_params;
+  real_t horizontal_offset;
+  cu::HorizontalStereoPairRectifierRadTan32fC1 rectifier(
+        transformed_left_cam_params,
+        transformed_right_cam_params,
+        horizontal_offset,
+        cv_left_img->size(),
+        left_cam_params,
+        left_distortion,
+        right_cam_params,
+        right_distortion,
+        T_C0_C1);
+
+  // Download maps from GPU
+  ImageCv32fC2 left_map(rectifier.getUndistortRectifyMap(0));
+  CHECK_EQ(cv_left_img->size(), left_map.size());
+  ImageCv32fC2 right_map(rectifier.getUndistortRectifyMap(1));
+  CHECK_EQ(cv_right_img->size(), right_map.size());
+
+  // Test against ground-truth maps
+  const std::string gt_left_map_x_path =
+      joinPath(test_folder, "map_x_left.bin");
+  const std::string gt_left_map_y_path =
+      joinPath(test_folder, "map_y_left.bin");
+  const std::string gt_right_map_x_path =
+      joinPath(test_folder, "map_x_right.bin");
+  const std::string gt_right_map_y_path =
+      joinPath(test_folder, "map_y_right.bin");
+
+  testRectificationMapAgainstFile(left_map,
+                                  gt_left_map_x_path,
+                                  gt_left_map_y_path,
+                                  c_map_tolearance);
+  testRectificationMapAgainstFile(right_map,
+                                  gt_right_map_x_path,
+                                  gt_right_map_y_path,
+                                  c_map_tolearance);
+}
+
+ZE_UNITTEST_ENTRYPOINT
diff --git a/RWR/src/ze_oss/imp_cu_imgproc/test/test_cu_str_tex_decomp.cpp b/RWR/src/ze_oss/imp_cu_imgproc/test/test_cu_str_tex_decomp.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..534a2123752d278b3be362fcf4ec65cc48ca4421
--- /dev/null
+++ b/RWR/src/ze_oss/imp_cu_imgproc/test/test_cu_str_tex_decomp.cpp
@@ -0,0 +1,108 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#include <sstream>
+#include <string>
+
+#include <ze/common/test_entrypoint.hpp>
+#include <ze/common/test_utils.hpp>
+#include <ze/common/benchmark.hpp>
+#include <ze/common/types.hpp>
+
+#include <imp/cu_core/cu_image_gpu.cuh>
+#include <imp/cu_core/cu_math.cuh>
+#include <imp/bridge/opencv/cu_cv_bridge.hpp>
+#include <imp/cu_imgproc/str_tex_decomposer.hpp>
+
+DEFINE_bool(visualize, false, "Show input images and results");
+
+template <typename Pixel>
+class CuStrTexDenoiseTestFixture : public ::testing::Test
+{
+protected:
+  CuStrTexDenoiseTestFixture()
+    : data_path_(ze::getTestDataDir("computer_vision_images"))
+  {
+  }
+
+  void loadLenaGrayscale()
+  {
+    ze::cu::cvBridgeLoad(
+          in_, data_path_ + "/lena_grayscale_511x512.png", ze::PixelOrder::gray);
+  }
+
+  void strTexDecompose()
+  {
+    tex_ = std::make_shared<ze::cu::ImageGpu<Pixel>>(in_->size());
+    str_ = std::make_shared<ze::cu::ImageGpu<Pixel>>(in_->size());
+    str_tex_decomp_ = std::make_shared<ze::cu::StrTexDecomposer<Pixel>>();
+    str_tex_decomp_->solve(tex_, in_, str_);
+
+    if (VLOG_IS_ON(2))
+    {
+      Pixel min, max;
+      ze::cu::minMax(*tex_, min, max);
+      VLOG(2) << "tex_ val range: [" << min << ", " << max << "]";
+      ze::cu::minMax(*str_, min, max);
+      VLOG(2) << "str_ val range: [" << min << ", " << max << "]";
+    }
+  }
+
+protected:
+  std::string data_path_;
+  ze::cu::StrTexDecomposerPtr<Pixel> str_tex_decomp_;
+  ze::cu::ImageGpuPtr<Pixel> str_, tex_, in_;
+};
+
+
+// The list of types we want to test.
+typedef testing::Types<
+ze::Pixel8uC1, ze::Pixel32fC1
+> PixelTypes;
+
+TYPED_TEST_CASE(CuStrTexDenoiseTestFixture, PixelTypes);
+
+TYPED_TEST(CuStrTexDenoiseTestFixture, testVisStrTexDecomp)
+{
+  using namespace ze::cu;
+
+  this->loadLenaGrayscale();
+  this->strTexDecompose();
+
+  if(FLAGS_visualize)
+  {
+    cvBridgeShow("Lena", *this->in_);
+    std::stringstream windowname;
+    windowname << "Texture part of Lena ("
+               << static_cast<int>(this->in_->bitDepth()) << "-bit)";
+    cvBridgeShow(windowname.str(), *this->tex_);
+    windowname.str(std::string());
+    windowname << "Structure part of Lena ("
+               << static_cast<int>(this->in_->bitDepth()) << "-bit)";
+    cvBridgeShow(windowname.str(), *this->str_);
+    cv::waitKey();
+  }
+}
+
+ZE_UNITTEST_ENTRYPOINT
diff --git a/RWR/src/ze_oss/imp_cu_imgproc/test/test_cu_undistortion.cpp b/RWR/src/ze_oss/imp_cu_imgproc/test/test_cu_undistortion.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..aa1eee262b7ae3bbb95a8be701e4cf8e831ec066
--- /dev/null
+++ b/RWR/src/ze_oss/imp_cu_imgproc/test/test_cu_undistortion.cpp
@@ -0,0 +1,189 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#include <imp/bridge/opencv/cv_bridge.hpp>
+#include <imp/cu_imgproc/cu_undistortion.cuh>
+
+#include <ze/cameras/camera_rig.hpp>
+#include <ze/common/benchmark.hpp>
+#include <ze/common/file_utils.hpp>
+#include <ze/common/test_entrypoint.hpp>
+#include <ze/common/test_utils.hpp>
+
+using namespace ze;
+
+TEST(impCuUndistortionTexture, radTan32fC1_zeroDistortion)
+{
+  const std::string test_data_name{"imp_cu_imgproc"};
+  const std::string predefined_img_data_file_name{"pyr_0.png"};
+
+  std::string path(
+        joinPath(
+          getTestDataDir(test_data_name),
+          predefined_img_data_file_name));
+
+  ImageCv32fC1::Ptr cv_img;
+  cvBridgeLoad(cv_img, path, PixelOrder::gray);
+  VLOG(2) << "loaded image " << path
+          << ", size " << cv_img->size();
+
+  VectorX cam_params(4);
+  cam_params << 471.690643292, 471.765601046, 371.087464172, 228.63874151;
+  VectorX dist_coeffs(4);
+  dist_coeffs << 0.0, 0.0, 0.0, 0.0;
+
+  cu::ImageGpu32fC1 gpu_src(*cv_img);
+  cu::ImageGpu32fC1 gpu_dst(cv_img->size());
+
+  cu::RadTanUndistort32fC1 undistorter(
+        gpu_src.size(), cam_params, dist_coeffs);
+  undistorter.undistort(gpu_dst, gpu_src);  // GPU warm-up
+  auto undistortLambda = [&](){
+    undistorter.undistort(gpu_dst, gpu_src);
+  };
+  runTimingBenchmark(
+        undistortLambda, 10, 20,
+        "CUDA undistortion using Textures", true);
+
+  ImageCv32fC1 cv_img_out(gpu_dst);
+  for (uint32_t y = 0; y < cv_img_out.height(); ++y)
+  {
+    for (uint32_t x = 0; x < cv_img_out.width(); ++x)
+    {
+      EXPECT_FLOAT_EQ(cv_img->cvMat().at<float>(y, x),
+                      cv_img_out.cvMat().at<float>(y, x));
+    }
+  }
+}
+
+TEST(impCuUndistortionTexture, equidist32fC1_testMap)
+{
+  const std::string test_data_name{"imp_cu_imgproc"};
+  const std::string predefined_img_data_file_name{"pyr_0.png"};
+
+  std::string path(
+        joinPath(
+          getTestDataDir(test_data_name),
+          predefined_img_data_file_name));
+
+  ImageCv32fC1::Ptr cv_img;
+  cvBridgeLoad(cv_img, path, PixelOrder::gray);
+  VLOG(2) << "loaded image " << path
+          << ", size " << cv_img->size();
+
+  VectorX cam_params(4);
+  cam_params << 471.690643292, 471.765601046, 371.087464172, 228.63874151;
+  VectorX dist_coeffs(4);
+  dist_coeffs << 0.00676530475436, -0.000811126898338, 0.0166458761987, -0.0172655346139;
+
+  cu::EquidistUndistort32fC1 undistorter(
+        cv_img->size(), cam_params, dist_coeffs);
+
+  // Copute map on CPU
+  Eigen::VectorXf cp_flt = cam_params.cast<float>();
+  Eigen::VectorXf dist_flt = dist_coeffs.cast<float>();
+  ImageCv32fC2 cv_computed_map(undistorter.getUndistortionMap());
+  for (uint32_t y = 0; y < cv_computed_map.height(); ++y)
+  {
+    for (uint32_t x = 0; x < cv_computed_map.width(); ++x)
+    {
+      float px[2];
+      px[0] = x;
+      px[1] = y;
+      PinholeGeometry::backProject(cp_flt.data(), px);
+      EquidistantDistortion::distort(dist_flt.data(), px);
+      PinholeGeometry::project(cp_flt.data(), px);
+      EXPECT_NEAR(px[0], cv_computed_map(x, y)[0], 0.0005);
+    }
+  }
+}
+
+TEST(impCuUndistortionTexture, equidist32fC1)
+{
+  const double tolerance_on_sad{0.0014};   // Tolerance in SAD/nelems
+  const double tolerance_on_sad_max{0.5};  // Tolerance on max of SAD
+  const std::string test_folder =
+      ze::getTestDataDir("imp_cu_imgproc");
+  const std::string calib_file =
+      ze::joinPath(test_folder, "visensor_22030_swe_params.yaml");
+
+  ze::CameraRig::Ptr rig = ze::cameraRigFromYaml(calib_file);
+  VLOG(2) << "loaded camera rig from yaml file " << calib_file;
+
+  for (int i = 0; i < 12; ++i)
+  {
+    for (int lr = 0; lr < 2; ++lr)
+    {
+      std::stringstream file_suffix;
+      file_suffix << lr << "_" << i << ".png";
+      const std::string dist_img_file =
+          ze::joinPath(test_folder, "distorted" + file_suffix.str());
+      const std::string undist_img_file =
+          ze::joinPath(test_folder, "undistorted" + file_suffix.str());
+      VLOG(2) << "undistorted GT file: " << undist_img_file;
+      VLOG(2) << "distorted file: " << dist_img_file;
+      // Load test image
+      ImageCv32fC1::Ptr cv_img;
+      cvBridgeLoad(cv_img, dist_img_file, PixelOrder::gray);
+      VLOG(2) << "loaded image " << dist_img_file
+              << ", size " << cv_img->size();
+      // Allocate GPU memory
+      cu::ImageGpu32fC1 gpu_src(*cv_img);
+      cu::ImageGpu32fC1 gpu_dst(cv_img->size());
+
+      // Camera parameters
+      VectorX cam_params = rig->at(lr).projectionParameters();
+      VectorX dist_coeffs = rig->at(lr).distortionParameters();
+
+      cu::EquidistUndistort32fC1 undistorter(
+            gpu_src.size(), cam_params, dist_coeffs);
+      undistorter.undistort(gpu_dst, gpu_src);  // GPU warm-up
+      auto undistortLambda = [&](){
+        undistorter.undistort(gpu_dst, gpu_src);
+      };
+      runTimingBenchmark(
+            undistortLambda, 10, 20,
+            "CUDA undistortion using Textures", true);
+
+      // Download result image
+      ImageCv32fC1 cv_img_out(gpu_dst);
+
+      // Load GT undistorted image
+      cvBridgeLoad(cv_img, undist_img_file, PixelOrder::gray);
+
+      // Compare
+      cv::Mat abs_diff = cv::abs(cv_img->cvMat() - cv_img_out.cvMat());
+      double ad_min, ad_max;
+
+      cv::minMaxLoc(abs_diff, &ad_min, &ad_max);
+      EXPECT_LT(ad_max, tolerance_on_sad_max);
+
+      double sad = cv::sum(abs_diff)[0];
+      EXPECT_LT(sad/static_cast<double>(cv_img->numel()), tolerance_on_sad)
+          << " - testing image " << dist_img_file ;
+    }
+  }
+}
+
+ZE_UNITTEST_ENTRYPOINT
diff --git a/RWR/src/ze_oss/imp_ros_rof_denoising/CATKIN_IGNORE b/RWR/src/ze_oss/imp_ros_rof_denoising/CATKIN_IGNORE
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/RWR/src/ze_oss/imp_ros_rof_denoising/CMakeLists.txt b/RWR/src/ze_oss/imp_ros_rof_denoising/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..390a9a568c6a3bf17ad13f61b70cdc96885c468e
--- /dev/null
+++ b/RWR/src/ze_oss/imp_ros_rof_denoising/CMakeLists.txt
@@ -0,0 +1,19 @@
+project(imp_ros_rof_denoising)
+cmake_minimum_required(VERSION 2.8.0)
+
+cmake_policy(SET CMP0002 OLD)
+
+find_package(catkin_simple REQUIRED)
+catkin_simple(ALL_DEPS_REQUIRED)
+
+include(ze_setup)
+include(ze_macros_cuda)
+find_cuda()
+
+## binaries
+cs_add_executable(rof_node src/rof_node.cpp)
+target_link_libraries(rof_node  ${CUDA_LIBRARIES})
+
+## exports
+cs_install()
+cs_export()
diff --git a/RWR/src/ze_oss/imp_ros_rof_denoising/cfg/RofNode.cfg b/RWR/src/ze_oss/imp_ros_rof_denoising/cfg/RofNode.cfg
new file mode 100755
index 0000000000000000000000000000000000000000..2349b0750d0d03d6af3216c2e1377cd585935caa
--- /dev/null
+++ b/RWR/src/ze_oss/imp_ros_rof_denoising/cfg/RofNode.cfg
@@ -0,0 +1,9 @@
+#!/usr/bin/env python
+PACKAGE = "imp_ros_denoising"
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+
+gen.add("lambda", double_t, 0, "Tradeoff between regularization and data term.", 10.0, 0, 200)
+
+exit(gen.generate(PACKAGE, "imp_ros_denoising", "RofNode"))
diff --git a/RWR/src/ze_oss/imp_ros_rof_denoising/package.xml b/RWR/src/ze_oss/imp_ros_rof_denoising/package.xml
new file mode 100644
index 0000000000000000000000000000000000000000..195474ee018b7d87dc94a08aefcbc3257d1cc178
--- /dev/null
+++ b/RWR/src/ze_oss/imp_ros_rof_denoising/package.xml
@@ -0,0 +1,29 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>imp_ros_rof_denoising</name>
+  <description>
+    ROS nodes for denoising functionalities
+  </description>
+  <version>0.1.4</version>
+  <license>ZE</license>
+
+  <maintainer email="code@werlberger.org">Manuel Werlberger</maintainer>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <buildtool_depend>catkin_simple</buildtool_depend>
+
+  <depend>roscpp</depend>
+  <depend>std_msgs</depend>
+  <depend>image_transport</depend>
+  <depend>rospy</depend>
+  <depend>dynamic_reconfigure</depend>
+
+  <depend>cv_bridge</depend>
+  <depend>ze_cmake</depend>
+  <depend>imp_core</depend>
+  <depend>imp_cu_core</depend>
+  <depend>imp_cu_imgproc</depend>
+  <depend>imp_bridge_opencv</depend>
+
+  <test_depend>gtest</test_depend>
+</package>
diff --git a/RWR/src/ze_oss/imp_ros_rof_denoising/src/rof_node.cpp b/RWR/src/ze_oss/imp_ros_rof_denoising/src/rof_node.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..4df7cf1dd401766d2fc24072d609f4b4034f6658
--- /dev/null
+++ b/RWR/src/ze_oss/imp_ros_rof_denoising/src/rof_node.cpp
@@ -0,0 +1,134 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#include <ros/ros.h>
+#include <image_transport/image_transport.h>
+
+#include <dynamic_reconfigure/server.h>
+#include <imp_ros_rof_denoising/RofNodeConfig.h>
+
+#include <opencv2/highgui/highgui.hpp>
+#include <cv_bridge/cv_bridge.h>
+
+#include <imp/bridge/opencv/image_cv.hpp>
+#include <imp/cu_imgproc/cu_rof_denoising.cuh>
+#include <imp/bridge/opencv/cu_cv_bridge.hpp>
+#include <imp/cu_core/cu_utils.hpp>
+
+#include <sensor_msgs/Image.h>
+
+
+namespace ze {
+
+//------------------------------------------------------------------------------
+class RofNode
+{
+public:
+  RofNode() = default;
+  ~RofNode() = default;
+
+  void imgCb(const sensor_msgs::ImageConstPtr& img_msg);
+
+  void paramCb(imp_ros_denoising::RofNodeConfig& config, uint32_t level);
+
+
+private:
+  ze::cu::RofDenoising8uC1::Ptr rof_;
+  ze::ImageCv8uC1::Ptr cv_img_;
+  ze::ImageCv8uC1::Ptr cv_denoised_;
+  ze::cu::ImageGpu8uC1::Ptr img_;
+  ze::cu::ImageGpu8uC1::Ptr denoised_;
+};
+
+//------------------------------------------------------------------------------
+void RofNode::imgCb(const sensor_msgs::ImageConstPtr &img_msg)
+{
+  cv::Mat mat;
+  try
+  {
+    mat = cv_bridge::toCvShare(img_msg, "mono8")->image;
+  }
+  catch (std::exception& e)
+  {
+    ROS_ERROR("Could not extract image from input message. Exception: %s", e.what());
+    return;
+  }
+
+  ze::Size2u im_size((std::uint32_t)mat.cols, (std::uint32_t)mat.rows);
+
+  if (!rof_ || !cv_img_ || !img_ || im_size != cv_img_->size())
+  {
+    rof_.reset(new ze::cu::RofDenoising8uC1());
+//    rof_->init(im_size);
+
+    cv_img_.reset(new ze::ImageCv8uC1(im_size));
+    cv_denoised_.reset(new ze::ImageCv8uC1(im_size));
+    img_.reset(new ze::cu::ImageGpu8uC1(im_size));
+    denoised_.reset(new ze::cu::ImageGpu8uC1(im_size));
+  }
+
+  cv_img_->cvMat() = mat;
+  img_->copyFrom(*cv_img_);
+  rof_->denoise(denoised_, img_);
+  denoised_->copyTo(*cv_denoised_);
+  cv::imshow("input", cv_img_->cvMat());
+  cv::imshow("denoised", cv_denoised_->cvMat());
+  cv::waitKey(1);
+}
+
+//------------------------------------------------------------------------------
+void RofNode::paramCb(imp_ros_denoising::RofNodeConfig& config, uint32_t level)
+{
+  if(!rof_)
+    return;
+  rof_->params().lambda = ze::cu::max(1e-6, config.lambda);
+}
+
+
+}
+
+//==============================================================================
+int main(int argc, char **argv)
+{
+  ros::init(argc, argv, "RofNode");
+
+
+  ros::NodeHandle nh;
+  ROS_INFO("testing the RofNode");
+  ze::RofNode rof_node;
+
+  // reconfigure stuff
+  dynamic_reconfigure::Server<imp_ros_denoising::RofNodeConfig> server;
+  dynamic_reconfigure::Server<imp_ros_denoising::RofNodeConfig>::CallbackType f;
+  f = boost::bind(&ze::RofNode::paramCb, &rof_node, _1, _2);
+  server.setCallback(f);
+
+
+  image_transport::ImageTransport it(nh);
+  image_transport::Subscriber sub = it.subscribe(
+        "camera/image_raw", 1, &ze::RofNode::imgCb, &rof_node);
+
+  ros::spin();
+  return EXIT_SUCCESS;
+}
diff --git a/RWR/src/ze_oss/imp_tools_cmd/CATKIN_IGNORE b/RWR/src/ze_oss/imp_tools_cmd/CATKIN_IGNORE
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/RWR/src/ze_oss/imp_tools_cmd/CMakeLists.txt b/RWR/src/ze_oss/imp_tools_cmd/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..b35f956c323d9bf14698ac1fdb593275ad4e1d55
--- /dev/null
+++ b/RWR/src/ze_oss/imp_tools_cmd/CMakeLists.txt
@@ -0,0 +1,46 @@
+project(imp_tools_cmd)
+cmake_minimum_required(VERSION 2.8.0)
+
+if(${CMAKE_MAJOR_VERSION} VERSION_GREATER 3.0)
+  cmake_policy(SET CMP0054 OLD)
+endif(${CMAKE_MAJOR_VERSION} VERSION_GREATER 3.0)
+
+find_package(catkin_simple REQUIRED)
+catkin_simple(ALL_DEPS_REQUIRED)
+
+include(ze_setup)
+include(ze_macros_cuda)
+find_cuda()
+
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-unused-local-typedefs" )
+
+###
+### command line tools
+###
+# find_package( OpenCV REQUIRED core highgui imgproc)
+# include_directories(${OpenCV_INCLUDE_DIRS})
+
+cs_cuda_add_executable(image_filter_median3x3_test image_filter_median3x3_test.cpp)
+# target_link_libraries(image_filter_median3x3_test)
+
+cs_cuda_add_executable(image_filter_gauss_test image_filter_gauss_test.cpp)
+# target_link_libraries(image_filter_gauss_test)
+
+cs_cuda_add_executable(natural_edges_test natural_edges_test.cpp)
+# target_link_libraries(natural_edges_test)
+
+cs_cuda_add_executable(cu_rof_denoising_test cu_rof_denoising_test.cpp)
+# target_link_libraries(cu_rof_denoising_test)
+
+cs_cuda_add_executable(cu_tvl1_denoising_test cu_tvl1_denoising_test.cpp)
+# target_link_libraries(cu_tvl1_denoising_test)
+
+cuda_add_executable(cu_min_max_test cu_min_max_test.cpp)
+target_link_libraries(cu_min_max_test ${CUDA_LIBRARIES} ${catkin_LIBRARIES})
+
+
+#cs_add_executable(example_executable src/main.cc)
+#target_link_libraries(example_executable example_library)
+
+cs_install()
+cs_export()
diff --git a/RWR/src/ze_oss/imp_tools_cmd/cu_min_max_test.cpp b/RWR/src/ze_oss/imp_tools_cmd/cu_min_max_test.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f80919576775622b9a5be9c76f754cd65fc00b0d
--- /dev/null
+++ b/RWR/src/ze_oss/imp_tools_cmd/cu_min_max_test.cpp
@@ -0,0 +1,70 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#include <assert.h>
+#include <cstdint>
+#include <iostream>
+#include <memory>
+
+#include <opencv2/core/core.hpp>
+#include <opencv2/highgui/highgui.hpp>
+
+#include <imp/core/roi.hpp>
+#include <imp/core/image_raw.hpp>
+#include <imp/bridge/opencv/image_cv.hpp>
+#include <imp/cu_core/cu_image_gpu.cuh>
+#include <imp/cu_core/cu_math.cuh>
+#include <imp/bridge/opencv/cu_cv_bridge.hpp>
+
+int main(int argc, char** argv)
+{
+  try
+  {
+    if (argc < 2)
+    {
+      std::cout << "usage: cu_rof_denoising_test input_image_filename";
+      return EXIT_FAILURE;
+    }
+    std::string in_filename(argv[1]);
+
+
+    // get image minmax
+    {
+      std::shared_ptr<ze::cu::ImageGpu8uC1> cu_im;
+      ze::cu::cvBridgeLoad(cu_im, in_filename, ze::PixelOrder::gray);
+      ze::Pixel8uC1 min_pixel, max_pixel;
+      ze::cu::minMax(*cu_im, min_pixel, max_pixel);
+      std::cout << "min: " << (int)min_pixel << ", max: " << (int)max_pixel << std::endl;
+    }
+
+  }
+  catch (std::exception& e)
+  {
+    std::cout << "[exception] " << e.what() << std::endl;
+    cudaDeviceReset();
+    assert(false);
+  }
+  cudaDeviceReset();
+  return EXIT_SUCCESS;
+}
diff --git a/RWR/src/ze_oss/imp_tools_cmd/cu_rof_denoising_test.cpp b/RWR/src/ze_oss/imp_tools_cmd/cu_rof_denoising_test.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..88f5d639e8da9348264866b38d953cadc363cfa2
--- /dev/null
+++ b/RWR/src/ze_oss/imp_tools_cmd/cu_rof_denoising_test.cpp
@@ -0,0 +1,102 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#include <assert.h>
+#include <cstdint>
+#include <iostream>
+#include <memory>
+
+#include <opencv2/core/core.hpp>
+#include <opencv2/highgui/highgui.hpp>
+
+#include <imp/core/roi.hpp>
+#include <imp/core/image_raw.hpp>
+#include <imp/bridge/opencv/image_cv.hpp>
+#include <imp/cu_core/cu_image_gpu.cuh>
+#include <imp/cu_imgproc/cu_rof_denoising.cuh>
+#include <imp/bridge/opencv/cu_cv_bridge.hpp>
+
+int main(int argc, char** argv)
+{
+  try
+  {
+    if (argc < 2)
+    {
+      std::cout << "usage: cu_rof_denoising_test input_image_filename";
+      return EXIT_FAILURE;
+    }
+    std::string in_filename(argv[1]);
+
+
+    // ROF denoising 8uC1
+    {
+      std::shared_ptr<ze::cu::ImageGpu8uC1> cu_im;
+      ze::cu::cvBridgeLoad(cu_im, in_filename, ze::PixelOrder::gray);
+      std::shared_ptr<ze::cu::ImageGpu8uC1> cu_im_denoised(
+            new ze::cu::ImageGpu8uC1(*cu_im));
+
+      ze::cu::RofDenoising8uC1 rof;
+      rof.params().primal_dual_energy_check_iter = 10;
+      rof.params().primal_dual_gap_tolerance = 1e-3;
+
+      std::cout << "\n" << rof << std::endl;
+      rof.denoise(cu_im_denoised, cu_im);
+
+      // show results
+      ze::cu::cvBridgeShow("input 8u", *cu_im);
+      ze::cu::cvBridgeShow("denoised 8u", *cu_im_denoised);
+    }
+
+    std::cout << "-------------------------------------------------------------"
+              << std::endl << std::endl;
+
+    // ROF denoising 32fC1
+    {
+      std::shared_ptr<ze::cu::ImageGpu32fC1> cu_im;
+      ze::cu::cvBridgeLoad(cu_im, in_filename, ze::PixelOrder::gray);
+      std::shared_ptr<ze::cu::ImageGpu32fC1> cu_im_denoised(
+            new ze::cu::ImageGpu32fC1(*cu_im));
+
+      ze::cu::RofDenoising32fC1 rof;
+      rof.params().primal_dual_energy_check_iter = 10;
+      rof.params().primal_dual_gap_tolerance = 1e-3;
+
+      std::cout << "\n" << rof << std::endl;
+      rof.denoise(cu_im_denoised, cu_im);
+
+      ze::cu::cvBridgeShow("input 32f", *cu_im);
+      ze::cu::cvBridgeShow("denoised 32f", *cu_im_denoised);
+    }
+
+    cv::waitKey();
+  }
+  catch (std::exception& e)
+  {
+    std::cout << "[exception] " << e.what() << std::endl;
+    assert(false);
+  }
+
+  return EXIT_SUCCESS;
+
+}
diff --git a/RWR/src/ze_oss/imp_tools_cmd/cu_tvl1_denoising_test.cpp b/RWR/src/ze_oss/imp_tools_cmd/cu_tvl1_denoising_test.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..16b3bdb918c2450681cc6c235eec2d63b26ffecb
--- /dev/null
+++ b/RWR/src/ze_oss/imp_tools_cmd/cu_tvl1_denoising_test.cpp
@@ -0,0 +1,94 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#include <assert.h>
+#include <cstdint>
+#include <iostream>
+#include <memory>
+
+#include <opencv2/core/core.hpp>
+#include <opencv2/highgui/highgui.hpp>
+
+#include <imp/core/roi.hpp>
+#include <imp/core/image_raw.hpp>
+#include <imp/bridge/opencv/image_cv.hpp>
+#include <imp/cu_core/cu_image_gpu.cuh>
+#include <imp/cu_imgproc/cu_tvl1_denoising.cuh>
+#include <imp/bridge/opencv/cu_cv_bridge.hpp>
+
+int main(int argc, char** argv)
+{
+  try
+  {
+    if (argc < 2)
+    {
+      std::cout << "usage: cu_tvl1_denoising_test input_image_filename";
+      return EXIT_FAILURE;
+    }
+    std::string in_filename(argv[1]);
+
+    // 8uC1
+    {
+      std::shared_ptr<ze::cu::ImageGpu8uC1> cu_im;
+      ze::cu::cvBridgeLoad(cu_im, in_filename, ze::PixelOrder::gray);
+      std::shared_ptr<ze::cu::ImageGpu8uC1> cu_im_denoised(
+            new ze::cu::ImageGpu8uC1(*cu_im));
+
+      ze::cu::TvL1Denoising8uC1 tvl1;
+      tvl1.params().lambda = 0.5f;
+      std::cout << "\n" << tvl1 << std::endl << std::endl;
+      tvl1.denoise(cu_im_denoised, cu_im);
+
+      // show results
+      ze::cu::cvBridgeShow("input 8u", *cu_im);
+      ze::cu::cvBridgeShow("denoised 8u", *cu_im_denoised);
+    }
+
+    // 32fC1
+    {
+      std::shared_ptr<ze::cu::ImageGpu32fC1> cu_im;
+      ze::cu::cvBridgeLoad(cu_im, in_filename, ze::PixelOrder::gray);
+      std::shared_ptr<ze::cu::ImageGpu32fC1> cu_im_denoised(
+            new ze::cu::ImageGpu32fC1(*cu_im));
+
+      ze::cu::TvL1Denoising32fC1 tvl1;
+      tvl1.params().lambda = 0.5f;
+      std::cout << "\n" << tvl1 << std::endl << std::endl;
+      tvl1.denoise(cu_im_denoised, cu_im);
+
+      ze::cu::cvBridgeShow("input 32f", *cu_im);
+      ze::cu::cvBridgeShow("denoised 32f", *cu_im_denoised);
+    }
+
+    cv::waitKey();
+  }
+  catch (std::exception& e)
+  {
+    std::cout << "[exception] " << e.what() << std::endl;
+    assert(false);
+  }
+
+  return EXIT_SUCCESS;
+
+}
diff --git a/RWR/src/ze_oss/imp_tools_cmd/image_filter_gauss_test.cpp b/RWR/src/ze_oss/imp_tools_cmd/image_filter_gauss_test.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..bf56fe4a10a2ecea487e4cc74a4baaaea3bfae1f
--- /dev/null
+++ b/RWR/src/ze_oss/imp_tools_cmd/image_filter_gauss_test.cpp
@@ -0,0 +1,120 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#include <assert.h>
+#include <cstdint>
+#include <iostream>
+#include <memory>
+
+#include <opencv2/core/core.hpp>
+#include <opencv2/highgui/highgui.hpp>
+
+#include <imp/core/roi.hpp>
+#include <imp/core/image_raw.hpp>
+#include <imp/bridge/opencv/image_cv.hpp>
+#include <imp/cu_core/cu_image_gpu.cuh>
+//#include <imp/cu_core/cu_texture.cuh>
+#include <imp/cu_imgproc/cu_image_filter.cuh>
+//#include <imp/bridge/opencv/cu_cv_bridge.hpp>
+
+int main(int argc, char** argv)
+{
+  try
+  {
+    if (argc < 2)
+    {
+      std::cout << "usage: " << argv[0] << " input_image_filename";
+      return EXIT_FAILURE;
+    }
+    std::string in_filename(argv[1]);
+
+    ze::ImageCv8uC1 h1_lena_8uC1(cv::imread(in_filename, CV_LOAD_IMAGE_GRAYSCALE), ze::PixelOrder::gray);
+
+    {
+      // copy host->device
+      std::unique_ptr<ze::cu::ImageGpu8uC1> d_lena_8uC1(
+            new ze::cu::ImageGpu8uC1(h1_lena_8uC1));
+
+      std::unique_ptr<ze::cu::ImageGpu8uC1> d_gauss_lena_8uC1(
+            new ze::cu::ImageGpu8uC1(d_lena_8uC1->size()));
+
+      ze::cu::filterGauss(*d_gauss_lena_8uC1, *d_lena_8uC1, 10.0);
+
+
+      ze::ImageCv8uC1 h_gauss_lena_8uC1(*d_gauss_lena_8uC1);
+      cv::imshow("lena 8u", h1_lena_8uC1.cvMat());
+      cv::imshow("lena gauss 8u", h_gauss_lena_8uC1.cvMat());
+
+    }
+
+    {
+      // 32fC1 test
+      ze::ImageCv32fC1 h1_lena_32fC1(h1_lena_8uC1.size());
+
+      h1_lena_8uC1.cvMat().convertTo(h1_lena_32fC1.cvMat(), CV_32F);
+      h1_lena_32fC1.cvMat() /= 255.f;
+
+      // copy host->device
+      std::unique_ptr<ze::cu::ImageGpu32fC1> d_lena_32fC1(
+            new ze::cu::ImageGpu32fC1(h1_lena_32fC1));
+
+      std::unique_ptr<ze::cu::ImageGpu32fC1> d_gauss_lena_32fC1(
+            new ze::cu::ImageGpu32fC1(d_lena_32fC1->size()));
+
+      ze::cu::filterGauss(*d_gauss_lena_32fC1, *d_lena_32fC1, 10.0);
+
+
+      ze::ImageCv32fC1 h_gauss_lena_32fC1(*d_gauss_lena_32fC1);
+      cv::imshow("lena 32f", h1_lena_32fC1.cvMat());
+      cv::imshow("lena gauss 32f", h_gauss_lena_32fC1.cvMat());
+
+    }
+
+//    // 32fC1 texture
+//    {
+//      std::shared_ptr<ze::cu::ImageGpu32fC1> d_lena;
+//      ze::cu::cvBridgeLoad(d_lena, "/home/mwerlberger/data/std/Lena.tiff",
+//                            ze::PixelOrder::gray);
+//      std::shared_ptr<ze::cu::ImageGpu32fC1> d_lena_denoised(
+//            new ze::cu::ImageGpu32fC1(d_lena->size()));
+
+//      std::shared_ptr<ze::cu::Texture2D> d_tex_lena =
+//          d_lena->genTexture(false, cudaFilterModeLinear);
+
+//      ze::cu::filterGauss(*d_lena_denoised, *d_tex_lena, 10.0);
+
+//      ze::cu::cvBridgeShow("lena input 32f texture", *d_lena);
+//      ze::cu::cvBridgeShow("lena denoised 32f texture", *d_lena_denoised);
+//    }
+    cv::waitKey();
+  }
+  catch (std::exception& e)
+  {
+    std::cout << "[exception] " << e.what() << std::endl;
+    assert(false);
+  }
+
+  return EXIT_SUCCESS;
+
+}
diff --git a/RWR/src/ze_oss/imp_tools_cmd/image_filter_median3x3_test.cpp b/RWR/src/ze_oss/imp_tools_cmd/image_filter_median3x3_test.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..83cf8975039458c2897d71789fcc34b08c24ed52
--- /dev/null
+++ b/RWR/src/ze_oss/imp_tools_cmd/image_filter_median3x3_test.cpp
@@ -0,0 +1,94 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#include <assert.h>
+#include <cstdint>
+#include <iostream>
+#include <memory>
+
+#include <opencv2/core/core.hpp>
+#include <opencv2/highgui/highgui.hpp>
+
+#include <imp/core/roi.hpp>
+#include <imp/core/image_raw.hpp>
+#include <imp/bridge/opencv/image_cv.hpp>
+#include <imp/cu_core/cu_image_gpu.cuh>
+#include <imp/cu_imgproc/cu_image_filter.cuh>
+#include <imp/bridge/opencv/cu_cv_bridge.hpp>
+
+
+void addImpulseNoise(cv::Mat& img, double perc)
+{
+  const int rows = img.rows;
+  const int cols = img.cols;
+  const int channels = img.channels();
+  int num_corrupted_pts = static_cast<int>((rows*cols*channels)*perc/100.0);
+
+  for (int i=0; i<num_corrupted_pts; ++i)
+  {
+    int r = rand() % rows;
+    int c = rand() % cols;
+    int channel = rand() % channels;
+
+    uchar* pixel = img.ptr<uchar>(r) + (c*channels) + channel;
+    *pixel = (rand()%2) ? 255 : 0;
+  }
+}
+
+int main(int /*argc*/, char** /*argv*/)
+{
+  try
+  {
+    ze::ImageCv8uC1 h1_lena_8uC1(cv::imread("/home/mwerlberger/data/std/Lena.tiff",
+                                             CV_LOAD_IMAGE_GRAYSCALE),
+                                  ze::PixelOrder::gray);
+
+    // add salt and pepper noise
+    addImpulseNoise(h1_lena_8uC1.cvMat(), 20);
+
+    {
+      // copy host->device
+      std::unique_ptr<ze::cu::ImageGpu8uC1> d_lena_8uC1(
+            new ze::cu::ImageGpu8uC1(h1_lena_8uC1));
+
+      std::unique_ptr<ze::cu::ImageGpu8uC1> d_median_lena_8uC1(
+            new ze::cu::ImageGpu8uC1(d_lena_8uC1->size()));
+
+      ze::cu::filterMedian3x3(*d_median_lena_8uC1, *d_lena_8uC1);
+
+      ze::cu::cvBridgeShow("lena 8u", *d_lena_8uC1);
+      ze::cu::cvBridgeShow("lena median 8u", *d_median_lena_8uC1);
+
+      cv::waitKey();
+    }
+  }
+  catch (std::exception& e)
+  {
+    std::cout << "[exception] " << e.what() << std::endl;
+    assert(false);
+  }
+
+  return EXIT_SUCCESS;
+
+}
diff --git a/RWR/src/ze_oss/imp_tools_cmd/natural_edges_test.cpp b/RWR/src/ze_oss/imp_tools_cmd/natural_edges_test.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b82534eab8e7e75a86479390d99708c0c1e3bd8c
--- /dev/null
+++ b/RWR/src/ze_oss/imp_tools_cmd/natural_edges_test.cpp
@@ -0,0 +1,75 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#include <assert.h>
+#include <cstdint>
+#include <iostream>
+#include <memory>
+
+#include <opencv2/core/core.hpp>
+#include <opencv2/highgui/highgui.hpp>
+
+#include <imp/core/roi.hpp>
+#include <imp/core/image_raw.hpp>
+#include <imp/bridge/opencv/image_cv.hpp>
+#include <imp/cu_core/cu_utils.hpp>
+#include <imp/cu_core/cu_image_gpu.cuh>
+#include <imp/cu_imgproc/edge_detectors.cuh>
+#include <imp/bridge/opencv/cu_cv_bridge.hpp>
+
+
+int main(int argc, char** argv)
+{
+  try
+  {
+    if (argc < 2)
+    {
+      std::cout << "usage: natural_edges_test input_image_filename";
+      return EXIT_FAILURE;
+    }
+    std::string in_filename(argv[1]);
+
+    std::shared_ptr<ze::cu::ImageGpu32fC1> im;
+    ze::cu::cvBridgeLoad(im, in_filename, ze::PixelOrder::gray);
+
+    std::unique_ptr<ze::cu::ImageGpu32fC1> edges(
+          new ze::cu::ImageGpu32fC1(*im));
+    IMP_CUDA_CHECK();
+
+    ze::cu::naturalEdges(*edges, *im, 1.f, 10.f, 0.7f);
+
+    ze::cu::cvBridgeShow("image", *im);
+    ze::cu::cvBridgeShow("edges", *edges, true);
+
+    cv::waitKey();
+  }
+  catch (std::exception& e)
+  {
+    std::cout << "[exception] " << e.what() << std::endl;
+    assert(false);
+  }
+
+  return EXIT_SUCCESS;
+
+}
diff --git a/RWR/src/ze_oss/imp_tools_cmd/package.xml b/RWR/src/ze_oss/imp_tools_cmd/package.xml
new file mode 100644
index 0000000000000000000000000000000000000000..a9e60399429d69fb70c583863e16c708be27103d
--- /dev/null
+++ b/RWR/src/ze_oss/imp_tools_cmd/package.xml
@@ -0,0 +1,26 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>imp_tools_cmd</name>
+  <description>
+    IMP command line tools for calling specific algorithms mainly for testing purpose.
+  </description>
+  <version>0.1.4</version>
+  <license>ZE</license>
+
+  <maintainer email="code@werlberger.org">Manuel Werlberger</maintainer>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <buildtool_depend>catkin_simple</buildtool_depend>
+
+  <depend>cv_bridge</depend>
+  <depend>glog_catkin</depend>
+  <depend>eigen_catkin</depend>
+  <depend>ze_cmake</depend>
+  <depend>imp_core</depend>
+  <depend>imp_cu_core</depend>
+  <depend>imp_cu_imgproc</depend>
+  <depend>imp_bridge_opencv</depend>
+  <depend>imp_bridge_pangolin</depend>
+
+  <test_depend>gtest</test_depend>
+</package>
diff --git a/RWR/src/ze_oss/ze_cameras/CMakeLists.txt b/RWR/src/ze_oss/ze_cameras/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..2ac8e948975acf220890af2ce83404be4dfb7b00
--- /dev/null
+++ b/RWR/src/ze_oss/ze_cameras/CMakeLists.txt
@@ -0,0 +1,50 @@
+project(ze_cameras)
+cmake_minimum_required(VERSION 2.8.3)
+
+find_package(catkin_simple REQUIRED)
+find_package(OpenCV REQUIRED)
+catkin_simple()
+
+include(ze_setup)
+
+
+#############
+# LIBRARIES #
+#############
+set(HEADERS
+  include/ze/cameras/camera.hpp
+  include/ze/cameras/camera_impl.hpp
+  include/ze/cameras/camera_models.hpp
+  include/ze/cameras/camera_rig.hpp
+  include/ze/cameras/camera_utils.hpp
+  include/ze/cameras/camera_yaml_serialization.hpp
+  )
+
+set(SOURCES
+  src/camera.cpp
+  src/camera_rig.cpp
+  src/camera_utils.cpp
+  src/camera_yaml_serialization.cpp
+  src/camera_impl.cpp
+  )
+
+cs_add_library(${PROJECT_NAME} ${SOURCES} ${HEADERS})
+
+
+##########
+# GTESTS #
+##########
+catkin_add_gtest(test_camera_impl test/test_camera_impl.cpp)
+target_link_libraries(test_camera_impl ${PROJECT_NAME} ${OpenCV_LIBRARIES})
+
+catkin_add_gtest(test_camera_rig test/test_camera_rig.cpp)
+target_link_libraries(test_camera_rig ${PROJECT_NAME} ${OpenCV_LIBRARIES})
+
+catkin_add_gtest(test_camera_utils test/test_camera_utils.cpp)
+target_link_libraries(test_camera_utils ${PROJECT_NAME} ${OpenCV_LIBRARIES})
+
+##########
+# EXPORT #
+##########
+cs_install()
+cs_export()
diff --git a/RWR/src/ze_oss/ze_cameras/include/ze/cameras/camera.hpp b/RWR/src/ze_oss/ze_cameras/include/ze/cameras/camera.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..26c8c7c5686051e290877c194f730afdc7d80fe6
--- /dev/null
+++ b/RWR/src/ze_oss/ze_cameras/include/ze/cameras/camera.hpp
@@ -0,0 +1,187 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#pragma once
+
+#include <string>
+#include <memory>
+#include <ze/common/logging.hpp>
+#pragma diagnostic push
+#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
+// Eigen 3.2.7 uses std::binder1st and std::binder2nd which are deprecated since c++11
+// Fix is in 3.3 devel (http://eigen.tuxfamily.org/bz/show_bug.cgi?id=872).
+#include <Eigen/Core>
+#pragma diagnostic pop
+
+#include <imp/core/image.hpp>
+#include <imp/core/size.hpp>
+#include <ze/common/macros.hpp>
+#include <ze/common/types.hpp>
+
+namespace ze {
+
+enum class CameraType {
+  Pinhole = 0,
+  PinholeFov = 1,
+  PinholeEquidistant = 2,
+  PinholeRadialTangential = 3
+};
+
+inline bool isPinholeType(CameraType type)
+{
+  switch (type)
+  {
+    case CameraType::Pinhole:
+    case CameraType::PinholeFov:
+    case CameraType::PinholeEquidistant:
+    case CameraType::PinholeRadialTangential:
+      return true;
+      break;
+    default:
+      LOG(FATAL) << "Camera type not known.";
+      break;
+  }
+  return false;
+}
+
+//! Wrapper class of various camera projection and distortion models.
+class Camera
+{
+public:
+  ZE_POINTER_TYPEDEFS(Camera);
+
+  Camera() = delete;
+
+  //! This constructor is used by the derived classes. If you want a camera,
+  //! create one of the derived cameras in camera_impl.
+  Camera(const uint32_t width, const uint32_t height, const CameraType type,
+         const VectorX& projection_params, const VectorX& distortion_params);
+
+  virtual ~Camera() = default;
+
+  //! @name: Projection and back-projection operations. The main use of the camera.
+  //! @{
+  //! Bearing vector from pixel coordinates. Z-component of return value is 1.0.
+  virtual Bearing backProject(const Eigen::Ref<const Keypoint>& px) const = 0;
+
+  //! Computes pixel coordinates from 3D-point.
+  virtual Keypoint project(const Eigen::Ref<const Position>& pos) const = 0;
+
+  //! Returns true or false if 3D-point is visible (no occlusion check) and if
+  //! visible also returns pixel coordinates.
+  virtual std::pair<Keypoint, bool> projectWithCheck(
+      const Eigen::Ref<const Position>& pos,
+      real_t border_margin = 0.0) const = 0;
+
+  //! Computes pixel coordinates from 3D-point in homogeneous coordinates.
+  virtual Keypoint projectHomogeneous(const Eigen::Ref<const HomPosition>& pos_h) const;
+
+  //! Returns true or false if homogeneous 3D-point is visible (no occlusion check)
+  //! and if visible also returns the pixel coordinates.
+  virtual std::pair<Keypoint, bool> projectHomogeneousWithCheck(
+      const Eigen::Ref<const HomPosition>& pos_h,
+      real_t border_margin = 0.0) const;
+
+  //! Computes Jacobian of projection w.r.t. bearing vector.
+  virtual Matrix23 dProject_dLandmark(const Eigen::Ref<const Position>& pos) const = 0;
+
+  //! Computes Jacobian of homogeneous projection w.r.t. homogeneous landmark position.
+  virtual Matrix24 dProjectHomogeneous_dLandmark(
+      const Eigen::Ref<const HomPosition>& pos_h) const;
+
+  //! Computes pixel coordinates from 3D point and corresponding Jacobian.
+  virtual std::pair<Keypoint, Matrix23> projectWithJacobian(
+      const Eigen::Ref<const Position>& pos) const = 0;
+
+  //! Computes pixel coordinates from 3D homogeneous point and corresponding Jacobian.
+  virtual std::pair<Keypoint, Matrix24> projectHomogeneousWithJacobian(
+      const Eigen::Ref<const HomPosition>& pos_h) const;
+  //! @}
+
+  //! @name Block projection and back-projection. Always prefer to avoid cache misses.
+  //! @{
+  virtual Bearings backProjectVectorized(const Eigen::Ref<const Keypoints>& px_vec) const;
+  virtual Keypoints projectVectorized(const Eigen::Ref<const Bearings>& bearing_vec) const;
+  virtual Matrix6X dProject_dLandmarkVectorized(const Positions& pos_vec) const;
+  //! @}
+
+  //! @name Image dimension.
+  //! @{
+  inline uint32_t width() const { return size_.width(); }
+  inline uint32_t height() const { return size_.height(); }
+  inline Size2u size() const { return size_; }
+  //! @}
+
+  //! CameraType value representing the camera model used by the derived class.
+  inline CameraType type() const { return type_; }
+
+  //! CameraType as descriptive string.
+  std::string typeAsString() const;
+
+  //! Camera projection parameters.
+  inline const VectorX& projectionParameters() const { return projection_params_; }
+
+  //! Camera distortion parameters.
+  inline const VectorX& distortionParameters() const { return distortion_params_; }
+
+  //! Name of the camera.
+  inline const std::string& label() const { return label_; }
+
+  //! Set user-specific camera label.
+  inline void setLabel(const std::string& label) { label_ = label; }
+
+  //! Get angle corresponding to one pixel in image plane.
+  //! @todo: make static cache.
+  virtual real_t getApproxAnglePerPixel() const = 0;
+
+  virtual real_t getApproxBearingAngleFromPixelDifference(real_t px_diff) const = 0;
+
+  //! Set mask: 0 = masked, >0 = unmasked.
+  void setMask(const Image8uC1::Ptr& mask);
+
+  //! Get mask.
+  inline Image8uC1::ConstPtr mask() const { return mask_; }
+
+protected:
+  Size2u size_;
+
+  //! Camera projection parameters, e.g., (fx, fy, cx, cy).
+  VectorX projection_params_;
+
+  //! Camera distortion parameters, e.g., (k1, k2, r1, r2).
+  VectorX distortion_params_;
+
+  std::string label_;
+  CameraType type_;
+  Image8uC1::Ptr mask_ = nullptr;
+};
+
+//! Load a camera rig form a yaml file. Returns a nullptr if the loading fails.
+Camera::Ptr cameraFromYaml(const std::string& path);
+
+//! Print camera:
+std::ostream& operator<<(std::ostream& out, const Camera& cam);
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_cameras/include/ze/cameras/camera_impl.hpp b/RWR/src/ze_oss/ze_cameras/include/ze/cameras/camera_impl.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..372cd27001a60776241df9d3d224f33357bacaa3
--- /dev/null
+++ b/RWR/src/ze_oss/ze_cameras/include/ze/cameras/camera_impl.hpp
@@ -0,0 +1,188 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#pragma once
+
+#include <ze/cameras/camera.hpp>
+#include <ze/cameras/camera_models.hpp>
+#include <ze/cameras/camera_utils.hpp>
+
+namespace ze {
+
+template<class Distortion>
+class PinholeProjection : public Camera
+{
+public:
+
+  static constexpr DistortionType distortion_type = Distortion::type;
+
+  //! Default constructor.
+  using Camera::Camera;
+
+  virtual ~PinholeProjection() = default;
+
+  virtual Keypoint project(
+      const Eigen::Ref<const Bearing>& bearing) const override
+  {
+    // Unit coordinates -> distortion -> pinhole, offset and scale.
+    Keypoint px = bearing.head<2>() / bearing(2);
+    Distortion::distort(this->distortion_params_.data(), px.data());
+    PinholeGeometry::project(this->projection_params_.data(), px.data());
+    return px;
+  }
+
+  virtual std::pair<Keypoint, bool> projectWithCheck(
+      const Eigen::Ref<const Position>& pos,
+      real_t border_margin) const override
+  {
+    if (pos[2] < 0.0)
+    {
+      return std::make_pair(Keypoint(), false);
+    }
+    Keypoint px = project(pos);
+    return std::make_pair(px, isVisibleWithMargin(size(), px, border_margin));
+  }
+
+  virtual Bearing backProject(
+      const Eigen::Ref<const Keypoint>& px) const override
+  {
+    Bearing bearing;
+    bearing << px(0), px(1), 1.0;
+    PinholeGeometry::backProject(this->projection_params_.data(), bearing.data());
+    Distortion::undistort(this->distortion_params_.data(), bearing.data());
+    return bearing.normalized();
+  }
+
+  virtual Matrix23 dProject_dLandmark(
+      const Eigen::Ref<const Position>& pos) const override
+  {
+    Matrix22 J_dist;
+    real_t z_inv = 1.0 / pos.z();
+    real_t z_inv_sq = z_inv * z_inv;
+    Keypoint px_unitplane = pos.head<2>() * z_inv;
+    Distortion::distort(
+          this->distortion_params_.data(), px_unitplane.data(), J_dist.data());
+    const real_t fx = this->projection_params_[0];
+    const real_t fy = this->projection_params_[1];
+    Matrix23 J;
+    J(0, 0) = fx * J_dist(0, 0) * z_inv;
+    J(0, 1) = fx * J_dist(0, 1) * z_inv;
+    J(0, 2) = -fx * (pos.x() * J_dist(0, 0) + pos.y() * J_dist(0, 1)) * z_inv_sq;
+    J(1, 0) = fy * J_dist(1, 0) * z_inv;
+    J(1, 1) = fy * J_dist(1, 1) * z_inv;
+    J(1, 2) = -fy * (pos.x() * J_dist(1, 0) + pos.y() * J_dist(1, 1)) * z_inv_sq;
+    return J;
+  }
+
+  std::pair<Keypoint, Matrix23> projectWithJacobian(
+        const Eigen::Ref<const Position>& pos) const
+  {
+    //! @todo: project and jacobian computation do many duplicate things.
+    Keypoint px = project(pos);
+    Matrix23 J = dProject_dLandmark(pos);
+    return std::make_pair(px, J);
+  }
+
+  virtual real_t getApproxAnglePerPixel() const override
+  {
+    //! @todo: Is this correct? And if yes, this is costlty to compute often!
+    //!        replace with acos and a dot product between the bearing vectors.
+    // abs() because ICL-NUIM has negative focal length.
+    return std::atan(1.0 / (2.0 * std::abs(this->projection_params_[0])))
+         + std::atan(1.0 / (2.0 * std::abs(this->projection_params_[1])));
+  }
+
+  virtual real_t getApproxBearingAngleFromPixelDifference(real_t px_diff) const override
+  {
+    //! @todo: Is this correct? And if yes, this is costlty to compute often!
+    //!        acos and a dot product between the bearing vectors.
+    // abs() because ICL-NUIM has negative focal length.
+    return std::atan(px_diff / (2.0 * std::abs(this->projection_params_[0])))
+         + std::atan(px_diff / (2.0 * std::abs(this->projection_params_[1])));
+  }
+};
+
+//-----------------------------------------------------------------------------
+// Convenience typedefs.
+// (sync with explicit template class instantiations at the end of the cpp file)
+typedef PinholeProjection<NoDistortion> PinholeCamera;
+typedef PinholeProjection<FovDistortion> FovCamera;
+typedef PinholeProjection<RadialTangentialDistortion> RadTanCamera;
+typedef PinholeProjection<EquidistantDistortion> EquidistantCamera;
+
+//-----------------------------------------------------------------------------
+// Convenience factory functions.
+
+inline PinholeCamera createPinholeCamera(
+    int width, int height, real_t fx, real_t fy, real_t cx, real_t cy)
+{
+  return PinholeCamera(width, height, CameraType::Pinhole,
+                       (Vector4() << fx, fy, cx, cy).finished(), VectorX());
+}
+
+inline FovCamera createFovCamera(
+    int width, int height, real_t fx, real_t fy, real_t cx, real_t cy,
+    real_t s)
+{
+  return FovCamera(width, height, CameraType::PinholeFov,
+                   (Vector4() << fx, fy, cx, cy).finished(),
+                   (Vector1() << s).finished());
+}
+
+inline RadTanCamera createRadTanCamera(
+    int width, int height, real_t fx, real_t fy, real_t cx, real_t cy,
+    real_t k1, real_t k2, real_t r1, real_t r2)
+{
+  return RadTanCamera(width, height, CameraType::PinholeRadialTangential,
+                       (Vector4() << fx, fy, cx, cy).finished(),
+                       (Vector4() << k1, k2, r1, r2).finished());
+}
+
+inline EquidistantCamera createEquidistantCamera(
+    int width, int height, real_t fx, real_t fy, real_t cx, real_t cy,
+    real_t k1, real_t k2, real_t k3, real_t k4)
+{
+  return EquidistantCamera(width, height, CameraType::PinholeEquidistant,
+                           (Vector4() << fx, fy, cx, cy).finished(),
+                           (Vector4() << k1, k2, k3, k4).finished());
+}
+
+inline Camera::Ptr createEquidistantCameraShared(
+    int width, int height, real_t fx, real_t fy, real_t cx, real_t cy,
+    real_t k1, real_t k2, real_t k3, real_t k4)
+{
+  return std::make_shared<EquidistantCamera>(
+        width, height, CameraType::PinholeEquidistant,
+        (Vector4() << fx, fy, cx, cy).finished(),
+        (Vector4() << k1, k2, k3, k4).finished());
+}
+
+//! Returns camera with some reasonable parameters.
+inline PinholeCamera createTestPinholeCamera()
+{
+  return createPinholeCamera(640, 480, 329.11, 329.11, 320.0, 240.0);
+}
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_cameras/include/ze/cameras/camera_models.hpp b/RWR/src/ze_oss/ze_cameras/include/ze/cameras/camera_models.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..de89ac63e8e7295727664cbab4827cad2cc5861d
--- /dev/null
+++ b/RWR/src/ze_oss/ze_cameras/include/ze/cameras/camera_models.hpp
@@ -0,0 +1,408 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#pragma once
+
+#include <iostream>
+#include <cmath>
+
+#ifdef WITH_CUDA
+#  include<cuda_runtime_api.h>
+#  define CUDA_HOST __host__
+#  define CUDA_DEVICE  __device__
+#else
+#  define CUDA_HOST
+#  define CUDA_DEVICE
+#endif
+
+namespace ze {
+
+// Pure static camera projection and distortion models, intended to be used in
+// both GPU and CPU code. Parameter checking should be performed in interface
+// classes.
+
+// Pinhole projection model.
+struct PinholeGeometry
+{
+  template <typename T>
+  CUDA_HOST CUDA_DEVICE
+  static void project(const T* params, T* px)
+  {
+    const T fx = params[0];
+    const T fy = params[1];
+    const T cx = params[2];
+    const T cy = params[3];
+    px[0] = px[0] * fx + cx;
+    px[1] = px[1] * fy + cy;
+  }
+
+  template <typename T>
+  CUDA_HOST CUDA_DEVICE
+  static void backProject(const T* params, T* px)
+  {
+    const T fx = params[0];
+    const T fy = params[1];
+    const T cx = params[2];
+    const T cy = params[3];
+    px[0] = (px[0] - cx) / fx;
+    px[1] = (px[1] - cy) / fy;
+  }
+
+  template <typename T>
+  CUDA_HOST CUDA_DEVICE
+  static void dProject_dBearing(const T* bearing, const T* params, T* H)
+  {
+    const T fx = params[0];
+    const T fy = params[1];
+    const T z_sq = bearing[2] * bearing[2];
+    const T z_inv = 1.0 / bearing[2];
+    H[0] = fx * z_inv;
+    H[1] = 0;
+    H[2] = 0;
+    H[3] = fy * z_inv;
+    H[4] = - fx * bearing[0] / z_sq;
+    H[5] = - fy * bearing[1] / z_sq;
+  }
+};
+
+enum class DistortionType
+{
+  No,
+  Fov,
+  RadTan,
+  Equidistant,
+};
+
+// -----------------------------------------------------------------------------
+// Dummy distortion.
+struct NoDistortion
+{
+  static constexpr DistortionType type = DistortionType::No;
+
+  template <typename T>
+  CUDA_HOST CUDA_DEVICE
+  static void distort(const T* /*params*/, T* /*px*/, T* jac_colmajor = nullptr)
+  {
+    if (jac_colmajor)
+    {
+      T& J_00 = jac_colmajor[0];
+      T& J_10 = jac_colmajor[1];
+      T& J_01 = jac_colmajor[2];
+      T& J_11 = jac_colmajor[3];
+      J_00 = 1.0;
+      J_01 = 0.0;
+      J_10 = 0.0;
+      J_11 = 1.0;
+    }
+  }
+
+  template <typename T>
+  CUDA_HOST CUDA_DEVICE
+  static void undistort(const T* /*params*/, T* /*px*/)
+  {}
+};
+
+// -----------------------------------------------------------------------------
+// This class implements the FOV distortion model of Deverneay and Faugeras,
+// Straight lines have to be straight, 2001. In PTAM this model is called ATAN.
+struct FovDistortion
+{
+  static constexpr DistortionType type = DistortionType::Fov;
+
+  template <typename T>
+  CUDA_HOST CUDA_DEVICE
+  static void distort(const T* params, T* px, T* jac_colmajor = nullptr)
+  {
+    const T x = px[0];
+    const T y = px[1];
+    const T s = params[0];
+    const T tan_s_half_x2 = params[1];
+    const T rad = std::sqrt(x * x + y * y);
+    const T factor = (rad < 0.001) ? 1.0 : std::atan(rad * tan_s_half_x2) / (s * rad);
+    px[0] *= factor;
+    px[1] *= factor;
+
+    if (jac_colmajor)
+    {
+      // no common factors
+      const T xx = x * x;
+      const T yy = y * y;
+      const T rad_sq = xx + yy;
+      T& J_00 = jac_colmajor[0];
+      T& J_10 = jac_colmajor[1];
+      T& J_01 = jac_colmajor[2];
+      T& J_11 = jac_colmajor[3];
+      if(s * s < 1e-5)
+      {
+        // Distortion parameter very small.
+        J_00 = 1.0; J_01 = 0.0;
+        J_10 = 0.0; J_11 = 1.0;
+      }
+      else if(rad_sq < 1e-5)
+      {
+        // Projection very close to image center
+        J_00 = 2.0 * std::tan(s / 2.0) / s;
+        J_11 = J_00;
+        J_01 = 0.0;
+        J_10 = 0.0;
+      }
+      else
+      {
+        // Standard case
+        const T xy = x * y;
+        const T rad = std::sqrt(rad_sq);
+        const T nominator = std::atan(tan_s_half_x2 * rad);
+        const T scale =
+            tan_s_half_x2 / (s * (xx + yy) * (tan_s_half_x2 * tan_s_half_x2 * (xx + yy) + 1.0))
+            - factor * 1.0  / rad_sq;
+        J_00 = xx * scale + factor;
+        J_11 = yy * scale + factor;
+        J_01 = xy * scale;
+        J_10 = J_01;
+      }
+    }
+  }
+
+  template <typename T>
+  CUDA_HOST CUDA_DEVICE
+  static void undistort(const T* params, T* px)
+  {
+    const T s = params[0];
+    const T tan_s_half_x2 = params[1];
+    const T rad = std::sqrt(px[0] * px[0] + px[1] * px[1]);
+    const T factor = (rad < 0.001) ? 1.0 : (std::tan(rad * s) / tan_s_half_x2) / rad;
+    px[0] *= factor;
+    px[1] *= factor;
+  }
+};
+
+// -----------------------------------------------------------------------------
+// This class implements the radial and tangential distortion model used by
+// OpenCV and ROS. Reference:
+// docs.opencv.org/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html
+struct RadialTangentialDistortion
+{
+  static constexpr DistortionType type = DistortionType::RadTan;
+
+  template <typename T>
+  CUDA_HOST CUDA_DEVICE
+  static void distort(const T* params, T* px, T* jac_colmajor = nullptr)
+  {
+    const T x = px[0];
+    const T y = px[1];
+    const T k1 = params[0];
+    const T k2 = params[1];
+    const T p1 = params[2];
+    const T p2 = params[3];
+    const T xx = x * x;
+    const T yy = y * y;
+    const T xy = x * y;
+    const T r2 = xx + yy;
+    const T cdist = (k1 + k2 * r2) * r2;
+    px[0] += px[0] * cdist + p1 * 2.0 * xy + p2 * (r2 + 2.0 * xx);
+    px[1] += px[1] * cdist + p2 * 2.0 * xy + p1 * (r2 + 2.0 * yy);
+
+    if (jac_colmajor)
+    {
+      const T k2_r2_x4 = k2 * r2 * 4.0;
+      const T cdist_p1 = cdist + 1.0;
+      T& J_00 = jac_colmajor[0];
+      T& J_10 = jac_colmajor[1];
+      T& J_01 = jac_colmajor[2];
+      T& J_11 = jac_colmajor[3];
+      J_00 = cdist_p1 + k1 * 2.0 * xx + k2_r2_x4 * xx + 2.0 * p1 * y + 6.0 * p2 * x;
+      J_11 = cdist_p1 + k1 * 2.0 * yy + k2_r2_x4 * yy + 2.0 * p2 * x + 6.0 * p1 * y;
+      J_10 = 2.0 * k1 * xy + k2_r2_x4 * xy + 2.0 * p1 * x + 2.0 * p2 * y;
+      J_01 = J_10;
+    }
+  }
+
+  template <typename T>
+  CUDA_HOST CUDA_DEVICE
+  static void undistort(const T* params, T* px)
+  {
+    T jac_colmajor[4];
+    T x[2];
+    T x_tmp[2];
+    x[0] = px[0]; x[1]= px[1];
+    for(int i = 0; i < 30; ++i)
+    {
+      x_tmp[0] = x[0]; x_tmp[1] = x[1];
+      distort(params, x_tmp, jac_colmajor);
+
+      const T e_u = px[0] - x_tmp[0];
+      const T e_v = px[1] - x_tmp[1];
+
+      const T a = jac_colmajor[0];
+      const T b = jac_colmajor[1];
+      const T d = jac_colmajor[3];
+
+      // direct gauss newton step
+      const T a_sqr = a * a;
+      const T b_sqr = b * b;
+      const T d_sqr = d * d;
+      const T abbd = a * b + b * d;
+      const T abbd_sqr = abbd * abbd;
+      const T a2b2 = a_sqr + b_sqr;
+      const T a2b2_inv = 1.0/a2b2;
+      const T adabdb = a_sqr * d_sqr - 2 * a * b_sqr * d + b_sqr * b_sqr;
+      const T adabdb_inv = 1.0 / adabdb;
+      const T c1 = abbd * adabdb_inv;
+
+      x[0] += e_u * (a * (abbd_sqr * a2b2_inv * adabdb_inv + a2b2_inv) - b * c1) + e_v * (b * (abbd_sqr * a2b2_inv * adabdb_inv + a2b2_inv) - d * c1);
+      x[1] += e_u * (-a * c1 + b * a2b2 * adabdb_inv) + e_v * (-b * c1 + d * a2b2 * adabdb_inv);
+
+      if ((e_u * e_u + e_v * e_v) < 1e-8)
+      {
+        break;
+      }
+    }
+
+    px[0] = x[0];
+    px[1] = x[1];
+  }
+};
+
+// -----------------------------------------------------------------------------
+// This class implements the distortion model described in the paper:
+// "A Generic Camera Model and Calibration Method for Conventional, Wide-Angle,
+// and Fish-Eye Lenses" by Juho Kannala and Sami S. Brandt, PAMI.
+struct EquidistantDistortion
+{
+  static constexpr DistortionType type = DistortionType::Equidistant;
+
+  template <typename T>
+  CUDA_HOST CUDA_DEVICE
+  static void distort(const T* params, T* px, T* jac_colmajor = nullptr)
+  {
+    const T x = px[0];
+    const T y = px[1];
+    const T k1 = params[0];
+    const T k2 = params[1];
+    const T k3 = params[2];
+    const T k4 = params[3];
+    const T r_sqr = x * x + y * y;
+    const T r = std::sqrt(r_sqr);
+    const T theta = std::atan(r);
+    const T theta2 = theta * theta;
+    const T theta4 = theta2 * theta2;
+    const T theta6 = theta4 * theta2;
+    const T theta8 = theta4 * theta4;
+    const T thetad = theta * (1.0 + k1 * theta2 + k2 * theta4 + k3 * theta6 + k4 * theta8);
+    const T scaling = (r > 1e-8) ? thetad / r : 1.0;
+    px[0] *= scaling;
+    px[1] *= scaling;
+
+    if (jac_colmajor)
+    {
+      T& J_00 = jac_colmajor[0];
+      T& J_10 = jac_colmajor[1];
+      T& J_01 = jac_colmajor[2];
+      T& J_11 = jac_colmajor[3];
+
+      if(r < 1e-7)
+      {
+        J_00 = 1.0; J_01 = 0.0;
+        J_10 = 0.0; J_11 = 1.0;
+      }
+      else
+      {
+        T xx = x * x;
+        T yy = y * y;
+        T xy = x * y;
+        T theta_inv_r = theta / r;
+        T theta_sqr = theta * theta;
+        T theta_four = theta_sqr * theta_sqr;
+
+        T t1 = 1.0 / (xx + yy + 1.0);
+        T t2 = k1 * theta_sqr
+             + k2 * theta_four
+             + k3 * theta_four * theta_sqr
+             + k4 * (theta_four * theta_four) + 1.0;
+        T t3 = t1 * theta_inv_r;
+
+        T offset = t2 * theta_inv_r;
+        T scale  = t2 * (t1 / r_sqr - theta_inv_r / r_sqr)
+            + theta_inv_r * t3 * (
+                  2.0 * k1
+                + 4.0 * k2 * theta_sqr
+                + 6.0 * k3 * theta_four
+                + 8.0 * k4 * theta_four * theta_sqr);
+
+        J_11 = yy * scale + offset;
+        J_00 = xx * scale + offset;
+        J_01 = xy * scale;
+        J_10 = J_01;
+      }
+    }
+  }
+
+  template <typename T>
+  CUDA_HOST CUDA_DEVICE
+  static void undistort(const T* params, T* px)
+  {
+    T jac_colmajor[4];
+    T x[2];
+    T x_tmp[2];
+    x[0] = px[0]; x[1]= px[1];
+    for(int i = 0; i < 30; ++i)
+    {
+      x_tmp[0] = x[0]; x_tmp[1] = x[1];
+      distort(params, x_tmp, jac_colmajor);
+
+      const T e_u = px[0] - x_tmp[0];
+      const T e_v = px[1] - x_tmp[1];
+
+      const T a = jac_colmajor[0];
+      const T b = jac_colmajor[1];
+      const T d = jac_colmajor[3];
+
+      // direct gauss newton step
+      const T a_sqr = a * a;
+      const T b_sqr = b * b;
+      const T d_sqr = d * d;
+      const T abbd = a * b + b * d;
+      const T abbd_sqr = abbd * abbd;
+      const T a2b2 = a_sqr + b_sqr;
+      const T a2b2_inv = 1.0 / a2b2;
+      const T adabdb = a_sqr * d_sqr - 2 * a * b_sqr * d + b_sqr * b_sqr;
+      const T adabdb_inv = 1.0 / adabdb;
+      const T c1 = abbd * adabdb_inv;
+
+      x[0] += e_u * (a * (abbd_sqr * a2b2_inv * adabdb_inv + a2b2_inv) - b * c1) + e_v * (b * (abbd_sqr * a2b2_inv * adabdb_inv + a2b2_inv) - d * c1);
+      x[1] += e_u * (-a * c1 + b * a2b2 * adabdb_inv) + e_v * (-b * c1 + d * a2b2 * adabdb_inv);
+
+      if ((e_u * e_u + e_v * e_v) < 1e-8)
+      {
+        break;
+      }
+    }
+
+    px[0] = x[0];
+    px[1] = x[1];
+  }
+};
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_cameras/include/ze/cameras/camera_rig.hpp b/RWR/src/ze_oss/ze_cameras/include/ze/cameras/camera_rig.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..15ec59fde466fc34715af04d7d6c03333cfbac68
--- /dev/null
+++ b/RWR/src/ze_oss/ze_cameras/include/ze/cameras/camera_rig.hpp
@@ -0,0 +1,168 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#pragma once
+
+#include <string>
+#include <vector>
+#include <gflags/gflags.h>
+
+#include <ze/cameras/camera.hpp>
+#include <ze/common/types.hpp>
+#include <ze/common/macros.hpp>
+#include <ze/common/transformation.hpp>
+
+DECLARE_string(calib_filename);
+DECLARE_string(mask_cam0);
+DECLARE_string(mask_cam1);
+DECLARE_string(mask_cam2);
+DECLARE_string(mask_cam3);
+DECLARE_bool(calib_use_single_camera);
+
+namespace ze {
+
+// convenience typedefs.
+using CameraVector     = std::vector<Camera::Ptr>;
+using StereoIndexPair  = std::pair<uint8_t, uint8_t>;
+using StereoIndexPairs = std::vector<StereoIndexPair>;
+
+//! A camera rig is a set of rigidly attached cameras. The cameras are
+//! assumed intrinsically and extrinsically calibrated.
+class CameraRig
+{
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  ZE_POINTER_TYPEDEFS(CameraRig);
+
+  CameraRig() = delete;
+
+  CameraRig(
+      const TransformationVector& T_C_B,
+      const CameraVector& cameras,
+      const std::string& label,
+      const real_t stereo_min_fov_overlap = 0.7,
+      const real_t stereo_min_baseline = 0.04);
+
+  //! @name Camera extrinsic calibration: Pose of (B)ody/Imu in (C)amera frame.
+  //! @{
+  inline const Transformation& T_C_B(size_t camera_index) const
+  {
+    DEBUG_CHECK_LT(camera_index, T_C_B_.size());
+    return T_C_B_[camera_index];
+  }
+
+  inline Transformation T_B_C(size_t camera_index) const
+  {
+    DEBUG_CHECK_LT(camera_index, T_C_B_.size());
+    return T_C_B_[camera_index].inverse();
+  }
+
+  inline const TransformationVector& T_C_B_vec() const
+  {
+    return T_C_B_;
+  }
+  //! @}
+
+  //! @name Camera accessors.
+  //! @{
+  inline const Camera& at(size_t camera_index) const
+  {
+    DEBUG_CHECK_LT(camera_index, cameras_.size());
+    return *cameras_[camera_index];
+  }
+
+  inline std::shared_ptr<Camera> atShared(size_t camera_index)
+  {
+    DEBUG_CHECK_LT(camera_index, cameras_.size());
+    return cameras_[camera_index];
+  }
+
+  inline std::shared_ptr<const Camera> atShared(size_t camera_index) const
+  {
+    DEBUG_CHECK_LT(camera_index, cameras_.size());
+    return cameras_[camera_index];
+  }
+
+  inline const CameraVector& cameras() const { return cameras_; }
+  //! @}
+
+  inline size_t size() const { return cameras_.size(); }
+
+  inline const std::string& label() const { return label_; }
+
+  inline const StereoIndexPairs& stereoPairs() const { return stereo_pairs_; }
+
+  inline void setStereoPairs(const StereoIndexPairs& pairs) { stereo_pairs_ = pairs; }
+
+  //! @name Camera iteration.
+  //! @{
+  typedef CameraVector::value_type value_type;
+  typedef CameraVector::iterator iterator;
+  typedef CameraVector::const_iterator const_iterator;
+  CameraVector::iterator begin() { return cameras_.begin(); }
+  CameraVector::iterator end() { return cameras_.end(); }
+  CameraVector::const_iterator begin() const { return cameras_.begin(); }
+  CameraVector::const_iterator end() const { return cameras_.end(); }
+  CameraVector::const_iterator cbegin() const { return cameras_.cbegin(); }
+  CameraVector::const_iterator cend() const { return cameras_.cend(); }
+  //! @}
+
+  //! Get a rig that contains only a subset of the cameras.
+  CameraRig::Ptr getSubRig(
+      const std::vector<uint32_t>& camera_indices,
+      const std::string& label);
+
+private:
+  //! The mounting transformations.
+  TransformationVector T_C_B_;
+
+  //! The camera geometries.
+  CameraVector cameras_;
+
+  //! Unique pairs of camera indices with overlapping field of view.
+  StereoIndexPairs stereo_pairs_;
+
+  //! A label for this camera rig, a name.
+  std::string label_;
+};
+
+//! Load a camera rig form a yaml file. Returns a nullptr if the loading fails.
+CameraRig::Ptr cameraRigFromYaml(const std::string& yaml_file);
+
+//! Load a camera rig form a gflag parameters. Returns a nullptr if the loading fails.
+CameraRig::Ptr cameraRigFromGflags();
+
+//! Formatted printing.
+std::ostream& operator<<(std::ostream& out, const CameraRig& rig);
+std::ostream& operator<<(std::ostream& out, const StereoIndexPairs& stereo_pairs);
+
+//! Compute overlapping field of view and baseline for all stereo pairs
+StereoIndexPairs identifyStereoPairsInRig(
+    const CameraRig& rig,
+    const real_t& min_fov_overlap,
+    const real_t& min_baseline);
+
+} // namespace ze
+
diff --git a/RWR/src/ze_oss/ze_cameras/include/ze/cameras/camera_utils.hpp b/RWR/src/ze_oss/ze_cameras/include/ze/cameras/camera_utils.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..68668061b8cfd71db62c74ad3bf39a8872026f62
--- /dev/null
+++ b/RWR/src/ze_oss/ze_cameras/include/ze/cameras/camera_utils.hpp
@@ -0,0 +1,137 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#pragma once
+
+#include <tuple>
+
+#include <ze/common/types.hpp>
+#include <imp/core/size.hpp>
+
+namespace ze {
+
+// fwd
+class Camera;
+class CameraRig;
+
+// -----------------------------------------------------------------------------
+// Generate visible keypoints and landmarks.
+
+//! Generate random visible keypoints.
+Keypoints generateRandomKeypoints(
+    const Size2u image_size,
+    const uint32_t margin,
+    const uint32_t num_keypoints);
+
+//! Generate count random visible keypoints.
+Keypoints generateUniformKeypoints(
+    const Size2u image_size,
+    const uint32_t margin,
+    const uint32_t num_keypoints);
+
+//! Generate random visible 3d points.
+std::tuple<Keypoints, Bearings, Positions> generateRandomVisible3dPoints(
+    const Camera& cam,
+    const uint32_t num_points,
+    const uint32_t margin = 10u,
+    const real_t min_depth = 1.0,
+    const real_t max_depth = 3.0);
+
+// -----------------------------------------------------------------------------
+// Check overlapping field of view.
+
+//! Check if two cameras in a rig have an overlapping field of view.
+//! @return Approximate percentage of overlapping field of view between cameras.
+real_t overlappingFieldOfView(
+    const CameraRig& rig,
+    const uint32_t cam_a,
+    const uint32_t cam_b);
+
+// -----------------------------------------------------------------------------
+// Check landmark visiblity.
+
+//! Return if pixel u is within image boundaries.
+template<typename DerivedKeyPoint>
+bool isVisible(
+    const Size2u image_size,
+    const Eigen::MatrixBase<DerivedKeyPoint>& px)
+{
+  return px[0] >= 0u
+      && px[1] >= 0u
+      && px[0] <  image_size.width()
+      && px[1] <  image_size.height();
+}
+
+//! Return if pixel u is within image boundaries.
+template<typename DerivedKeyPoint>
+bool isVisible(
+    const typename DerivedKeyPoint::Scalar image_width,
+    const typename DerivedKeyPoint::Scalar image_height,
+    const Eigen::MatrixBase<DerivedKeyPoint>& px)
+{
+  return px[0] >= 0
+      && px[1] >= 0
+      && px[0] <  image_width
+      && px[1] <  image_height;
+}
+
+//! Return if pixel px is within image boundaries with margin.
+template<typename DerivedKeyPoint>
+bool isVisibleWithMargin(
+    const typename DerivedKeyPoint::Scalar image_width,
+    const typename DerivedKeyPoint::Scalar image_height,
+    const Eigen::MatrixBase<DerivedKeyPoint>& px,
+    const typename DerivedKeyPoint::Scalar margin)
+{
+  return px[0] >= margin
+      && px[1] >= margin
+      && px[0] < (image_width - margin)
+      && px[1] < (image_height - margin);
+}
+
+//! Return if pixel px is within image boundaries with margin.
+template<typename DerivedKeyPoint>
+bool isVisibleWithMargin(
+    const Size2u image_size,
+    const Eigen::MatrixBase<DerivedKeyPoint>& px,
+    const typename DerivedKeyPoint::Scalar margin)
+{
+  return px[0] >= margin
+      && px[1] >= margin
+      && px[0] < (static_cast<typename DerivedKeyPoint::Scalar>(image_size.width()) - margin)
+      && px[1] < (static_cast<typename DerivedKeyPoint::Scalar>(image_size.height()) - margin);
+}
+
+//! Return if pixel px is within image boundaries with margin.
+inline bool isVisibleWithMargin(
+    const int image_width, const int image_height, const int x, const int y, const int margin)
+{
+  return x >= margin
+      && y >= margin
+      && x < (image_width - margin)
+      && y < (image_height - margin);
+}
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_cameras/include/ze/cameras/camera_yaml_serialization.hpp b/RWR/src/ze_oss/ze_cameras/include/ze/cameras/camera_yaml_serialization.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..31d9b631fe63c97feb171f73123421248a0d5edd
--- /dev/null
+++ b/RWR/src/ze_oss/ze_cameras/include/ze/cameras/camera_yaml_serialization.hpp
@@ -0,0 +1,57 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#pragma once
+
+#include <memory>
+
+#include <ze/common/logging.hpp>
+#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
+// The yaml-cpp version in yaml_cpp_catkin uses auto_ptr which is deprecated.
+#include <yaml-cpp/yaml.h>
+#pragma diagnostic pop
+
+namespace ze {
+class Camera;
+class CameraRig;
+}  // namespace ze
+
+namespace YAML {
+
+template<>
+struct convert<std::shared_ptr<ze::Camera>>
+{
+  static bool decode(const Node& node, std::shared_ptr<ze::Camera>& camera);
+  static Node encode(const std::shared_ptr<ze::Camera>& camera);
+};
+
+template<>
+struct convert<std::shared_ptr<ze::CameraRig>>
+{
+  static bool decode(const Node& node, std::shared_ptr<ze::CameraRig>& camera);
+  static Node encode(const std::shared_ptr<ze::CameraRig>& camera_rig);
+};
+
+}  // namespace YAML
diff --git a/RWR/src/ze_oss/ze_cameras/package.xml b/RWR/src/ze_oss/ze_cameras/package.xml
new file mode 100644
index 0000000000000000000000000000000000000000..699e958a60e86d1f586b8fe4521e9114663235fa
--- /dev/null
+++ b/RWR/src/ze_oss/ze_cameras/package.xml
@@ -0,0 +1,25 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>ze_cameras</name>
+  <version>0.1.4</version>
+  <description>
+    Camera class.
+  </description>
+  <maintainer email="christian.forster@WyssZurich.ch">Christian Forster</maintainer>
+  <license>ZE</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <buildtool_depend>catkin_simple</buildtool_depend>
+
+  <depend>eigen_catkin</depend>
+  <depend>glog_catkin</depend>
+  <depend>gflags_catkin</depend>
+  <depend>imp_core</depend>
+  <depend>imp_bridge_opencv</depend>
+  <depend>minkindr</depend>
+  <depend>yaml_cpp_catkin</depend>
+  <depend>ze_cmake</depend>
+  <depend>ze_common</depend>
+
+  <test_depend>gtest</test_depend>
+</package>
diff --git a/RWR/src/ze_oss/ze_cameras/src/camera.cpp b/RWR/src/ze_oss/ze_cameras/src/camera.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..129054ac6f9b0fb6d050a00cf82ca06a71f0fc7d
--- /dev/null
+++ b/RWR/src/ze_oss/ze_cameras/src/camera.cpp
@@ -0,0 +1,195 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include <ze/cameras/camera.hpp>
+
+#include <string>
+#include <ze/cameras/camera_yaml_serialization.hpp>
+
+namespace ze {
+
+Camera::Camera(const uint32_t width, const uint32_t height, const CameraType type,
+               const VectorX& projection_params, const VectorX& distortion_params)
+  : size_(width, height)
+  , projection_params_(projection_params)
+  , distortion_params_(distortion_params)
+  , type_(type)
+{
+  CHECK_EQ(projection_params_.size(), 4);
+  switch (type_)
+  {
+    case CameraType::Pinhole:
+      CHECK_EQ(distortion_params_.size(), 0);
+      break;
+    case CameraType::PinholeRadialTangential:
+      CHECK_EQ(distortion_params_.size(), 4);
+      break;
+    case CameraType::PinholeEquidistant:
+      CHECK_EQ(distortion_params_.size(), 4);
+      break;
+    case CameraType::PinholeFov:
+    {
+      CHECK_EQ(distortion_params_.size(), 1);
+      // Pre-computations for improved speed.
+      const real_t s = distortion_params_(0);
+      const real_t tan_s_half_x2 = std::tan(s / 2.0) * 2.0;
+      distortion_params_ = Vector2(s, tan_s_half_x2);
+      break;
+    }
+    default:
+      LOG(FATAL) << "Unknown camera model.";
+      break;
+  }
+}
+
+Keypoint Camera::projectHomogeneous(
+    const Eigen::Ref<const HomPosition>& pos_h) const
+{
+  if (pos_h[3] < 0.0)
+  {
+    return this->project(-pos_h.head<3>());
+  }
+  else
+  {
+    return this->project(pos_h.head<3>());
+  }
+}
+
+std::pair<Keypoint, bool> Camera::projectHomogeneousWithCheck(
+    const Eigen::Ref<const HomPosition>& pos_h,
+    real_t border_margin) const
+{
+  if (pos_h[3] < 0.0)
+  {
+    return this->projectWithCheck(-pos_h.head<3>(), border_margin);
+  }
+  else
+  {
+    return this->projectWithCheck(pos_h.head<3>(), border_margin);
+  }
+}
+
+Matrix24 Camera::dProjectHomogeneous_dLandmark(
+    const Eigen::Ref<const HomPosition>& pos_h) const
+{
+  Matrix24 J;
+  if (pos_h[3] < 0.0)
+  {
+    J.topLeftCorner<2, 3>() = this->dProject_dLandmark(-pos_h.head<3>());
+  }
+  else
+  {
+    J.topLeftCorner<2, 3>() = this->dProject_dLandmark(pos_h.head<3>());
+  }
+  J.bottomRightCorner<2, 1>().setZero();
+  return J;
+}
+
+std::pair<Keypoint, Matrix24> Camera::projectHomogeneousWithJacobian(
+    const Eigen::Ref<const HomPosition>& pos_h) const
+{
+  return std::make_pair(projectHomogeneous(pos_h),
+                        dProjectHomogeneous_dLandmark(pos_h));
+}
+
+Bearings Camera::backProjectVectorized(const Eigen::Ref<const Keypoints>& px_vec) const
+{
+  Bearings bearings(3, px_vec.cols());
+  for(int i = 0; i < px_vec.cols(); ++i)
+  {
+    bearings.col(i) = this->backProject(px_vec.col(i));
+  }
+  return bearings;
+}
+
+Keypoints Camera::projectVectorized(const Eigen::Ref<const Bearings>& bearing_vec) const
+{
+  Keypoints px_vec(2, bearing_vec.cols());
+  for(int i = 0; i < bearing_vec.cols(); ++i)
+  {
+    px_vec.col(i) = this->project(bearing_vec.col(i));
+  }
+  return px_vec;
+}
+
+Matrix6X Camera::dProject_dLandmarkVectorized(const Positions& pos_vec) const
+{
+  Matrix6X J_vec(6, pos_vec.cols());
+  for(int i = 0; i < pos_vec.cols(); ++i)
+  {
+    J_vec.col(i) =
+        Eigen::Map<Matrix61>(this->dProject_dLandmark(pos_vec.col(i)).data());
+  }
+  return J_vec;
+}
+
+std::string Camera::typeAsString() const
+{
+  switch (type_)
+  {
+    case CameraType::Pinhole: return "Pinhole";
+    case CameraType::PinholeFov: return "PinholeFov";
+    case CameraType::PinholeEquidistant: return "PinholeEquidistant";
+    case CameraType::PinholeRadialTangential: return "PinholeRadialTangential";
+    default:
+      LOG(FATAL) << "Unknown parameter type";
+  }
+  return "";
+}
+
+void Camera::setMask(const Image8uC1::Ptr& mask)
+{
+  CHECK_NOTNULL(mask.get());
+  CHECK_EQ(mask->size(), size_);
+  mask_ = mask;
+}
+
+Camera::Ptr cameraFromYaml(const std::string& path)
+{
+  try
+  {
+    YAML::Node doc = YAML::LoadFile(path.c_str());
+    return doc.as<Camera::Ptr>();
+  }
+  catch (const std::exception& ex)
+  {
+    LOG(ERROR) << "Failed to load Camera from file " << path << " with the error: \n"
+               << ex.what();
+  }
+  return Camera::Ptr();
+}
+
+std::ostream& operator<<(std::ostream& out, const Camera& cam)
+{
+  out << "    Label = " << cam.label() << "\n"
+      << "    Model = " << cam.typeAsString() << "\n"
+      << "    Dimensions = " << cam.width() << "x" << cam.height() << "\n"
+      << "    Proj. parameters = " << cam.projectionParameters().transpose() << "\n"
+      << "    Dist. parameters = " << cam.distortionParameters().transpose() << "\n"
+      << "    Masked = " << (cam.mask() ? "True" : "False");
+  return out;
+}
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_cameras/src/camera_impl.cpp b/RWR/src/ze_oss/ze_cameras/src/camera_impl.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..941cc765d8b3f756a7959b9793bded56ce0925a3
--- /dev/null
+++ b/RWR/src/ze_oss/ze_cameras/src/camera_impl.cpp
@@ -0,0 +1,38 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include <ze/cameras/camera_impl.hpp>
+
+namespace ze {
+
+//-----------------------------------------------------------------------------
+// Explicitely instantiate the desired classes.
+// (sync with typedefs at the end of the hpp file)
+template class PinholeProjection<NoDistortion>;
+template class PinholeProjection<FovDistortion>;
+template class PinholeProjection<RadialTangentialDistortion>;
+template class PinholeProjection<EquidistantDistortion>;
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_cameras/src/camera_rig.cpp b/RWR/src/ze_oss/ze_cameras/src/camera_rig.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..4ac20b04925a1328335298a066d3d103e08279fa
--- /dev/null
+++ b/RWR/src/ze_oss/ze_cameras/src/camera_rig.cpp
@@ -0,0 +1,213 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include <ze/cameras/camera_rig.hpp>
+
+#include <imp/bridge/opencv/cv_bridge.hpp>
+#include <imp/core/image_raw.hpp>
+#include <ze/cameras/camera_utils.hpp>
+#include <ze/cameras/camera_yaml_serialization.hpp>
+#include <ze/common/path_utils.hpp>
+
+DEFINE_string(calib_filename, "", "Camera calibration file.");
+DEFINE_string(mask_cam0, "", "Mask for camera 0");
+DEFINE_string(mask_cam1, "", "Mask for camera 1");
+DEFINE_string(mask_cam2, "", "Mask for camera 2");
+DEFINE_string(mask_cam3, "", "Mask for camera 3");
+DEFINE_bool(calib_use_single_camera, false, "");
+
+namespace ze {
+
+// -----------------------------------------------------------------------------
+CameraRig::CameraRig(
+    const TransformationVector& T_C_B,
+    const CameraVector& cameras,
+    const std::string& label,
+    const real_t stereo_min_fov_overlap,
+    const real_t stereo_min_baseline)
+  : T_C_B_(T_C_B)
+  , cameras_(cameras)
+  , label_(label)
+{
+  CHECK_EQ(T_C_B_.size(), cameras_.size());
+  for(size_t i = 0; i < size(); ++i)
+  {
+    CHECK_NOTNULL(cameras_[i].get());
+  }
+
+  if (size() > 1u)
+  {
+    setStereoPairs(identifyStereoPairsInRig(
+                     *this, stereo_min_fov_overlap, stereo_min_baseline));
+  }
+}
+
+// -----------------------------------------------------------------------------
+CameraRig::Ptr CameraRig::getSubRig(
+    const std::vector<uint32_t>& camera_indices,
+    const std::string& label)
+{
+  CameraVector cameras;
+  TransformationVector T;
+  for (uint32_t i : camera_indices)
+  {
+    cameras.push_back(atShared(i));
+    T.push_back(T_C_B(i));
+  }
+  return std::make_shared<CameraRig>(T, cameras, label);
+}
+
+// -----------------------------------------------------------------------------
+CameraRig::Ptr cameraRigFromYaml(const std::string& yaml_file)
+{
+  CHECK(fileExists(yaml_file)) << "File does not exist: " << yaml_file;
+
+  CameraRig::Ptr rig;
+  try
+  {
+    YAML::Node doc = YAML::LoadFile(yaml_file.c_str());
+    rig = doc.as<CameraRig::Ptr>();
+  }
+  catch (const std::exception& ex)
+  {
+    LOG(ERROR) << "Cannot load CameraRig from file:" << yaml_file << "\n"
+               << ex.what();
+    return nullptr;
+  }
+
+  return rig;
+}
+
+// -----------------------------------------------------------------------------
+CameraRig::Ptr cameraRigFromGflags()
+{
+  CHECK(fileExists(FLAGS_calib_filename)) << "Camera file does not exist.";
+  CameraRig::Ptr rig = cameraRigFromYaml(FLAGS_calib_filename);
+  CHECK(rig);
+  if (FLAGS_calib_use_single_camera)
+  {
+    rig = rig->getSubRig({0}, rig->label());
+    CHECK(rig);
+  }
+
+  if(!FLAGS_mask_cam0.empty())
+  {
+    CHECK_GT(rig->size(), 0u);
+    CHECK(fileExists(FLAGS_mask_cam0));
+    ImageCv8uC1::Ptr mask;
+    cvBridgeLoad<Pixel8uC1>(mask, FLAGS_mask_cam0, PixelOrder::gray);
+    rig->atShared(0)->setMask(mask);
+  }
+
+  if(!FLAGS_mask_cam1.empty())
+  {
+    CHECK_GT(rig->size(), 1u);
+    CHECK(fileExists(FLAGS_mask_cam1));
+    ImageCv8uC1::Ptr mask;
+    cvBridgeLoad<Pixel8uC1>(mask, FLAGS_mask_cam1, PixelOrder::gray);
+    rig->atShared(1)->setMask(mask);
+  }
+
+  if(!FLAGS_mask_cam2.empty())
+  {
+    CHECK_GT(rig->size(), 2u);
+    CHECK(fileExists(FLAGS_mask_cam2));
+    ImageCv8uC1::Ptr mask;
+    cvBridgeLoad<Pixel8uC1>(mask, FLAGS_mask_cam2, PixelOrder::gray);
+    rig->atShared(2)->setMask(mask);
+  }
+
+  if(!FLAGS_mask_cam3.empty())
+  {
+    CHECK_GT(rig->size(), 3u);
+    CHECK(fileExists(FLAGS_mask_cam3));
+    ImageCv8uC1::Ptr mask;
+    cvBridgeLoad<Pixel8uC1>(mask, FLAGS_mask_cam3, PixelOrder::gray);
+    rig->atShared(3)->setMask(mask);
+  }
+
+  return rig;
+}
+
+// -----------------------------------------------------------------------------
+std::ostream& operator<<(std::ostream& out, const CameraRig& rig)
+{
+  out << "Camera Rig: \n"
+      << "  Label = " << rig.label() << "\n"
+      << "  Stereo pairs =" << rig.stereoPairs() << "\n";
+  for (size_t i = 0; i < rig.size(); ++i)
+  {
+    out << "- Camera " << i << "\n"
+        << rig.at(i) << "\n"
+        << "    T_B_C = \n" << rig.T_C_B(i).inverse() << "\n";
+  }
+  return out;
+}
+
+// -----------------------------------------------------------------------------
+std::ostream& operator<<(std::ostream& out, const StereoIndexPairs& stereo_pairs)
+{
+  for (auto it : stereo_pairs)
+  {
+    out << " (" << static_cast<int>(it.first) << ", "
+        << static_cast<int>(it.second) << ")";
+  }
+  return out;
+}
+
+// -----------------------------------------------------------------------------
+StereoIndexPairs identifyStereoPairsInRig(
+    const CameraRig& rig,
+    const real_t& min_fov_overlap,
+    const real_t& min_baseline)
+{
+  StereoIndexPairs pairs;
+  for (uint32_t cam_A = 0u; cam_A < rig.size(); ++cam_A)
+  {
+    for (uint32_t cam_B = cam_A + 1u; cam_B < rig.size(); ++cam_B)
+    {
+      real_t overlap = overlappingFieldOfView(rig, cam_A, cam_B);
+      real_t baseline = (rig.T_C_B(cam_B) * rig.T_C_B(cam_A).inverse()).getPosition().norm();
+
+
+      if (overlap > min_fov_overlap && baseline > min_baseline)
+      {
+        VLOG(1) << "Camera " << cam_A << " and " << cam_B
+                << ": Overlap = " << overlap << ", Baseline = " << baseline
+                << " -> Stereo Rig.";
+        pairs.push_back(std::make_pair(cam_A, cam_B));
+      }
+      else
+      {
+        VLOG(1) << "Camera " << cam_A << " and " << cam_B
+                << ": Overlap = " << overlap << ", Baseline = " << baseline
+                << " -> No stereo rig (baseline or overlap too small)";
+      }
+    }
+  }
+  return pairs;
+}
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_cameras/src/camera_utils.cpp b/RWR/src/ze_oss/ze_cameras/src/camera_utils.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..dc8900a94958aca588aa5ad2e222c2ee5e737bd8
--- /dev/null
+++ b/RWR/src/ze_oss/ze_cameras/src/camera_utils.cpp
@@ -0,0 +1,131 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include <ze/cameras/camera_utils.hpp>
+
+#include <random>
+#include <ze/common/config.hpp>
+#include <ze/common/logging.hpp>
+#include <ze/common/random_matrix.hpp>
+
+#include <ze/cameras/camera_rig.hpp>
+
+namespace ze {
+
+// -----------------------------------------------------------------------------
+Keypoints generateRandomKeypoints(
+    const Size2u size,
+    const uint32_t margin,
+    const uint32_t num_keypoints)
+{
+  DEBUG_CHECK_GT(size.width(), margin + 1u);
+  DEBUG_CHECK_GT(size.height(), margin + 1u);
+
+  Keypoints kp(2, num_keypoints);
+  for(uint32_t i = 0u; i < num_keypoints; ++i)
+  {
+    kp(0,i) = sampleUniformRealDistribution<real_t>(false, margin, size.width() - 1 - margin);
+    kp(1,i) = sampleUniformRealDistribution<real_t>(false, margin, size.height() - 1 - margin);
+  }
+  return kp;
+}
+
+// -----------------------------------------------------------------------------
+Keypoints generateUniformKeypoints(
+    const Size2u size,
+    const uint32_t margin,
+    const uint32_t num_cols)
+{
+  DEBUG_CHECK_GT(size.width(), margin + 1u);
+  DEBUG_CHECK_GT(size.height(), margin + 1u);
+  const uint32_t num_rows = num_cols * size.height() / size.width();
+
+  // Compute width and height of a cell:
+  real_t w = (static_cast<real_t>(size.width() - 0.01)  - 2.0 * margin) / (num_cols - 1);
+  real_t h = (static_cast<real_t>(size.height() - 0.01) - 2.0 * margin) / (num_rows - 1);
+
+  // Sample keypoints:
+  Keypoints kp(2, num_rows * num_cols);
+  for (uint32_t y = 0u; y < num_rows; ++y)
+  {
+    for (uint32_t x = 0u; x < num_cols; ++x)
+    {
+      uint32_t i = y * num_cols + x;
+      kp(0,i) = margin + x * w;
+      kp(1,i) = margin + y * h;
+    }
+  }
+  return kp;
+}
+
+// -----------------------------------------------------------------------------
+std::tuple<Keypoints, Bearings, Positions> generateRandomVisible3dPoints(
+    const Camera& cam,
+    const uint32_t num_points,
+    const uint32_t margin,
+    const real_t min_depth,
+    const real_t max_depth)
+{
+  Keypoints px = generateRandomKeypoints(cam.size(), margin, num_points);
+  Bearings f = cam.backProjectVectorized(px);
+  Positions pos = f;
+  for(uint32_t i = 0u; i < num_points; ++i)
+  {
+    pos.col(i) *= sampleUniformRealDistribution<real_t>(false, min_depth, max_depth);
+  }
+  return std::make_tuple(px, f, pos);
+}
+
+// -----------------------------------------------------------------------------
+real_t overlappingFieldOfView(
+    const CameraRig& rig,
+    const uint32_t cam_A,
+    const uint32_t cam_B)
+{
+  DEBUG_CHECK_LT(cam_A, rig.size());
+  DEBUG_CHECK_LT(cam_B, rig.size());
+
+  // We sample uniformly keypoints in camera a and project them into camera b,
+  // assuming the landmark is at infinity (i.e. only rotate).
+  Keypoints px_A = generateUniformKeypoints(rig.at(cam_A).size(), 0u, 20u);
+  Bearings f_A = rig.at(cam_A).backProjectVectorized(px_A);
+  Transformation T_B_A = rig.T_C_B(cam_B) * rig.T_C_B(cam_A).inverse();
+  Positions p_B = T_B_A.getRotation().rotateVectorized(f_A);
+  Keypoints px_B = rig.at(cam_B).projectVectorized(p_B);
+
+  uint32_t num_visible = 0u;
+  for (int i = 0; i < px_B.cols(); ++i)
+  {
+    //! @todo: Omnidirectional cameras: Improve check.
+    if (p_B.col(i)(2) > 0 && isVisible(rig.at(cam_B).size(), px_B.col(i)))
+    {
+      ++num_visible;
+    }
+  }
+
+  return static_cast<real_t>(num_visible) / px_B.cols();
+}
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_cameras/src/camera_yaml_serialization.cpp b/RWR/src/ze_oss/ze_cameras/src/camera_yaml_serialization.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e8bada28857cd5e5d79f5216f6bad9fa383a59cb
--- /dev/null
+++ b/RWR/src/ze_oss/ze_cameras/src/camera_yaml_serialization.cpp
@@ -0,0 +1,199 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include <ze/cameras/camera_yaml_serialization.hpp>
+#include <ze/cameras/camera_impl.hpp>
+#include <ze/cameras/camera_rig.hpp>
+#include <ze/common/yaml_serialization.hpp>
+#include <ze/common/types.hpp>
+
+namespace YAML {
+
+//------------------------------------------------------------------------------
+// Camera loading.
+bool convert<std::shared_ptr<ze::Camera>>::decode(
+    const Node& node, ze::Camera::Ptr& camera)
+{
+  camera.reset();
+  if (!node.IsMap())
+  {
+    LOG(ERROR) << "Parsing Camera failed because node is not a map.";
+    return false;
+  }
+  try 
+  {
+    std::string camera_type = extractChild<std::string>(node, "type");
+    int width = extractChild<int>(node, "image_width");
+    int height = extractChild<int>(node, "image_height");
+    ze::VectorX intrinsics = extractChild<ze::VectorX>(node, "intrinsics");
+    const YAML::Node distortion_config = node["distortion"];
+    std::string distortion_type = extractChild<std::string>(distortion_config, "type");
+    ze::VectorX distortion_parameters = extractChild<ze::VectorX>(distortion_config, "parameters");
+
+    if(camera_type == "pinhole" && distortion_type == "none")
+    {
+      camera = std::make_shared<ze::PinholeCamera>(
+            width, height, ze::CameraType::Pinhole, intrinsics,
+            distortion_parameters);
+    }
+    else if(camera_type == "pinhole" && distortion_type == "radial-tangential")
+    {
+      camera = std::make_shared<ze::RadTanCamera>(
+            width, height, ze::CameraType::PinholeRadialTangential, intrinsics,
+            distortion_parameters);
+    }
+    else if(camera_type == "pinhole" && distortion_type == "equidistant")
+    {
+      camera = std::make_shared<ze::EquidistantCamera>(
+            width, height, ze::CameraType::PinholeEquidistant, intrinsics,
+            distortion_parameters);
+    }
+    else if(camera_type == "pinhole" && distortion_type == "fisheye")
+    {
+      camera = std::make_shared<ze::FovCamera>(
+            width, height, ze::CameraType::PinholeFov, intrinsics,
+            distortion_parameters);
+    }
+    else
+    {
+      LOG(FATAL) << "Camera model not yet supported.";
+    }
+
+    if(node["label"])
+    {
+      camera->setLabel(node["label"].as<std::string>());
+    }
+  }
+  catch(const std::exception& e)
+  {
+    LOG(ERROR) << "YAML exception during parsing: " << e.what();
+    camera.reset();
+    return false;
+  }
+  return true;
+}
+
+//------------------------------------------------------------------------------
+// Camera writing.
+Node convert<ze::Camera::Ptr>::encode(const ze::Camera::Ptr& camera)
+{
+  LOG(FATAL) << "Not implemented!";
+  Node camera_node;
+  return camera_node;
+}
+
+//------------------------------------------------------------------------------
+// CameraRig loading.
+bool convert<std::shared_ptr<ze::CameraRig>>::decode(
+    const Node& node, ze::CameraRig::Ptr& camera_rig)
+{
+  camera_rig.reset();
+  try {
+    if (!node.IsMap())
+    {
+      LOG(ERROR) << "Parsing CameraRig failed because node is not a map.";
+      return false;
+    }
+
+    std::string label = extractChild<std::string>(node, "label");
+
+    const Node cameras_node = node["cameras"];
+    if (!cameras_node.IsSequence())
+    {
+      LOG(ERROR) << "Parsing CameraRig failed because 'cameras' is not a sequence.";
+      return false;
+    }
+
+    size_t num_cameras = cameras_node.size();
+    if (num_cameras == 0)
+    {
+      LOG(ERROR) << "Parsing CameraRig failed. Number of cameras is 0.";
+      return false;
+    }
+
+    ze::TransformationVector T_Ci_B;
+    ze::CameraVector cameras;
+    for (size_t i = 0; i < num_cameras; ++i)
+    {
+      const Node& camera_node = cameras_node[i];
+      if (!camera_node)
+      {
+        LOG(ERROR) << "Unable to get camera node for camera " << i;
+        return false;
+      }
+      if (!camera_node.IsMap())
+      {
+        LOG(ERROR) << "Camera node for camera " << i << " is not a map.";
+        return false;
+      }
+
+      ze::Camera::Ptr camera = extractChild<ze::Camera::Ptr>(camera_node, "camera");
+      cameras.push_back(camera);
+
+      if (camera_node["T_B_C"])
+      {
+        ze::Matrix4 T_B_C = extractChild<ze::Matrix4>(camera_node, "T_B_C");
+        T_Ci_B.push_back(ze::Transformation(
+                           ze::Quaternion::fromApproximateRotationMatrix(T_B_C.block<3,3>(0,0)),
+                           T_B_C.block<3,1>(0,3)).inverse());
+      }
+      else if (camera_node["T_C_B"])
+      {
+        ze::Matrix4 T_C_B = extractChild<ze::Matrix4>(camera_node, "T_C_B");
+        T_Ci_B.push_back(ze::Transformation(
+                           ze::Quaternion::fromApproximateRotationMatrix(T_C_B.block<3,3>(0,0)),
+                           T_C_B.block<3,1>(0,3)));
+      }
+      else
+      {
+        LOG(ERROR) << "Unable to get extrinsic transformation T_B_C or T_C_B "
+                   << "for camera " << i;
+        return false;
+      }
+    }
+
+    camera_rig.reset(new ze::CameraRig(T_Ci_B, cameras, label));
+  }
+  catch (const std::exception& ex)
+  {
+    LOG(ERROR) << "YAML exception during parsing: " << ex.what();
+    camera_rig.reset();
+    return false;
+  }
+  return true;
+}
+
+//------------------------------------------------------------------------------
+// CameraRig writing.
+Node convert<std::shared_ptr<ze::CameraRig> >::encode(
+    const std::shared_ptr<ze::CameraRig>& /*camera_rig*/)
+{
+  LOG(FATAL) << "Not implemented!";
+  Node camera_rig_node;
+  return camera_rig_node;
+}
+
+}  // namespace YAML
+
diff --git a/RWR/src/ze_oss/ze_cameras/test/test_camera_impl.cpp b/RWR/src/ze_oss/ze_cameras/test/test_camera_impl.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..0b6ac888fffa696df064ada57e032eb59c467506
--- /dev/null
+++ b/RWR/src/ze_oss/ze_cameras/test/test_camera_impl.cpp
@@ -0,0 +1,338 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include <string>
+#include <vector>
+#include <iostream>
+#include <functional>
+
+#include <ze/common/benchmark.hpp>
+#include <ze/common/test_entrypoint.hpp>
+#include <ze/common/test_utils.hpp>
+#include <ze/common/path_utils.hpp>
+#include <ze/common/matrix.hpp>
+#include <ze/common/manifold.hpp>
+#include <ze/common/numerical_derivative.hpp>
+#include <ze/cameras/camera_rig.hpp>
+#include <ze/cameras/camera.hpp>
+#include <ze/cameras/camera_impl.hpp>
+#include <ze/cameras/camera_utils.hpp>
+
+DEFINE_bool(run_benchmark, false, "Benchmark the camera models?");
+
+namespace ze {
+
+class CameraBenchmark
+{
+public:
+  CameraBenchmark(const Camera& cam, size_t sample_size, const std::string& test_name)
+    : cam_(cam)
+    , sample_size_(sample_size)
+    , test_name_(test_name)
+  {
+    px_ = generateRandomKeypoints(cam_.size(), 10u, sample_size_);
+    f_ = cam.backProjectVectorized(px_);
+  }
+
+  void benchmarkAll()
+  {
+    if (!FLAGS_run_benchmark) {
+      return;
+    }
+
+    real_t p1 = project();
+    real_t p = (p1 - projectVectorized()) / p1;
+    real_t p2 = backProject();
+    real_t bp = (p2 - backProjectVectorized()) / p2;
+    VLOG(1) << "[" << test_name_ << "]" << "Vectorized back projection " << bp * 100 << "% faster." << "\n"
+            << "[" << test_name_ << "]" << "Vectorized projection " << p * 100 << "% faster.";
+  }
+
+  real_t backProjectVectorized()
+  {
+    // Test back-project vectorized
+    Bearings f(3, sample_size_);
+    auto backProjectVectorizedLambda = [&]()
+    {
+      f = cam_.backProjectVectorized(px_);
+    };
+    return runTimingBenchmark(backProjectVectorizedLambda, 10, 20,
+                       test_name_ + ": Back-project vectorized", true);
+  }
+
+  real_t backProject()
+  {
+    // Test not vectorized.
+    Bearings f(3, sample_size_);
+    auto backProjectLambda = [&]()
+    {
+      for (size_t i = 0; i < sample_size_; ++i)
+      {
+        f.col(i) = cam_.backProjectVectorized(px_.col(i));
+      }
+    };
+    return runTimingBenchmark(backProjectLambda, 10, 20,
+                       test_name_ + ": Back-project N-times", true);
+  }
+
+  real_t projectVectorized()
+  {
+    // Test back-project vectorized
+    Keypoints px(2, sample_size_);
+    auto projectVectorizedLambda = [&]()
+    {
+      px = cam_.projectVectorized(f_);
+    };
+    return runTimingBenchmark(projectVectorizedLambda, 10, 20,
+                       test_name_ + ": Project vectorized", true);
+  }
+
+  real_t project()
+  {
+    // Test not vectorized.
+    Keypoints px(2, sample_size_);
+    auto projectLambda = [&]()
+    {
+      for (size_t i = 0; i < sample_size_; ++i)
+      {
+        px.col(i) = cam_.project(f_.col(i));
+      }
+    };
+    return runTimingBenchmark(projectLambda, 10, 20,
+                       test_name_ + ": Project vectorized", true);
+  }
+private:
+  const Camera& cam_;
+  size_t sample_size_;
+  std::string test_name_;
+  Keypoints px_;
+  Bearings f_;
+};
+
+class CameraTestHarness
+{
+public:
+  CameraTestHarness(const Camera& cam, size_t sample_size, const std::string& test_name)
+    : cam_(cam)
+    , sample_size_(sample_size)
+    , test_name_(test_name)
+  {}
+
+  void testProjection()
+  {
+    uint32_t N = 300;
+    Keypoints px1 = generateRandomKeypoints(cam_.size(), 10u, sample_size_);
+    Bearings f1 = cam_.backProjectVectorized(px1);
+    Keypoints px2 = cam_.projectVectorized(f1);
+    Keypoints px_error = px1 - px2;
+    real_t max_error = px_error.colwise().norm().array().maxCoeff();
+    EXPECT_LT(max_error, 1.3e-4);
+  }
+
+  void testProjectionSingle()
+  {
+    Vector3 bearing = cam_.backProject(Vector2(200, 300));
+    Vector2 px = cam_.project(bearing);
+#ifndef ZE_SINGLE_PRECISION_FLOAT
+    EXPECT_TRUE(EIGEN_MATRIX_NEAR(px, Vector2(200, 300), 1e-6));
+#else
+    EXPECT_TRUE(EIGEN_MATRIX_NEAR(px, Vector2(200, 300), 1e-4));
+#endif
+  }
+
+  void testHomogeneousProjection()
+  {
+    Vector3 bearing = cam_.backProject(Vector2(200.0, 300.0));
+    Vector4 hom_position;
+    {
+      // normal case:
+      hom_position << bearing, 1.0;
+      Vector2 px = cam_.projectHomogeneous(hom_position);
+      EXPECT_TRUE(EIGEN_MATRIX_NEAR(px, Vector2(200.0, 300.0), 1e-6));
+    }
+
+    {
+      // point at infinity:
+      hom_position << bearing, 0.0;
+      Vector2 px = cam_.projectHomogeneous(hom_position);
+      EXPECT_TRUE(EIGEN_MATRIX_NEAR(px, Vector2(200.0, 300.0), 1e-6));
+    }
+
+    {
+      // point behind camera.
+      hom_position << bearing, -1.0;
+      Vector2 px = cam_.projectHomogeneous(hom_position);
+      EXPECT_TRUE(EIGEN_MATRIX_NEAR(px, Vector2(200.0, 300.0), 1e-6));
+    }
+
+    {
+      // some arbitrary scaling.
+      hom_position << bearing, 10.0;
+      Vector2 px = cam_.projectHomogeneous(hom_position);
+      EXPECT_TRUE(EIGEN_MATRIX_NEAR(px, Vector2(200.0, 300.0), 1e-6));
+    }
+  }
+
+  void testJacobian()
+  {
+#ifndef ZE_SINGLE_PRECISION_FLOAT
+    Vector3 bearing = cam_.backProject(Vector2(200, 300));
+    Matrix23 H = cam_.dProject_dLandmark(bearing);
+    Matrix23 H_numerical =
+        numericalDerivative<Vector2, Vector3>(
+        std::bind(&Camera::project, &cam_, std::placeholders::_1), bearing);
+    EXPECT_TRUE(EIGEN_MATRIX_NEAR(H, H_numerical, 1e-6));
+#else
+    LOG(WARNING) << "Numerical derivative test ignored for single precision float.";
+#endif
+  }
+
+  void testHomogeneousJacobian()
+  {
+    Vector4 hom_position;
+    hom_position.head<3>() = cam_.backProject(Vector2(200.0, 300.0));
+    hom_position[3] = 1.0;
+    Matrix24 H = cam_.dProjectHomogeneous_dLandmark(hom_position);
+    Matrix24 H_numerical =
+        numericalDerivative<Vector2, Vector4>(
+          std::bind(&Camera::projectHomogeneous, &cam_, std::placeholders::_1),
+          hom_position);
+    EXPECT_TRUE(EIGEN_MATRIX_NEAR(H, H_numerical, 1e-6));
+  }
+
+  void testAll()
+  {
+    {
+      SCOPED_TRACE("Projection");
+      testProjection();
+    }
+    {
+      SCOPED_TRACE("ProjectionSingle");
+      testProjectionSingle();
+    }
+    {
+      SCOPED_TRACE("HomogeneousProjection");
+      testHomogeneousProjection();
+    }
+    {
+      SCOPED_TRACE("Jacobian");
+      testJacobian();
+    }
+    {
+      SCOPED_TRACE("HomogeneousJacobian");
+      testHomogeneousJacobian();
+    }
+  }
+
+private:
+  const Camera& cam_;
+  size_t sample_size_;
+  std::string test_name_;
+};
+
+} // namespace ze
+
+TEST(CameraImplTests, testPinhole)
+{
+  using namespace ze;
+  PinholeCamera cam = createPinholeCamera(752, 480, 310, 320, 376.0, 240.0);
+  CameraTestHarness test(cam, 300, "Pinhole");
+  test.testAll();
+  CameraBenchmark benchmark(cam, 300, "Pinhole");
+  benchmark.benchmarkAll();
+}
+
+TEST(CameraImplTests, testFov)
+{
+  using namespace ze;
+  FovCamera cam = createFovCamera(752, 480, 310, 320, 376.0, 240.0, 0.947367);
+  CameraTestHarness test(cam, 300, "Fov");
+  test.testAll();
+  CameraBenchmark benchmark(cam, 300, "Fov");
+  benchmark.benchmarkAll();
+}
+
+TEST(CameraImplTests, testRadTan)
+{
+  using namespace ze;
+  RadTanCamera cam = createRadTanCamera(752, 480, 310, 320, 376.0, 240.0,
+                                        -0.2834, 0.0739, 0.00019, 1.76e-05);
+  CameraTestHarness test(cam, 300, "RadTan");
+  test.testAll();
+  CameraBenchmark benchmark(cam, 300, "RadTan");
+  benchmark.benchmarkAll();
+}
+
+TEST(CameraImplTests, testEquidistant)
+{
+  using namespace ze;
+  EquidistantCamera cam = createEquidistantCamera(752, 480, 310, 320, 376.0, 240.0,
+                                                  -0.00279, 0.02414, -0.04304, 0.03118);
+  CameraTestHarness test(cam, 300, "Equidistant");
+  test.testAll();
+  CameraBenchmark benchmark(cam, 300, "Equidistant");
+  benchmark.benchmarkAll();
+}
+
+TEST(CameraImplTests, testYamlParsingPinhole)
+{
+  std::string data_dir = ze::getTestDataDir("camera_models");
+  std::string yaml_file = data_dir + "/camera_pinhole_nodistortion.yaml";
+  ASSERT_TRUE(ze::fileExists(yaml_file));
+  ze::Camera::Ptr cam = ze::cameraFromYaml(yaml_file);
+  EXPECT_FLOATTYPE_EQ(cam->projectionParameters()(0), 320.0);
+  EXPECT_FLOATTYPE_EQ(cam->projectionParameters()(1), 310.0);
+  EXPECT_FLOATTYPE_EQ(cam->projectionParameters()(2), 376.5);
+  EXPECT_FLOATTYPE_EQ(cam->projectionParameters()(3), 240.5);
+}
+
+TEST(CameraImplTests, testYamlParsingFoV)
+{
+  std::string data_dir = ze::getTestDataDir("camera_models");
+  std::string yaml_file = data_dir + "/camera_pinhole_fov.yaml";
+  ze::Camera::Ptr cam = ze::cameraFromYaml(yaml_file);
+  EXPECT_FLOATTYPE_EQ(cam->projectionParameters()(0), 320.0);
+  EXPECT_FLOATTYPE_EQ(cam->distortionParameters()(0), 0.940454);
+}
+
+TEST(CameraImplTests, testYamlParsingRadTan)
+{
+  std::string data_dir = ze::getTestDataDir("camera_models");
+  std::string yaml_file = data_dir + "/camera_pinhole_radtan.yaml";
+  ze::Camera::Ptr cam = ze::cameraFromYaml(yaml_file);
+  EXPECT_FLOATTYPE_EQ(cam->projectionParameters()(0), 320.0);
+  EXPECT_FLOATTYPE_EQ(cam->distortionParameters()(0), -0.28340811217029355);
+}
+
+TEST(CameraImplTests, testYamlParsingEquidistant)
+{
+  std::string data_dir = ze::getTestDataDir("camera_models");
+  std::string yaml_file = data_dir + "/camera_pinhole_equidistant.yaml";
+  ze::Camera::Ptr cam = ze::cameraFromYaml(yaml_file);
+  EXPECT_FLOATTYPE_EQ(cam->projectionParameters()(0), 320.0);
+  EXPECT_FLOATTYPE_EQ(cam->distortionParameters()(0), -0.0027973061697674074);
+}
+
+ZE_UNITTEST_ENTRYPOINT
diff --git a/RWR/src/ze_oss/ze_cameras/test/test_camera_rig.cpp b/RWR/src/ze_oss/ze_cameras/test/test_camera_rig.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b6614056dfa928f50c6867327a1783d77055e117
--- /dev/null
+++ b/RWR/src/ze_oss/ze_cameras/test/test_camera_rig.cpp
@@ -0,0 +1,72 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include <iostream>
+#include <ze/common/test_entrypoint.hpp>
+#include <ze/common/test_utils.hpp>
+#include <ze/common/path_utils.hpp>
+#include <ze/cameras/camera.hpp>
+#include <ze/cameras/camera_rig.hpp>
+
+TEST(CameraRigTests, testYamlLoading)
+{
+  using namespace ze;
+  CameraRig::Ptr rig =
+      cameraRigFromYaml(joinPath(getTestDataDir("camera_models"),
+                                 "camera_rig_1.yaml"));
+
+  EXPECT_EQ(rig->size(), 2);
+  EXPECT_NEAR(rig->at(0).projectionParameters()(0), 458.654, 1e-3);
+  EXPECT_NEAR(rig->at(1).projectionParameters()(0), 457.587, 1e-3);
+  EXPECT_NEAR(rig->T_C_B(0).getTransformationMatrix()(0, 0), 0.0148655, 1e-3);
+  EXPECT_NEAR(rig->T_C_B(1).getTransformationMatrix()(0, 0), 0.0125553, 1e-3);
+  EXPECT_STREQ(rig->label().c_str(), "Euroc");
+  EXPECT_STREQ(rig->at(0).label().c_str(), "cam0");
+  EXPECT_STREQ(rig->at(1).label().c_str(), "cam1");
+}
+
+TEST(CameraRigTests, testStereoPairIdentification)
+{
+  using namespace ze;
+
+  {
+    CameraRig::Ptr rig =
+        cameraRigFromYaml(joinPath(getTestDataDir("camera_models"),
+                                   "camera_rig_1.yaml"));
+    StereoIndexPairs pairs = identifyStereoPairsInRig(*rig, 0.7, 0.1);
+    EXPECT_EQ(pairs.size(), 1u);
+  }
+
+  {
+    CameraRig::Ptr rig =
+        cameraRigFromYaml(joinPath(getTestDataDir("camera_models"),
+                                   "camera_rig_2.yaml"));
+    StereoIndexPairs pairs = identifyStereoPairsInRig(*rig, 0.7, 0.1);
+    EXPECT_EQ(pairs.size(), 1u);
+  }
+
+}
+
+ZE_UNITTEST_ENTRYPOINT
diff --git a/RWR/src/ze_oss/ze_cameras/test/test_camera_utils.cpp b/RWR/src/ze_oss/ze_cameras/test/test_camera_utils.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..3dfabefef06e49930bc65d4b84cf4c003592311d
--- /dev/null
+++ b/RWR/src/ze_oss/ze_cameras/test/test_camera_utils.cpp
@@ -0,0 +1,106 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include <iostream>
+#include <ze/common/test_entrypoint.hpp>
+#include <ze/cameras/camera_utils.hpp>
+#include <ze/cameras/camera_rig.hpp>
+#include <ze/common/test_utils.hpp>
+#include <ze/common/path_utils.hpp>
+#include <ze/common/config.hpp>
+#ifdef ZE_USE_OPENCV
+#include <opencv2/highgui/highgui.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
+#endif
+
+TEST(CameraUtilsTest, randomKeypoints)
+{
+  const int margin = 20;
+  const int num_keypoints = 200;
+  ze::Keypoints kps = ze::generateRandomKeypoints(ze::Size2u(640, 480), margin, num_keypoints);
+  ASSERT_EQ(kps.cols(), num_keypoints);
+  for (size_t i = 0; i < num_keypoints; ++i)
+  {
+    EXPECT_GE(kps(0,i), margin);
+    EXPECT_GE(kps(1,i), margin);
+    EXPECT_LT(kps(0,i), 640 - margin - 1);
+    EXPECT_LT(kps(1,i), 480 - margin - 1);
+  }
+
+#ifdef ZE_USE_OPENCV
+  if (false)
+  {
+    cv::Mat img(480, 640, CV_8UC1, cv::Scalar(0));
+    for (size_t i = 0; i < num_keypoints; ++i)
+    {
+      cv::circle(img, cv::Point(kps(0,i), kps(1,i)), 2, cv::Scalar(255));
+    }
+    cv::imshow("img", img);
+    cv::waitKey(0);
+  }
+#endif
+}
+
+TEST(CameraUtilsTest, uniformKeypoints)
+{
+  using namespace ze;
+
+  {
+    uint32_t margin = 0;
+    Size2u img_size(752, 480);
+    Keypoints kps = generateUniformKeypoints(img_size, margin, 50);
+    for (int i = 0; i < kps.cols(); ++i)
+    {
+      EXPECT_TRUE(isVisibleWithMargin(img_size, kps.col(i), margin));
+    }
+  }
+
+  {
+    uint32_t margin = 10;
+    Size2u img_size(752, 480);
+    Keypoints kps = generateUniformKeypoints(img_size, margin, 50);
+    for (int i = 0; i < kps.cols(); ++i)
+    {
+      EXPECT_TRUE(isVisibleWithMargin(img_size, kps.col(i), margin));
+    }
+  }
+}
+
+TEST(CameraUtilsTest, overlappingFieldOfView)
+{
+  using namespace ze;
+
+  std::string data_dir = ze::getTestDataDir("camera_models");
+  std::string yaml_file = ze::joinPath(data_dir, "camera_rig_1.yaml");
+  ze::CameraRig::Ptr rig = ze::cameraRigFromYaml(yaml_file);
+
+  real_t overlap1 = overlappingFieldOfView(*rig, 0u, 1u);
+  real_t overlap2 = overlappingFieldOfView(*rig, 1u, 0u);
+
+  EXPECT_NEAR(overlap1, overlap2, 0.1);
+}
+
+ZE_UNITTEST_ENTRYPOINT
+
diff --git a/RWR/src/ze_oss/ze_cmake/CMakeLists.txt b/RWR/src/ze_oss/ze_cmake/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..621668b1e6a645cc8c7094de8bfdd805e10608fc
--- /dev/null
+++ b/RWR/src/ze_oss/ze_cmake/CMakeLists.txt
@@ -0,0 +1,11 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(ze_cmake)
+
+find_package(catkin_simple REQUIRED)
+catkin_simple()
+
+# install all included cmake files
+install(DIRECTORY cmake DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/cmake)
+
+cs_install()
+cs_export(CFG_EXTRAS ze_cmake-extras.cmake)
diff --git a/RWR/src/ze_oss/ze_cmake/cmake/macros/ze_macros_common.cmake b/RWR/src/ze_oss/ze_cmake/cmake/macros/ze_macros_common.cmake
new file mode 100644
index 0000000000000000000000000000000000000000..58e2f3032006e41a7c9c515ad478d4cf931e0400
--- /dev/null
+++ b/RWR/src/ze_oss/ze_cmake/cmake/macros/ze_macros_common.cmake
@@ -0,0 +1,22 @@
+macro(ze_debug)
+  string(REPLACE ";" " " __msg "${ARGN}")
+  message(STATUS "${__msg}")
+endmacro()
+
+macro(ze_warn)
+  string(REPLACE ";" " " __msg "${ARGN}")
+  message(WARNING "${__msg}")
+endmacro()
+
+macro(ze_fatal)
+  string(REPLACE ";" " " __msg "${ARGN}")
+  message(FATAL_ERROR "${__msg}")
+endmacro()
+
+macro(ze_include)
+  #ze_debug("[MACRO] ze_include( ${ARGN} )")
+  include_directories(${ARGN})
+  if (WITH_CUDA AND CUDA_FOUND)
+    cuda_include_directories(${ARGN})
+  endif()
+endmacro()
diff --git a/RWR/src/ze_oss/ze_cmake/cmake/macros/ze_macros_cuda.cmake b/RWR/src/ze_oss/ze_cmake/cmake/macros/ze_macros_cuda.cmake
new file mode 100644
index 0000000000000000000000000000000000000000..ff43d28fe894c178cfbfbaa7a8686a87af97a08e
--- /dev/null
+++ b/RWR/src/ze_oss/ze_cmake/cmake/macros/ze_macros_cuda.cmake
@@ -0,0 +1,148 @@
+# finding cuda and adding compile flags
+macro(find_cuda)
+  cmake_parse_arguments(add_args "REQUIRED;NOT_AUTOMATIC" "" "" ${ARGN})
+
+  if(add_args_REQUIRED)
+    find_package(CUDA REQUIRED)
+  else()
+    find_package(CUDA)
+  endif()
+
+  message(STATUS "CUDA_FOUND: ${CUDA_FOUND}")
+  message(STATUS "CUDA_VERSION_STRING: ${CUDA_VERSION_STRING}")
+  message(STATUS "CUDA_VERSION_MAJOR: ${CUDA_VERSION_MAJOR}")
+  
+  if ((NOT ${CUDA_FOUND}) OR (${CUDA_VERSION_MAJOR} LESS 7))
+    message(WARNING "CUDA not found or too old CUDA version (CUDA_VERSION_MAJOR < 7). Skipping this package.")
+    return()
+  endif()
+  add_definitions(-DWITH_CUDA)
+
+  #list (APPEND CUDA_NVCC_FLAGS -relaxed-constexpr -use_fast_math -std=c++11)
+  list (APPEND CUDA_NVCC_FLAGS -use_fast_math -std=c++11)
+  #list (APPEND CUDA_NVCC_FLAGS --compiler-options;-fno-strict-aliasing;)
+  list (APPEND CUDA_NVCC_FLAGS --compiler-options;-fPIC;)
+  list (APPEND CUDA_NVCC_FLAGS --compiler-options;-Wall;)
+  #list (APPEND CUDA_NVCC_FLAGS --compiler-options;-Werror;)
+  if(CMAKE_BUILD_TYPE MATCHES Debug)
+    list (APPEND CUDA_NVCC_FLAGS --device-debug)
+    list (APPEND CUDA_NVCC_FLAGS --compiler-options;-g;)
+    list (APPEND CUDA_NVCC_FLAGS --compiler-options;-rdynamic;)
+    list (APPEND CUDA_NVCC_FLAGS --compiler-options;-lineinfo;)
+    list (APPEND CUDA_NVCC_FLAGS --ptxas-options=-v;)
+  endif()
+
+  #list (APPEND CUDA_NVCC_FLAGS --device-c)
+  #list (APPEND CUDA_NVCC_FLAGS -rdc=true)
+
+  set(CUDA_SEPARABLE_COMPILATION OFF)
+  # set to OFF cuda files are added to multiple targets
+  set(CUDA_ATTACH_VS_BUILD_RULE_TO_CUDA_FILE ON)
+  set(BUILD_SHARED_LIBS ON)
+
+  # nvcc and ccache are not very good friends, hence we set the host compiler
+  # for cuda manually if ccache is enabled.
+  ##! @todo (MWE) find/check the path of the compiler as well as the OS
+  get_filename_component(COMPILER_EXE ${CMAKE_C_COMPILER} REALPATH)
+  message(STATUS "CMAKE_C_COMPILER: ${CMAKE_C_COMPILER}")
+  message(STATUS "COMPILER_EXE: ${COMPILER_EXE}")
+  string(REGEX MATCH "ccache" SYSTEM_USE_CCACHE "${COMPILER_EXE}")
+  if(SYSTEM_USE_CCACHE)
+    if(EXISTS "${CUDA_TOOLKIT_ROOT_DIR}/bin/gcc")
+      set(CUDA_HOST_COMPILER ${CUDA_TOOLKIT_ROOT_DIR}/bin/gcc)
+    elseif(EXISTS "/usr/bin/gcc")
+      set(CUDA_HOST_COMPILER /usr/bin/gcc)
+    endif()
+    MESSAGE(STATUS "CUDA_HOST_COMPILER: ${CUDA_HOST_COMPILER}")
+  endif()
+
+  include_directories(
+    ${CUDA_INCLUDE_DIRS}
+    ${CUDA_SDK_INCLUDE_DIR}
+    ${CMAKE_CURRENT_SOURCE_DIR}/../../cuda_toolkit/${CUDA_VERSION_STRING}/include
+    )
+
+  # Checking cuda version
+  if(CUDA_VERSION_STRING STREQUAL "7.0")
+    # CUDA 7.0
+    add_definitions(-DCUDA_VERSION_70)
+  elseif(CUDA_VERSION_STRING STREQUAL "7.5")
+    # CUDA 7.5
+    add_definitions(-DCUDA_VERSION_75)
+  elseif(CUDA_VERSION_STRING STREQUAL "8.0")
+    # CUDA 8.0
+    add_definitions(-DCUDA_VERSION_80)
+  else()
+    message(FATAL_ERROR "unknown CUDA version. some things might not be tested.")
+  endif()
+
+
+  # Selection of compute capability via environment variable
+  if("$ENV{NV_COMPUTE_CAPABILITY}" MATCHES "1.1")
+    list(APPEND CUDA_NVCC_FLAGS -arch=sm_11)
+  elseif("$ENV{NV_COMPUTE_CAPABILITY}" MATCHES "1.2")
+    list(APPEND CUDA_NVCC_FLAGS -arch=sm_12)
+  elseif("$ENV{NV_COMPUTE_CAPABILITY}" MATCHES "1.3")
+    list(APPEND CUDA_NVCC_FLAGS -arch=sm_13)
+  elseif("$ENV{NV_COMPUTE_CAPABILITY}" MATCHES "2.0")
+    list(APPEND CUDA_NVCC_FLAGS -arch=sm_20)
+    list(APPEND CUDA_NVCC_FLAGS -gencode arch=compute_20,code=sm_20)
+  elseif("$ENV{NV_COMPUTE_CAPABILITY}" MATCHES "2.1")
+    list(APPEND CUDA_NVCC_FLAGS -arch=sm_21)
+  elseif("$ENV{NV_COMPUTE_CAPABILITY}" MATCHES "3.0")
+    list(APPEND CUDA_NVCC_FLAGS -arch=sm_30)
+    list(APPEND CUDA_NVCC_FLAGS -gencode arch=compute_30,code=sm_30)
+  elseif("$ENV{NV_COMPUTE_CAPABILITY}" MATCHES "3.5")
+    list(APPEND CUDA_NVCC_FLAGS -arch=sm_35)
+    list(APPEND CUDA_NVCC_FLAGS -gencode arch=compute_35,code=sm_35)
+  else()
+    list(APPEND CUDA_NVCC_FLAGS -arch=sm_30)
+  endif()
+
+
+  # define can be used to define exceptions and throw it if a CUDA error is caught by the error-check
+  ##! @todo (MWE) make this configurable
+  add_definitions(-DTHROW_ON_CUDA_ERROR)
+  
+endmacro()
+
+##------------------------------------------------------------------------------
+# macro for adding cuda executables in the style of catkin_simple
+macro(cs_cuda_add_executable _target)
+  if(${_target} STREQUAL ${PROJECT_NAME}_package)
+    message(WARNING "Could not create executable with name '${_target}' as '${PROJECT_NAME}_package' is reserved for the top level target name for this project.")
+  endif()
+  cmake_parse_arguments(cs_cuda_add_executable_args "NO_AUTO_LINK;NO_AUTO_DEP" "" "" ${ARGN})
+  cuda_add_executable(${_target} ${cs_cuda_add_executable_args_UNPARSED_ARGUMENTS})
+  if(NOT cs_cuda_add_executable_args_NO_AUTO_LINK)
+    target_link_libraries(${_target} ${CUDA_LIBRARIES} ${catkin_LIBRARIES})
+  endif()
+  if(NOT cs_cuda_add_executable_args_NO_AUTO_DEP)
+    if(NOT "${${PROJECT_NAME}_CATKIN_BUILD_DEPENDS_EXPORTED_TARGETS}" STREQUAL "")
+      add_dependencies(${_target} ${${PROJECT_NAME}_CATKIN_BUILD_DEPENDS_EXPORTED_TARGETS})
+    endif()
+  endif()
+  cs_add_targets_to_package(${_target})
+endmacro()
+
+##------------------------------------------------------------------------------
+# macro for adding cuda library in the style of catkin_simple
+macro(cs_cuda_add_library _target)
+  if(${_target} STREQUAL ${PROJECT_NAME}_package)
+    message(WARNING "Could not create library with name '${_target}' as '${PROJECT_NAME}_package' is reserved for the top level target name for this project.")
+  endif()
+  cmake_parse_arguments(cs_cuda_add_library "NO_AUTO_LINK;NO_AUTO_DEP;NO_AUTO_EXPORT" "" "" ${ARGN})
+  cuda_add_library(${_target} ${cs_cuda_add_library_UNPARSED_ARGUMENTS})
+  if(NOT cs_cuda_add_library_NO_AUTO_LINK)
+    target_link_libraries(${_target} ${CUDA_LIBRARIES} ${catkin_LIBRARIES})
+  endif()
+  if(NOT cs_cuda_add_library_NO_AUTO_DEP)
+    if(NOT "${${PROJECT_NAME}_CATKIN_BUILD_DEPENDS_EXPORTED_TARGETS}" STREQUAL "")
+      add_dependencies(${_target} ${${PROJECT_NAME}_CATKIN_BUILD_DEPENDS_EXPORTED_TARGETS})
+    endif()
+  endif()
+  if(NOT cs_cuda_add_library_NO_AUTO_EXPORT)
+    list(APPEND ${PROJECT_NAME}_LIBRARIES ${_target})
+  endif()
+  cs_add_targets_to_package(${_target})
+endmacro()
\ No newline at end of file
diff --git a/RWR/src/ze_oss/ze_cmake/cmake/modules/FindDepthSense.cmake b/RWR/src/ze_oss/ze_cmake/cmake/modules/FindDepthSense.cmake
new file mode 100644
index 0000000000000000000000000000000000000000..66b73a9a20057459cc80f598d63b69036492a481
--- /dev/null
+++ b/RWR/src/ze_oss/ze_cmake/cmake/modules/FindDepthSense.cmake
@@ -0,0 +1,41 @@
+# Try to find the DepthSense SDK For SoftKinetic Cameras
+#
+# DepthSense_INCLUDE_DIRS
+# DepthSense_LIBRARIES
+# DepthSense_FOUND
+
+FIND_PATH( DepthSense_INCLUDE_DIR DepthSense.hxx
+  PATHS
+      "$ENV{PROGRAMFILES}/SoftKinetic/DepthSenseSDK/include"
+      /usr/include
+      /usr/local/include
+      /opt/local/include
+      /opt/softkinetic/DepthSenseSDK/include
+)
+
+FIND_LIBRARY( DepthSense_LIBRARY DepthSense
+  PATHS
+      "$ENV{PROGRAMFILES}/SoftKinetic/DepthSenseSDK/lib"
+      /usr/lib64
+      /usr/lib
+      /usr/local/lib
+      /opt/local/lib
+      /opt/softkinetic/DepthSenseSDK/lib
+)
+
+IF(DepthSense_INCLUDE_DIR AND DepthSense_LIBRARY)
+  SET( DepthSense_FOUND TRUE )
+  SET( DepthSense_LIBRARIES ${DepthSense_LIBRARY} )
+  SET( DepthSense_INCLUDE_DIRS ${DepthSense_INCLUDE_DIR} )
+ENDIF()
+
+IF(DepthSense_FOUND)
+   IF(NOT DepthSense_FIND_QUIETLY)
+      MESSAGE(STATUS "Found DepthSense: ${DepthSense_LIBRARY}")
+   ENDIF()
+ELSE()
+   IF(DepthSense_FIND_REQUIRED)
+      MESSAGE(FATAL_ERROR "Could not find DepthSense")
+   ENDIF()
+ENDIF()
+
diff --git a/RWR/src/ze_oss/ze_cmake/cmake/modules/ze_brisk.cmake b/RWR/src/ze_oss/ze_cmake/cmake/modules/ze_brisk.cmake
new file mode 100644
index 0000000000000000000000000000000000000000..15d0529200c949ef32b1d22989a37a81edf587b6
--- /dev/null
+++ b/RWR/src/ze_oss/ze_cmake/cmake/modules/ze_brisk.cmake
@@ -0,0 +1,8 @@
+if(ZE_USE_BRISK)
+  find_package(brisk REQUIRED)
+  message(STATUS "BRISK found.")
+  message(STATUS "BRISK libraries: ${brisk_LIBRARIES}")
+  message(STATUS "BRISK headers: ${brisk_INCLUDE_DIRS}")
+  list(APPEND catkin_LIBRARIES  ${brisk_LIBRARIES})
+  include_directories(${brisk_INCLUDE_DIRS})
+endif()
\ No newline at end of file
diff --git a/RWR/src/ze_oss/ze_cmake/cmake/modules/ze_gtsam.cmake b/RWR/src/ze_oss/ze_cmake/cmake/modules/ze_gtsam.cmake
new file mode 100644
index 0000000000000000000000000000000000000000..662757e5b952841a377de34445717154d3d13d60
--- /dev/null
+++ b/RWR/src/ze_oss/ze_cmake/cmake/modules/ze_gtsam.cmake
@@ -0,0 +1,11 @@
+if(ZE_USE_GTSAM)
+  find_package(gtsam_catkin REQUIRED)
+  message(STATUS "GTSAM headers: ${GTSAM_INCLUDE_DIR}")
+  message(STATUS "GTSAM (gtsam_catkin) found.")
+  message(STATUS "GTSAM (gtsam_catkin) libraries: ${gtsam_catkin_LIBRARIES}.")
+  message(STATUS "GTSAM (gtsam_catkin) headers: ${gtsam_catkin_INCLUDE_DIRS}.")
+  list(APPEND catkin_LIBRARIES  ${gtsam_catkin_LIBRARIES})
+  list(APPEND INCLUDE_DIRS ${GTSAM_INCLUDE_DIR})
+  list(APPEND INCLUDE_DIRS ${gtsam_catkin_INCLUDE_DIRS})
+  include_directories(${INCLUDE_DIRS})
+endif()
\ No newline at end of file
diff --git a/RWR/src/ze_oss/ze_cmake/cmake/modules/ze_options.cmake b/RWR/src/ze_oss/ze_cmake/cmake/modules/ze_options.cmake
new file mode 100644
index 0000000000000000000000000000000000000000..2cba16a430e920564e4f0eeac3fc08068a7efcdb
--- /dev/null
+++ b/RWR/src/ze_oss/ze_cmake/cmake/modules/ze_options.cmake
@@ -0,0 +1,7 @@
+option(ZE_SINGLE_PRECISION_FLOAT "Compile with single precision float" OFF)
+option(ZE_USE_BRISK "Compile with BRISK" ON)
+option(ZE_USE_OPENCV "Compile with OPENCV" ON)
+option(ZE_USE_GTSAM "Compile with GTSAM" ON)
+option(ZE_USE_ARRAYFIRE "Compile ArrayFire and IMP wrapper" OFF)
+option(ZE_DETERMINISTIC "Use deterministic random numbers" ON)
+option(ZE_VIO_LIMITED "Limited functionality in VIO" OFF)
diff --git a/RWR/src/ze_oss/ze_cmake/cmake/modules/ze_setup.cmake b/RWR/src/ze_oss/ze_cmake/cmake/modules/ze_setup.cmake
new file mode 100644
index 0000000000000000000000000000000000000000..cc9c1e32e273a4d47e56b140e50083c1f3deeae9
--- /dev/null
+++ b/RWR/src/ze_oss/ze_cmake/cmake/modules/ze_setup.cmake
@@ -0,0 +1,46 @@
+include(ze_options)
+
+# common flags
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Werror -fPIC -D_REENTRANT -Wno-maybe-uninitialized -Wno-unused-variable -Wno-unused-but-set-variable -Wno-unknown-pragmas -Wno-error=deprecated-declarations")
+set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS} -O3 -fsee -fomit-frame-pointer -fno-signed-zeros -fno-math-errno -funroll-loops -ffast-math -fno-finite-math-only")
+if (CMAKE_BUILD_TYPE STREQUAL "Debug")
+  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g")
+endif()
+
+
+find_package(Threads)
+set(CMAKE_THREAD_PREFER_PTHREAD TRUE)
+find_package(Threads REQUIRED)
+if (CMAKE_USE_PTHREADS_INIT)
+  set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -pthread")
+  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pthread")
+endif ()
+
+# arm
+if(DEFINED ENV{ARM_ARCHITECTURE})
+  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -mfpu=neon -march=armv7-a")
+  add_definitions(-DHAVE_FAST_NEON)
+else()
+  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -mmmx -msse -msse -msse2 -msse3 -mssse3")
+endif()
+
+# c++11
+if (CMAKE_VERSION VERSION_LESS "3.1" OR Boost_VERSION VERSION_LESS "1.56")
+  if (CMAKE_CXX_COMPILER_ID STREQUAL "GNU")
+    set (CMAKE_CXX_FLAGS "--std=gnu++11 ${CMAKE_CXX_FLAGS}")
+  elseif ("${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang")
+    set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -stdlib=libc++")
+  else ()
+    message(SEND_ERROR "Unknown or unsupported system.")
+  endif ()
+else ()
+  set (CMAKE_CXX_STANDARD 11)
+endif ()
+
+# RT library
+find_library(RT_LIBRARY NAMES rt librt)
+list(APPEND ZE_LIBRARIES ${RT_LIBRARY})
+
+# forward pure cmake dependencies if not added by <depend> in the package xml
+# but should be forwarded from/to the packages
+list(APPEND catkin_LIBRARIES ${ZE_LIBRARIES})
diff --git a/RWR/src/ze_oss/ze_cmake/cmake/ze_cmake-extras.cmake.develspace.in b/RWR/src/ze_oss/ze_cmake/cmake/ze_cmake-extras.cmake.develspace.in
new file mode 100644
index 0000000000000000000000000000000000000000..521edbfa5442aecc80310e93917ce7cdcc56aa7c
--- /dev/null
+++ b/RWR/src/ze_oss/ze_cmake/cmake/ze_cmake-extras.cmake.develspace.in
@@ -0,0 +1,2 @@
+list(INSERT CMAKE_MODULE_PATH 0 "@CMAKE_CURRENT_SOURCE_DIR@/cmake/modules")
+list(INSERT CMAKE_MODULE_PATH 0 "@CMAKE_CURRENT_SOURCE_DIR@/cmake/macros")
diff --git a/RWR/src/ze_oss/ze_cmake/cmake/ze_cmake-extras.cmake.installspace.in b/RWR/src/ze_oss/ze_cmake/cmake/ze_cmake-extras.cmake.installspace.in
new file mode 100644
index 0000000000000000000000000000000000000000..72e3dfb4953834a43318b739ef2e8581158c657f
--- /dev/null
+++ b/RWR/src/ze_oss/ze_cmake/cmake/ze_cmake-extras.cmake.installspace.in
@@ -0,0 +1,5 @@
+list(INSERT CMAKE_MODULE_PATH 0
+   "${cmake_modules_DIR}/../../../@CATKIN_PACKAGE_SHARE_DESTINATION@/cmake/modules")
+
+list(INSERT CMAKE_MODULE_PATH 0
+   "${cmake_modules_DIR}/../../../@CATKIN_PACKAGE_SHARE_DESTINATION@/cmake/macros")
diff --git a/RWR/src/ze_oss/ze_cmake/package.xml b/RWR/src/ze_oss/ze_cmake/package.xml
new file mode 100644
index 0000000000000000000000000000000000000000..dffe5ec4ec877fde45045b0611034555d55c7cef
--- /dev/null
+++ b/RWR/src/ze_oss/ze_cmake/package.xml
@@ -0,0 +1,14 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>ze_cmake</name>
+  <version>0.1.4</version>
+  <description>
+    CMake configuration, scripts and macros common for common ZE usage.
+  </description>
+  <maintainer email="christian.forster@WyssZurich.ch">Christian Forster</maintainer>
+  <maintainer email="manuel.werlberger@WyssZurich.ch">Manuel Werlberger</maintainer>
+  <license>ZE</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <buildtool_depend>catkin_simple</buildtool_depend>
+</package>
diff --git a/RWR/src/ze_oss/ze_common/.gitignore b/RWR/src/ze_oss/ze_common/.gitignore
new file mode 100644
index 0000000000000000000000000000000000000000..6df87ef60621819cf578f7589d198e10ed17c887
--- /dev/null
+++ b/RWR/src/ze_oss/ze_common/.gitignore
@@ -0,0 +1 @@
+include/ze/common/config.hpp
\ No newline at end of file
diff --git a/RWR/src/ze_oss/ze_common/CMakeLists.txt b/RWR/src/ze_oss/ze_common/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..6f1819f60d9e8feb77e437a3d0ed193bcfc754d6
--- /dev/null
+++ b/RWR/src/ze_oss/ze_common/CMakeLists.txt
@@ -0,0 +1,151 @@
+project(ze_common)
+cmake_minimum_required(VERSION 2.8.3)
+
+find_package(catkin_simple REQUIRED)
+catkin_simple(ALL_DEPS_REQUIRED)
+
+include(ze_setup)
+
+
+############
+# SETTINGS #
+############
+include(ze_options)
+message(STATUS "ZE_USE_OPENCV: ${ZE_USE_OPENCV}")
+message(STATUS "ZE_USE_BRISK: ${ZE_USE_BRISK}")
+message(STATUS "ZE_USE_GTSAM: ${ZE_USE_GTSAM}")
+message(STATUS "ZE_SINGLE_PRECISION_FLOAT: ${ZE_SINGLE_PRECISION_FLOAT}")
+message(STATUS "ZE_USE_ARRAYFIRE: ${ZE_USE_ARRAYFIRE}")
+message(STATUS "ZE_DETERMINISTIC: ${ZE_DETERMINISTIC}")
+message(STATUS "ZE_VIO_LIMITED: ${ZE_VIO_LIMITED}")
+
+configure_file(include/ze/common/config.hpp.in ${CMAKE_CURRENT_SOURCE_DIR}/include/ze/common/config.hpp)
+
+
+#############
+# LIBRARIES #
+#############
+set(HEADERS
+  include/ze/common/benchmark.hpp
+  include/ze/common/buffer.hpp
+  include/ze/common/buffer-inl.hpp
+  include/ze/common/config.hpp
+  include/ze/common/combinatorics.hpp
+  include/ze/common/csv_trajectory.hpp
+  include/ze/common/file_utils.hpp
+  include/ze/common/logging.hpp
+  include/ze/common/macros.hpp
+  include/ze/common/manifold.hpp
+  include/ze/common/math.hpp
+  include/ze/common/matrix.hpp
+  include/ze/common/nonassignable.hpp
+  include/ze/common/noncopyable.hpp
+  include/ze/common/numerical_derivative.hpp
+  include/ze/common/path_utils.hpp
+  include/ze/common/random.hpp
+  include/ze/common/random_matrix.hpp
+  include/ze/common/ringbuffer.hpp
+  include/ze/common/ringbuffer-inl.hpp
+  include/ze/common/ring_view.hpp
+  include/ze/common/running_statistics.hpp
+  include/ze/common/running_statistics_collection.hpp
+  include/ze/common/signal_handler.hpp
+  include/ze/common/statistics.hpp
+  include/ze/common/stl_utils.hpp
+  include/ze/common/string_utils.hpp
+  include/ze/common/test_entrypoint.hpp
+  include/ze/common/test_utils.hpp
+  include/ze/common/test_thread_blocking.hpp
+  include/ze/common/thread_pool.hpp
+  include/ze/common/thread_safe_fifo.hpp
+  include/ze/common/time_conversions.hpp
+  include/ze/common/timer.hpp
+  include/ze/common/timer_collection.hpp
+  include/ze/common/timer_statistics.hpp
+  include/ze/common/transformation.hpp
+  include/ze/common/types.hpp
+  include/ze/common/versioned_slot_handle.hpp
+  include/ze/common/yaml_serialization.hpp
+  )
+
+set(SOURCES
+  src/csv_trajectory.cpp
+  src/matrix.cpp
+  src/random.cpp
+  src/signal_handler.cpp
+  src/test_utils.cpp
+  src/test_thread_blocking.cpp
+  src/thread_pool.cpp
+  )
+
+cs_add_library(${PROJECT_NAME} ${SOURCES} ${HEADERS})
+
+
+##########
+# GTESTS #
+##########
+catkin_add_gtest(test_benchmark test/test_benchmark.cpp)
+target_link_libraries(test_benchmark ${PROJECT_NAME})
+
+catkin_add_gtest(test_buffer test/test_buffer.cpp)
+target_link_libraries(test_buffer ${PROJECT_NAME})
+
+catkin_add_gtest(test_csv_trajectory test/test_csv_trajectory.cpp)
+target_link_libraries(test_csv_trajectory ${PROJECT_NAME})
+
+catkin_add_gtest(test_manifold test/test_manifold.cpp)
+target_link_libraries(test_manifold ${PROJECT_NAME})
+
+catkin_add_gtest(test_matrix test/test_matrix.cpp)
+target_link_libraries(test_matrix ${PROJECT_NAME})
+
+catkin_add_gtest(test_numerical_derivative test/test_numerical_derivative.cpp)
+target_link_libraries(test_numerical_derivative ${PROJECT_NAME})
+
+catkin_add_gtest(test_random test/test_random.cpp)
+target_link_libraries(test_random ${PROJECT_NAME})
+
+catkin_add_gtest(test_random_matrix test/test_random_matrix.cpp)
+target_link_libraries(test_random_matrix ${PROJECT_NAME})
+
+catkin_add_gtest(test_running_statistics test/test_running_statistics.cpp)
+target_link_libraries(test_running_statistics ${PROJECT_NAME})
+
+catkin_add_gtest(test_statistics test/test_statistics.cpp)
+target_link_libraries(test_statistics ${PROJECT_NAME})
+
+catkin_add_gtest(test_stl_utils test/test_stl_utils.cpp)
+target_link_libraries(test_stl_utils ${PROJECT_NAME})
+
+catkin_add_gtest(test_string_utils test/test_string_utils.cpp)
+target_link_libraries(test_string_utils ${PROJECT_NAME})
+
+catkin_add_gtest(test_test_utils test/test_test_utils.cpp)
+target_link_libraries(test_test_utils ${PROJECT_NAME})
+
+catkin_add_gtest(test_timer test/test_timer.cpp)
+target_link_libraries(test_timer ${PROJECT_NAME})
+
+catkin_add_gtest(test_transformation test/test_transformation.cpp)
+target_link_libraries(test_transformation ${PROJECT_NAME})
+
+catkin_add_gtest(test_thread_pool test/test_thread_pool.cpp)
+target_link_libraries(test_thread_pool ${PROJECT_NAME})
+
+catkin_add_gtest(test_thread_safe_fifo test/test_thread_safe_fifo.cpp)
+target_link_libraries(test_thread_safe_fifo ${PROJECT_NAME})
+
+catkin_add_gtest(test_versioned_slot_handle test/test_versioned_slot_handle.cpp)
+target_link_libraries(test_versioned_slot_handle ${PROJECT_NAME})
+
+catkin_add_gtest(test_ring_view test/test_ring_view.cpp)
+target_link_libraries(test_ring_view ${PROJECT_NAME})
+
+catkin_add_gtest(test_ringbuffer test/test_ringbuffer.cpp)
+target_link_libraries(test_ringbuffer ${PROJECT_NAME})
+
+##########
+# EXPORT #
+##########
+cs_install()
+cs_export()
diff --git a/RWR/src/ze_oss/ze_common/include/ze/common/benchmark.hpp b/RWR/src/ze_oss/ze_common/include/ze/common/benchmark.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..59bef511b08650c2274ad283e21c104671f67a8c
--- /dev/null
+++ b/RWR/src/ze_oss/ze_common/include/ze/common/benchmark.hpp
@@ -0,0 +1,82 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#pragma once
+
+#include <functional>
+#include <limits>
+
+#include <ze/common/logging.hpp>
+#include <ze/common/types.hpp>
+#include <ze/common/time_conversions.hpp>
+#include <ze/common/timer.hpp>
+
+namespace ze {
+
+//! Benchmark utilty for unit tests. Runs a function many times and reports the
+//! minimum time. See test_benchmark.cpp for usage examples.
+template <typename Lambda>
+uint64_t runTimingBenchmark(
+    const Lambda& benchmark_fun, uint32_t num_iter_per_epoch, uint32_t num_epochs,
+    const std::string& benchmark_name = "", bool print_results = false)
+{
+  // Define lambda that runs experiment num_iter_per_epoch times and measures.
+  auto executeFunMultipleTimes = [=]() -> int64_t
+  {
+    Timer t;
+    // Measurement starts.
+    for (uint32_t i = 0; i < num_iter_per_epoch; ++i)
+    {
+      benchmark_fun();
+    }
+    // Measurement ends.
+    return t.stopAndGetNanoseconds();
+  };
+
+  uint64_t min_time = std::numeric_limits<uint64_t>::max();
+  for (uint32_t i = 0; i < num_epochs; ++i)
+  {
+    // Call function.
+    uint64_t timing = executeFunMultipleTimes();
+
+    // According to Andrei Alexandrescu, the best measure is to take the minimum.
+    // See talk: https://www.youtube.com/watch?v=vrfYLlR8X8k
+    min_time = std::min(timing, min_time);
+  }
+
+  if(print_results)
+  {
+    VLOG(1) << "Benchmark: " << benchmark_name << "\n"
+            << "> Time for " << num_iter_per_epoch << " iterations: "
+            << nanosecToMillisecTrunc(min_time) << " milliseconds\n"
+            << "> Time for 1 iteration: "
+            << nanosecToMillisecTrunc(min_time) / num_iter_per_epoch << " milliseconds";
+  }
+
+  return min_time;
+}
+
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_common/include/ze/common/buffer-inl.hpp b/RWR/src/ze_oss/ze_common/include/ze/common/buffer-inl.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..a940ce6f6860add8b3cc201a96e9610bf373195c
--- /dev/null
+++ b/RWR/src/ze_oss/ze_common/include/ze/common/buffer-inl.hpp
@@ -0,0 +1,232 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#pragma once
+
+#include <ze/common/buffer.hpp>
+
+namespace ze {
+
+template <typename Scalar, int Dim>
+std::tuple<int64_t, Eigen::Matrix<Scalar, Dim, 1>, bool>
+Buffer<Scalar,Dim>::getNearestValue(int64_t stamp)
+{
+  CHECK_GE(stamp, 0);
+
+  std::lock_guard<std::mutex> lock(mutex_);
+  if(buffer_.empty())
+  {
+    LOG(WARNING) << "Buffer is empty.";
+    return std::make_tuple(-1, Vector(), false);
+  }
+
+  auto it_before = iterator_equal_or_before(stamp);
+  if(it_before->first == stamp)
+  {
+    return std::make_tuple(it_before->first, it_before->second, true);
+  }
+
+  // Compute time difference between stamp and closest entries.
+  auto it_after = iterator_equal_or_after(stamp);
+  int64_t dt_after = -1, dt_before = -1;
+  if(it_after != buffer_.end())
+  {
+    dt_after = it_after->first - stamp;
+  }
+  if(it_before != buffer_.end())
+  {
+    dt_before = stamp - it_before->first;
+  }
+
+  // Select which entry is closest based on time difference.
+  std::pair<int64_t,Vector> result;
+  if(dt_after < 0 && dt_before < 0)
+  {
+    CHECK(false) << "Should not occur.";
+    return std::make_tuple(-1, Vector(), false);
+  }
+  else if(dt_after < 0)
+  {
+    return std::make_tuple(it_before->first, it_before->second, true);
+  }
+  else if(dt_before < 0)
+  {
+    return std::make_tuple(it_after->first, it_after->second, true);
+  }
+  else if(dt_after > 0 && dt_before > 0 && dt_after < dt_before)
+  {
+    return std::make_tuple(it_after->first, it_after->second, true);
+  }
+  return std::make_tuple(it_before->first, it_before->second, true);
+}
+
+template <typename Scalar, int Dim>
+std::pair<Eigen::Matrix<Scalar, Dim, 1>, bool> Buffer<Scalar,Dim>::getOldestValue() const
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  if(buffer_.empty())
+  {
+    return std::make_pair(Vector(), false);
+  }
+  return std::make_pair(buffer_.begin()->second, true);
+}
+
+template <typename Scalar, int Dim>
+std::pair<Eigen::Matrix<Scalar, Dim, 1>, bool> Buffer<Scalar,Dim>::getNewestValue() const
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  if(buffer_.empty())
+  {
+    return std::make_pair(Vector(), false);
+  }
+  return std::make_pair(buffer_.rbegin()->second, true);
+}
+
+template <typename Scalar, int Dim>
+std::tuple<int64_t, int64_t, bool> Buffer<Scalar,Dim>::getOldestAndNewestStamp() const
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  if(buffer_.empty())
+  {
+    return std::make_tuple(-1, -1, false);
+  }
+  return std::make_tuple(buffer_.begin()->first, buffer_.rbegin()->first, true);
+}
+
+template <typename Scalar, int Dim>
+std::pair<Eigen::Matrix<int64_t, Eigen::Dynamic, 1>, Eigen::Matrix<Scalar, Dim, Eigen::Dynamic> >
+Buffer<Scalar,Dim>::getBetweenValuesInterpolated(int64_t stamp_from, int64_t stamp_to)
+{
+  CHECK_GE(stamp_from, 0);
+  CHECK_LT(stamp_from, stamp_to);
+  Eigen::Matrix<int64_t, Eigen::Dynamic, 1> stamps;
+  Eigen::Matrix<Scalar, Dim, Eigen::Dynamic> values;
+
+  std::lock_guard<std::mutex> lock(mutex_);
+  if(buffer_.size() < 2)
+  {
+    LOG(WARNING) << "Buffer has less than 2 entries.";
+    return std::make_pair(stamps, values); // return empty means unsuccessful.
+  }
+
+  const int64_t oldest_stamp = buffer_.begin()->first;
+  const int64_t newest_stamp = buffer_.rbegin()->first;
+  if(stamp_from < oldest_stamp)
+  {
+    LOG(WARNING) << "Requests older timestamp than in buffer.";
+    return std::make_pair(stamps, values); // return empty means unsuccessful.
+  }
+  if(stamp_to > newest_stamp)
+  {
+    LOG(WARNING) << "Requests newer timestamp than in buffer.";
+    return std::make_pair(stamps, values); // return empty means unsuccessful.
+  }
+
+  auto it_from_before = iterator_equal_or_before(stamp_from);
+  auto it_to_after = iterator_equal_or_after(stamp_to);
+  CHECK(it_from_before != buffer_.end());
+  CHECK(it_to_after != buffer_.end());
+  auto it_from_after = it_from_before;
+  ++it_from_after;
+  auto it_to_before = it_to_after;
+  --it_to_before;
+  if(it_from_after == it_to_before)
+  {
+    LOG(WARNING) << "Not enough data for interpolation";
+    return std::make_pair(stamps, values); // return empty means unsuccessful.
+  }
+
+  // Count number of measurements.
+  size_t n = 0;
+  auto it = it_from_after;
+  while(it != it_to_after)
+  {
+    ++n;
+    ++it;
+  }
+  n += 2;
+
+  // Interpolate values at start and end and copy in output vector.
+  stamps.resize(n);
+  values.resize(kDim, n);
+  for(size_t i = 0; i < n; ++i)
+  {
+    if(i == 0)
+    {
+      stamps(i) = stamp_from;
+      const double w =
+          static_cast<double>(stamp_from - it_from_before->first) /
+          static_cast<double>(it_from_after->first - it_from_before->first);
+      values.col(i) = (1.0 - w) * it_from_before->second + w * it_from_after->second;
+    }
+    else if(i == n-1)
+    {
+      stamps(i) = stamp_to;
+      const double w =
+          static_cast<double>(stamp_to - it_to_before->first) /
+          static_cast<double>(it_to_after->first - it_to_before->first);
+      values.col(i) = (1.0 - w) * it_to_before->second + w * it_to_after->second;
+    }
+    else
+    {
+      stamps(i) = it_from_after->first;
+      values.col(i) = it_from_after->second;
+      ++it_from_after;
+    }
+  }
+
+  return std::make_pair(stamps, values);
+}
+
+template <typename Scalar, int Dim>
+typename Buffer<Scalar,Dim>::VectorBuffer::iterator Buffer<Scalar,Dim>::iterator_equal_or_before(int64_t stamp)
+{
+  DEBUG_CHECK(!mutex_.try_lock()) << "Call lock() before accessing data.";
+  auto it = buffer_.lower_bound(stamp);
+
+  if(it->first == stamp)
+  {
+    return it; // Return iterator to key if exact key exists.
+  }
+  if(stamp > buffer_.rbegin()->first)
+  {
+    return (--buffer_.end()); // Pointer to last value.
+  }
+  if(it == buffer_.begin())
+  {
+    return buffer_.end(); // Invalid if data before first value.
+  }
+  --it;
+  return it;
+}
+
+template <typename Scalar, int Dim>
+typename Buffer<Scalar,Dim>::VectorBuffer::iterator Buffer<Scalar,Dim>::iterator_equal_or_after(int64_t stamp)
+{
+  DEBUG_CHECK(!mutex_.try_lock()) << "Call lock() before accessing data.";
+  return buffer_.lower_bound(stamp);
+}
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_common/include/ze/common/buffer.hpp b/RWR/src/ze_oss/ze_common/include/ze/common/buffer.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..9ad24ff6843234a12f138d6d9ff129c8f9ab350d
--- /dev/null
+++ b/RWR/src/ze_oss/ze_common/include/ze/common/buffer.hpp
@@ -0,0 +1,190 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#pragma once
+
+#include <map>
+#include <tuple>
+#include <thread>
+#include <utility>
+#include <mutex>
+
+#include <ze/common/logging.hpp>
+#include <ze/common/types.hpp>
+#include <ze/common/time_conversions.hpp>
+
+namespace ze {
+
+// Oldest entry: buffer.begin(), newest entry: buffer.rbegin()
+template <typename Scalar, int Dim>
+class Buffer
+{
+public:
+  using Vector = Eigen::Matrix<Scalar, Dim, 1>;
+  using VectorBuffer = std::map<int64_t, Vector, std::less<int64_t>, Eigen::aligned_allocator<std::pair<const int64_t,Vector>>>;
+
+  static constexpr int kDim = Dim;
+
+  Buffer() = default;
+  Buffer(real_t buffer_size_seconds)
+    : buffer_size_nanosec_(secToNanosec(buffer_size_seconds))
+  {}
+
+  Buffer(const Buffer& from)
+  {
+    std::lock_guard<std::mutex>(from.mutex_);
+    if (from.buffer_.size() != 0)
+    {
+      throw std::runtime_error("Non-empty buffers are not copyable.");
+    }
+    buffer_size_nanosec_ = from.buffer_size_nanosec_;
+  }
+
+  inline void insert(int64_t stamp, const Vector& data)
+  {
+    std::lock_guard<std::mutex> lock(mutex_);
+    buffer_[stamp] = data;
+    if(buffer_size_nanosec_ > 0)
+    {
+      removeDataBeforeTimestamp_impl(
+            buffer_.rbegin()->first - buffer_size_nanosec_);
+
+    }
+  }
+
+  //! Get value with timestamp closest to stamp. Boolean in returns if successful.
+  std::tuple<int64_t, Vector, bool> getNearestValue(int64_t stamp);
+
+  //! Get oldest value in buffer.
+  std::pair<Vector, bool> getOldestValue() const;
+
+  //! Get newest value in buffer.
+  std::pair<Vector, bool> getNewestValue() const;
+
+  //! Get timestamps of newest and oldest entry.
+  std::tuple<int64_t, int64_t, bool> getOldestAndNewestStamp() const;
+
+  /*! @brief Get Values between timestamps.
+   *
+   * If timestamps are not matched, the values
+   * are interpolated. Returns a vector of timestamps and a block matrix with
+   * values as columns. Returns empty matrices if not successful.
+   */
+  std::pair<Eigen::Matrix<int64_t, Eigen::Dynamic, 1>, Eigen::Matrix<Scalar, Dim, Eigen::Dynamic> >
+  getBetweenValuesInterpolated(int64_t stamp_from, int64_t stamp_to);
+
+  inline void clear()
+  {
+    std::lock_guard<std::mutex> lock(mutex_);
+    buffer_.clear();
+  }
+
+  inline size_t size() const
+  {
+    std::lock_guard<std::mutex> lock(mutex_);
+    return buffer_.size();
+  }
+
+  inline bool empty() const
+  {
+    std::lock_guard<std::mutex> lock(mutex_);
+    return buffer_.empty();
+  }
+
+  inline void removeDataBeforeTimestamp(int64_t stamp)
+  {
+    std::lock_guard<std::mutex> lock(mutex_);
+    removeDataBeforeTimestamp_impl(stamp);
+  }
+
+  inline void removeDataOlderThan(double seconds)
+  {
+    std::lock_guard<std::mutex> lock(mutex_);
+    if(buffer_.empty())
+      return;
+
+    removeDataBeforeTimestamp_impl(
+          buffer_.rbegin()->first - secToNanosec(seconds));
+  }
+
+  inline void lock() const
+  {
+    mutex_.lock();
+  }
+
+  inline void unlock() const
+  {
+    mutex_.unlock();
+  }
+
+  const VectorBuffer& data() const
+  {
+    CHECK(!mutex_.try_lock()) << "Call lock() before accessing data.";
+    return buffer_;
+  }
+
+  typename VectorBuffer::iterator iterator_equal_or_before(int64_t stamp);
+
+  typename VectorBuffer::iterator iterator_equal_or_after(int64_t stamp);
+
+protected:
+  mutable std::mutex mutex_;
+  VectorBuffer buffer_;
+  int64_t buffer_size_nanosec_ = -1; // Negative means, no fixed size.
+
+  inline void removeDataBeforeTimestamp_impl(int64_t stamp)
+  {
+    auto it = buffer_.lower_bound(stamp);
+    buffer_.erase(buffer_.begin(), it);
+  }
+};
+
+// -----------------------------------------------------------------------------
+template<typename BuffScalar, int BuffDim>
+bool findNearestTimeStamp(Buffer<BuffScalar, BuffDim>& in_buff,
+                          const int64_t& in_ts,
+                          int64_t& out_ts,
+                          Eigen::Matrix<BuffScalar, BuffDim, 1>& out_data,
+                          real_t max_diff_secs = 0.02,
+                          real_t offset_secs = 0.0)
+{
+  bool success;
+  int64_t offset_nsecs = secToNanosec(offset_secs);
+  std::tie(out_ts, out_data, success) = in_buff.getNearestValue(in_ts+offset_nsecs);
+  if (!success)
+  {
+    return false;
+  }
+  int64_t max_diff_nsecs = secToNanosec(max_diff_secs);
+  if (std::abs(out_ts - in_ts) > max_diff_nsecs)
+  {
+    return false;
+  }
+  else return true;
+}
+
+} // namespace ze
+
+#include <ze/common/buffer-inl.hpp>
diff --git a/RWR/src/ze_oss/ze_common/include/ze/common/combinatorics.hpp b/RWR/src/ze_oss/ze_common/include/ze/common/combinatorics.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..ad6fd237e830af7c08ecfe24fb38596a1897b72f
--- /dev/null
+++ b/RWR/src/ze_oss/ze_common/include/ze/common/combinatorics.hpp
@@ -0,0 +1,157 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#pragma once
+
+#include <algorithm>
+#include <functional>
+#include <type_traits>
+#include <vector>
+
+#include <ze/common/types.hpp>
+
+//! @file combinatorics.hpp
+//! Some useful combinatorial functions. E.g. find matching index-entries in
+//! two vectors.
+
+namespace ze {
+
+// -----------------------------------------------------------------------------
+//! Returns indices (1. A, 2. B) of matching values in provided vectors.
+template<typename T>
+std::vector<std::pair<uint32_t, uint32_t>> getMatchIndices(
+    const Eigen::Ref<const Eigen::Matrix<T, Eigen::Dynamic, 1>>& A,
+    const Eigen::Ref<const Eigen::Matrix<T, Eigen::Dynamic, 1>>& B,
+    const std::function<bool (T)>& isValidId)
+{
+  // First, create a sorted vector of the reference ids with corresponding index.
+  std::vector<std::pair<T, uint32_t>> B_indexed;
+  B_indexed.reserve(B.size());
+  for(uint32_t i = 0; i < B.size(); ++i)
+  {
+    if(isValidId(B(i)))
+    {
+      B_indexed.push_back(std::make_pair(B(i), i));
+    }
+  }
+  std::sort(B_indexed.begin(), B_indexed.end(),
+             [](const std::pair<T, uint32_t>& lhs, const std::pair<T, uint32_t>& rhs)
+             { return lhs.first < rhs.first; });
+
+  // For each current id, find matching reference id.
+  std::vector<std::pair<uint32_t, uint32_t>> matches_AB;
+  matches_AB.reserve(B_indexed.size());
+  for(uint32_t i = 0; i < A.size(); ++i)
+  {
+    if(isValidId(A(i)))
+    {
+      // Efficient search for matching id in sorted range.
+      auto it = std::lower_bound(B_indexed.begin(),
+                                 B_indexed.end(), A(i),
+                                 [](const std::pair<T, uint32_t>& lhs, T rhs)
+                                 { return lhs.first < rhs; });
+      if(it != B_indexed.end() && it->first == A(i))
+      {
+        // Success.
+        matches_AB.push_back(std::make_pair(i, it->second));
+      }
+    }
+  }
+  return matches_AB;
+}
+
+// -----------------------------------------------------------------------------
+//! Returns indices of A that don't have a matching index in B.
+template<typename T>
+std::vector<uint32_t> getUnmatchedIndices(
+    const Eigen::Ref<const Eigen::Matrix<T, Eigen::Dynamic, 1>>& A,
+    const Eigen::Ref<const Eigen::Matrix<T, Eigen::Dynamic, 1>>& B,
+    const std::function<bool (T)>& isValidId)
+{
+  // First, create a sorted vector of the reference ids with corresponding index.
+  std::vector<std::pair<T, uint32_t>> B_indexed;
+  B_indexed.reserve(B.size());
+  for (uint32_t i = 0; i < B.size(); ++i)
+  {
+    if (isValidId(B(i)))
+    {
+      B_indexed.push_back(std::make_pair(B(i), i));
+    }
+  }
+  std::sort(B_indexed.begin(), B_indexed.end(),
+             [](const std::pair<T, uint32_t>& lhs, const std::pair<T, uint32_t>& rhs)
+             { return lhs.first < rhs.first; });
+
+  // For each current id, find matching reference id.
+  std::vector<uint32_t> unmached_A;
+  unmached_A.reserve(B_indexed.size());
+  for(uint32_t i = 0; i < A.size(); ++i)
+  {
+    if(isValidId(A(i)))
+    {
+      // Efficient search for matching id in sorted range.
+      auto it = std::lower_bound(B_indexed.begin(),
+                                 B_indexed.end(), A(i),
+                                 [](const std::pair<T, uint32_t>& lhs, T rhs)
+                                 { return lhs.first < rhs; });
+      if(it == B_indexed.end() || it->first != A(i))
+      {
+        // No match found:
+        unmached_A.push_back(i);
+      }
+    }
+  }
+  return unmached_A;
+}
+
+// -----------------------------------------------------------------------------
+template<typename T>
+std::vector<T> getOutlierIndicesFromInlierIndices(
+    std::vector<T>& inliers, const T size)
+{
+  static_assert(std::is_integral<T>::value, "Type T must be an integral type");
+
+  std::sort(inliers.begin(), inliers.end(), std::less<T>());
+  std::vector<T> outliers;
+  outliers.reserve(size - inliers.size());
+
+  T k = 0;
+  for (T i = 0; i < size; ++i)
+  {
+    if (k < static_cast<T>(inliers.size()) && inliers[k] < i)
+    {
+      ++k;
+    }
+
+    if (k >= size || inliers[k] != i)
+    {
+      outliers.push_back(i);
+    }
+  }
+
+  return outliers;
+}
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_common/include/ze/common/config.hpp.in b/RWR/src/ze_oss/ze_common/include/ze/common/config.hpp.in
new file mode 100644
index 0000000000000000000000000000000000000000..1a7371f3e67b0ec6248233f4dc2363064d5534ce
--- /dev/null
+++ b/RWR/src/ze_oss/ze_common/include/ze/common/config.hpp.in
@@ -0,0 +1,12 @@
+#pragma once
+
+//! @file config.hpp.in
+//! Compile-time configuration via ze_cmake/ze_options module.
+
+#cmakedefine ZE_SINGLE_PRECISION_FLOAT
+#cmakedefine ZE_USE_BRISK
+#cmakedefine ZE_USE_GTSAM
+#cmakedefine ZE_USE_ARRAYFIRE
+#cmakedefine ZE_USE_OPENCV
+#cmakedefine ZE_DETERMINISTIC
+#cmakedefine ZE_VIO_LIMITED
diff --git a/RWR/src/ze_oss/ze_common/include/ze/common/csv_trajectory.hpp b/RWR/src/ze_oss/ze_common/include/ze/common/csv_trajectory.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..f3d315851fca75b64c7abe740ed95926312a0826
--- /dev/null
+++ b/RWR/src/ze_oss/ze_common/include/ze/common/csv_trajectory.hpp
@@ -0,0 +1,112 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#pragma once
+
+#include <ze/common/buffer.hpp>
+#include <ze/common/file_utils.hpp>
+#include <ze/common/macros.hpp>
+#include <ze/common/types.hpp>
+#include <ze/common/transformation.hpp>
+
+namespace ze {
+
+//! Reading of various csv trajectory file formats (e.g. swe, euroc, pose).
+//! Reads the result in a buffer that allows accessing the pose via the
+//! timestamps.
+class CSVTrajectory
+{
+public:
+  ZE_POINTER_TYPEDEFS(CSVTrajectory);
+
+  virtual void load(const std::string& in_file_path) = 0;
+  virtual int64_t getTimeStamp(const std::string& ts_str) const;
+
+protected:
+  CSVTrajectory() = default;
+
+  void readHeader(const std::string& in_file_path);
+  Vector3 readTranslation(const std::vector<std::string>& items);
+  Vector4 readOrientation(const std::vector<std::string>& items);
+  Vector7 readPose(const std::vector<std::string>& items);
+
+  std::ifstream in_str_;
+  std::map<std::string, int> order_;
+  std::string header_;
+  const char delimiter_{','};
+  size_t num_tokens_in_line_;
+};
+
+class PositionSeries : public CSVTrajectory
+{
+public:
+  ZE_POINTER_TYPEDEFS(PositionSeries);
+
+  PositionSeries();
+  virtual void load(const std::string& in_file_path) override;
+  const Buffer<real_t, 3>& getBuffer() const;
+  Buffer<real_t, 3>& getBuffer();
+
+protected:
+  Buffer<real_t, 3> position_buf_;
+};
+
+class PoseSeries : public CSVTrajectory
+{
+public:
+  ZE_POINTER_TYPEDEFS(PoseSeries);
+
+  PoseSeries();
+
+  virtual void load(const std::string& in_file_path) override;
+  virtual const Buffer<real_t, 7>& getBuffer() const;
+  virtual Buffer<real_t, 7>& getBuffer();
+  virtual StampedTransformationVector getStampedTransformationVector();
+
+  static Transformation getTransformationFromVec7(const Vector7& data);
+
+protected:
+  Buffer<real_t, 7> pose_buf_;
+};
+
+class SWEResultSeries : public PoseSeries
+{
+public:
+  SWEResultSeries();
+};
+
+class SWEGlobalSeries : public PoseSeries
+{
+public:
+  SWEGlobalSeries();
+};
+
+class EurocResultSeries : public PoseSeries
+{
+public:
+  EurocResultSeries();
+};
+
+} // ze namespace
diff --git a/RWR/src/ze_oss/ze_common/include/ze/common/file_utils.hpp b/RWR/src/ze_oss/ze_common/include/ze/common/file_utils.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..85f7ef6d537a440268395e7add50f1bc1f74f211
--- /dev/null
+++ b/RWR/src/ze_oss/ze_common/include/ze/common/file_utils.hpp
@@ -0,0 +1,71 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#pragma once
+
+#include <fstream>
+#include <iostream>
+#include <ze/common/path_utils.hpp>
+#include <ze/common/string_utils.hpp>
+
+//! @file file_utils.hpp
+//! Utility to open read or write filestreams and do the appropriate checks.
+
+namespace ze {
+
+inline void openFileStream(
+    const std::string& filename,
+    std::ifstream* fs)
+{
+  CHECK_NOTNULL(fs);
+  CHECK(fileExists(filename)) << "File does not exist: " << filename;
+  fs->open(filename.c_str(), std::ios::in);
+  CHECK(*fs);
+  CHECK(fs->is_open()) << "Failed to open file: " << filename;
+  CHECK(!fs->eof()) << "File seems to contain no content!";
+}
+
+inline void openFileStreamAndCheckHeader(
+    const std::string& filename,
+    const std::string& header,
+    std::ifstream* fs)
+{
+  openFileStream(filename, fs);
+  std::string line;
+  std::getline(*fs, line);
+  CHECK_EQ(line, header) << "Invalid header.";
+}
+
+inline void openOutputFileStream(
+    const std::string& filename,
+    std::ofstream* fs)
+{
+  CHECK_NOTNULL(fs);
+  fs->open(filename.c_str(), std::ios::out);
+  CHECK(*fs) << "Failed to open filestream " << filename;
+  CHECK(fs->is_open()) << "Failed to open file: " << filename;
+}
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_common/include/ze/common/logging.hpp b/RWR/src/ze_oss/ze_common/include/ze/common/logging.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..6010da2ceae3400879b26a3bfa8b000428c82dbe
--- /dev/null
+++ b/RWR/src/ze_oss/ze_common/include/ze/common/logging.hpp
@@ -0,0 +1,48 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#pragma once
+
+#pragma diagnostic push
+#pragma GCC diagnostic ignored "-Wunused-local-typedefs"
+// glog has an unused typedef.
+// https://github.com/google/glog/pull/33
+#include <glog/logging.h>
+#pragma diagnostic pop
+
+//! @file logging.hpp
+//! Includes Glog framework and defines macros for DEBUG_CHECK_* which
+//! can be compiled away.
+
+#define DEBUG_CHECK(val) CHECK(val)
+#define DEBUG_CHECK_NOTNULL(val) CHECK_NOTNULL(val)
+#define DEBUG_CHECK_EQ(val1, val2) CHECK_EQ(val1, val2)
+#define DEBUG_CHECK_NE(val1, val2) CHECK_NE(val1, val2)
+#define DEBUG_CHECK_LE(val1, val2) CHECK_LE(val1, val2)
+#define DEBUG_CHECK_LT(val1, val2) CHECK_LT(val1, val2)
+#define DEBUG_CHECK_GE(val1, val2) CHECK_GE(val1, val2)
+#define DEBUG_CHECK_GT(val1, val2) CHECK_GT(val1, val2)
+#define DEBUG_CHECK_DOUBLE_EQ(val1, val2) CHECK_DOUBLE_EQ(val1, val2)
+#define DEBUG_CHECK_NEAR(val1, val2, margin) CHECK_NEAR(val1, val2, margin)
diff --git a/RWR/src/ze_oss/ze_common/include/ze/common/macros.hpp b/RWR/src/ze_oss/ze_common/include/ze/common/macros.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..68bbb3e638ce2d805d6edcb19027e7e5343b801d
--- /dev/null
+++ b/RWR/src/ze_oss/ze_common/include/ze/common/macros.hpp
@@ -0,0 +1,48 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#pragma once
+
+#include <memory>
+
+//! @file macros.hpp
+//! Common macros such as ZE_POINTER_TYPEDEFS, ZE_DELETE_COPY_ASSIGN, UNLIKELY.
+
+#define ZE_POINTER_TYPEDEFS(TypeName)               \
+  typedef std::unique_ptr<TypeName> UniquePtr;      \
+  typedef std::shared_ptr<TypeName> Ptr;            \
+  typedef std::shared_ptr<const TypeName> ConstPtr
+
+#define ZE_DELETE_COPY_ASSIGN(TypeName)             \
+  TypeName(const TypeName&) = delete;               \
+  void operator=(const TypeName&) = delete
+
+// Give the compiler a hint that an if statement is likely true or false.
+// You should use it only in cases when the likeliest branch is very very very
+// likely, or when the unlikeliest branch is very very very unlikely.
+#define UNLIKELY(x)                                 \
+  __builtin_expect((bool)(x), 0)
+#define LIKELY(x)                                   \
+  __builtin_expect((bool)(x), 1)
diff --git a/RWR/src/ze_oss/ze_common/include/ze/common/manifold.hpp b/RWR/src/ze_oss/ze_common/include/ze/common/manifold.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..fe15841eaec8f4c2e25d919f0d57e262204e2736
--- /dev/null
+++ b/RWR/src/ze_oss/ze_common/include/ze/common/manifold.hpp
@@ -0,0 +1,399 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#pragma once
+
+#include <functional>
+#include <Eigen/Dense>
+#include <ze/common/types.hpp>
+#include <ze/common/transformation.hpp>
+
+namespace ze {
+
+//! Manifold traits define a retract (boxPlus), and local (boxMinus) operation
+//! for scalars, vectors, rotations and transformations. The traits are used for
+//! simplified computation of numerical derivatives to verify the analytical ones
+//! and in the optimizer. These traits are inspired by GTSAM and for rotations
+//! and transformation we use the same convention (updates are applied on the
+//! right hand side / in the body frame).
+template<typename T> struct traits;
+
+// -----------------------------------------------------------------------------
+// Manifold traits for scalars.
+namespace internal {
+template<typename Scalar>
+struct ScalarTraits
+{
+  enum { dimension = 1 }; //! @todo(cfo): static constexpr int fails on GCC 4.8, works for Eigen because no derived class.
+  typedef Eigen::Matrix<Scalar, 1, 1> TangentVector;
+  typedef Eigen::Matrix<Scalar, 1, 1> Jacobian;
+
+  static int getDimension(const Scalar /*v*/)
+  {
+    return 1;
+  }
+
+  static bool equals(Scalar v1, Scalar v2, Scalar tol = 1e-8)
+  {
+    return std::abs(v1 - v2) < tol;
+  }
+
+  static TangentVector local(const Scalar origin, Scalar other,
+                             Jacobian* H1 = nullptr, Jacobian* H2 = nullptr)
+  {
+    if (H1)
+    {
+      (*H1)[0] = -1.0; // dlocal(origin, other) / dorigin
+    }
+    if (H2)
+    {
+      (*H2)[0] =  1.0; // dlocal(origin, other) / dother
+    }
+    TangentVector result;
+    result(0) = other - origin;
+    return result;
+  }
+
+  static Scalar retract(const Scalar origin, const TangentVector& v,
+                        Jacobian* H1 = nullptr, Jacobian* H2 = nullptr)
+  {
+    if (H1)
+    {
+      (*H1)[0] = 1.0; // dretract(origin, v) / dorigin
+    }
+    if (H2)
+    {
+      (*H2)[0] = 1.0; // dretract(origin, v) / dv
+    }
+    return origin + v[0];
+  }
+};
+} // namespace internal
+
+// Define scalar traits for float and double
+template<> struct traits<double> : public internal::ScalarTraits<double> {};
+template<> struct traits<float> : public internal::ScalarTraits<float> {};
+
+
+// -----------------------------------------------------------------------------
+// Manifold traits for fixed-size Eigen matrices and vectors with double precision.
+template<int M, int N, int Options, int MaxRows, int MaxCols>
+struct traits<Eigen::Matrix<real_t, M, N, Options, MaxRows, MaxCols> >
+{
+  //static constexpr int dimension = M * N;
+  enum { dimension = M * N };
+  typedef Eigen::Matrix<real_t, M, N, Options, MaxRows, MaxCols> Matrix;
+  typedef Eigen::Matrix<real_t, dimension, 1> TangentVector;
+  typedef Eigen::Matrix<real_t, dimension, dimension> Jacobian;
+
+  static int getDimension(const Matrix& /*v*/)
+  {
+    return M * N;
+  }
+
+  static bool equals(const Matrix& v1, const Matrix& v2, real_t tol = 1e-8)
+  {
+    if (v1.size() != v2.size())
+    {
+      return false;
+    }
+    return (v1 - v2).array().abs().maxCoeff() < tol;
+    // TODO(cfo): Check for nan entries.
+  }
+
+  static TangentVector local(
+      const Matrix& origin, Matrix other,
+      Jacobian* H1 = nullptr, Jacobian* H2 = nullptr)
+  {
+    if (H1)
+    {
+      *H1 = -Jacobian::Identity(); // dLocal(origin, other)/dOrigin
+    }
+    if (H2)
+    {
+      *H2 =  Jacobian::Identity(); // dLocal(origin, other)/dOther
+    }
+    TangentVector result;
+    Eigen::Map<Matrix>(result.data()) = other - origin;
+    return result;
+  }
+
+  static Matrix retract(
+      const Matrix& origin, const TangentVector& v,
+      Jacobian* H1 = nullptr, Jacobian* H2 = nullptr)
+  {
+    if (H1)
+    {
+      *H1 = Jacobian::Identity(); // dretract(origin, v) / dorigin
+    }
+    if (H2)
+    {
+      *H2 = Jacobian::Identity(); // dretract(origin, v) / dv
+    }
+    return origin + Eigen::Map<const Matrix>(v.data());
+  }
+};
+
+// -----------------------------------------------------------------------------
+// Manifold traits for dynamic-size Eigen matrices and vectors with double precision.
+namespace internal {
+
+// traits for dynamic Eigen matrices
+template<int M, int N, int Options, int MaxRows, int MaxCols>
+struct DynamicMatrixTraits {
+
+  typedef Eigen::Matrix<real_t, M, N, Options, MaxRows, MaxCols> DynamicMatrix;
+
+  enum Dimension : int { dimension = Eigen::Dynamic };
+
+  typedef VectorX TangentVector;
+  typedef Eigen::Matrix<real_t, dimension, dimension> Jacobian;
+  typedef DynamicMatrix ManifoldType;
+
+  static int getDimension(const DynamicMatrix& m)
+  {
+    return m.rows() * m.cols();
+  }
+
+  static Jacobian eye(const DynamicMatrix& m)
+  {
+    int dim = getDimension(m);
+    return Eigen::Matrix<real_t, dimension, dimension>::Identity(dim, dim);
+  }
+
+  static TangentVector local(
+      const DynamicMatrix& origin, const DynamicMatrix& other,
+      Jacobian* H1 = nullptr, Jacobian* H2 = nullptr)
+  {
+    if (H1)
+    {
+      *H1 = -eye(origin); // dlocal(origin, other) / dorigin
+    }
+    if (H2)
+    {
+      *H2 =  eye(origin); // dlocal(origin, other) / dother
+    }
+    TangentVector v(getDimension(origin));
+    Eigen::Map<DynamicMatrix>(v.data(), origin.rows(), origin.cols()) = other - origin;
+    return v;
+  }
+
+  static DynamicMatrix retract(
+      const DynamicMatrix& origin, const TangentVector& v,
+      Jacobian* H1 = nullptr, Jacobian* H2 = nullptr)
+  {
+    if (H1)
+    {
+      *H1 = eye(origin); // dretract(origin, v) / dorigin
+    }
+    if (H2)
+    {
+      *H2 = eye(origin); // dretract(origin, v) / dv
+    }
+    return origin + Eigen::Map<const DynamicMatrix>(v.data(), origin.rows(), origin.cols());
+  }
+};
+
+} // namespace internal
+
+// traits for fully dynamic matrix
+template<int Options, int MaxRows, int MaxCols>
+struct traits<Eigen::Matrix<real_t, Eigen::Dynamic, Eigen::Dynamic, Options, MaxRows, MaxCols> > :
+    public internal::DynamicMatrixTraits<Eigen::Dynamic, Eigen::Dynamic, Options, MaxRows, MaxCols> {
+};
+
+// traits for dynamic column vector
+template<int Options, int MaxRows, int MaxCols>
+struct traits<Eigen::Matrix<real_t, Eigen::Dynamic, 1, Options, MaxRows, MaxCols> > :
+    public internal::DynamicMatrixTraits<Eigen::Dynamic, 1, Options, MaxRows, MaxCols> {
+};
+
+// traits for dynamic row vector
+template<int Options, int MaxRows, int MaxCols>
+struct traits<Eigen::Matrix<real_t, 1, Eigen::Dynamic, Options, MaxRows, MaxCols> > :
+    public internal::DynamicMatrixTraits<1, Eigen::Dynamic, Options, MaxRows, MaxCols> {
+};
+
+// -----------------------------------------------------------------------------
+// Manifold traits for SO(3)
+template<> struct traits<Quaternion>
+{
+  enum { dimension = 3 }; // The dimension of the manifold.
+
+  typedef Eigen::Matrix<real_t, dimension, 1> TangentVector;
+  typedef Eigen::Matrix<real_t, dimension, dimension> Jacobian;
+
+  static int getDimension(const Quaternion& /*v*/)
+  {
+    return 3;
+  }
+
+  static bool equals(
+      const Quaternion& q1, const Quaternion& q2, real_t tol = 1e-8)
+  {
+    return (q1.getUnique().vector()
+            - q2.getUnique().vector()).array().abs().maxCoeff() < tol;
+  }
+
+  static TangentVector local(
+      const Quaternion& origin, const Quaternion& other,
+      Jacobian* H1 = nullptr, Jacobian* H2 = nullptr)
+  {
+    const Quaternion h = origin.inverse() * other;
+    const TangentVector v = h.log();
+    if(H1 || H2)
+    {
+      Jacobian D_v_h = logmapDerivativeSO3(v);
+      if(H1)
+      {
+        // dlocal(origin, other) / dorigin, using that Adjoint(h.inverse()) = h.inverse()
+        *H1 = - D_v_h * h.inverse().getRotationMatrix();
+      }
+      if(H2)
+      {
+        // dlocal(origin, other) / dother
+        *H2 = D_v_h;
+      }
+    }
+    return v;
+  }
+
+  static Quaternion retract(
+      const Quaternion& origin, const Vector3& v,
+      Jacobian* H1 = nullptr, Jacobian* H2 = nullptr)
+  {
+    const Quaternion g = Quaternion::exp(v);
+    const Quaternion h = origin * g;
+    if (H1)
+    {
+      // dretract(origin, v) / dorigin
+      *H1 = g.inverse().getRotationMatrix(); // Adjoint(g.inverse()) = g.inverse()
+    }
+    if (H2)
+    {
+      // dretract(origin, v) / dv
+      *H2 = expmapDerivativeSO3(v);
+    }
+    return h;
+  }
+};
+
+// -----------------------------------------------------------------------------
+// Manifold traits for SE(3)
+template<> struct traits<Transformation>
+{
+  enum { dimension = 6 }; // The dimension of the manifold.
+
+  typedef Eigen::Matrix<real_t, dimension, 1> TangentVector;
+  typedef Eigen::Matrix<real_t, dimension, dimension> Jacobian;
+
+  static int getDimension(const Transformation& /*v*/)
+  {
+    return 6;
+  }
+
+  static bool equals(
+      const Transformation& T1, const Transformation& T2, real_t tol = 1e-8)
+  {
+    return (T1.getRotation().getUnique().vector()
+            - T2.getRotation().getUnique().vector()).array().abs().maxCoeff() < tol
+        && (T1.getPosition() - T2.getPosition()).array().abs().maxCoeff() < tol;
+  }
+
+  static TangentVector local(
+      const Transformation& origin, const Transformation& other,
+      Jacobian* H1 = nullptr, Jacobian* H2 = nullptr)
+  {
+    const Transformation h = origin.inverse() * other;
+    const TangentVector v = (Vector6() << h.getPosition(), h.getRotation().log()).finished();
+    if (H1 || H2)
+    {
+      Matrix3 J_r_inv = logmapDerivativeSO3(v.tail<3>());
+      if (H1)
+      {
+        // dlocal(origin, other) / dorigin
+        H1->block<3,3>(0,0) = -I_3x3;
+        H1->block<3,3>(0,3) = skewSymmetric(v.head<3>());
+        H1->block<3,3>(3,0) = Z_3x3;
+        H1->block<3,3>(3,3) = - J_r_inv * h.getRotation().inverse().getRotationMatrix();
+      }
+      if(H2)
+      {
+        // dlocal(origin, other) / dother
+        H2->block<3,3>(0,0) = h.getRotationMatrix();
+        H2->block<3,3>(0,3) = Z_3x3;
+        H2->block<3,3>(3,0) = Z_3x3;
+        H2->block<3,3>(3,3) = J_r_inv;
+      }
+    }
+    return v;
+  }
+
+  static Transformation retract(
+      const Transformation& origin, const Vector6& v,
+      Jacobian* H1 = nullptr, Jacobian* H2 = nullptr)
+  {
+    Transformation g(Quaternion::exp(v.tail<3>()), v.head<3>());
+    Transformation h  = origin * g;
+
+    if (H1 || H2)
+    {
+      const Matrix3 R_CB = Quaternion::exp(v.tail<3>()).inverse().getRotationMatrix();
+      if (H1)
+      {
+        // dretract(origin, other) / dorigin
+
+        // Remember: Computation of the translation components must be in the
+        // body frame of the result.
+        // Retraction: T_AC = T_AB * exp(xi_BC) = T_AB * T_BC
+        // Let's look at the translation component:
+        //    A_t_AC = A_t_AB + R_AB * B_t_BC
+        // Perturb it the translation (in the body frame B):
+        //    A_t_AC = A_t_AB + (R_AB * B_dt) + R_AB * B_t_BC
+        //           = A_t_AC + (R_AB * B_dt)
+        // !!! What we want is the perturbation in the body frame of the result
+        // i.e. in the frame C:
+        //    A_t_AC = A_t_AC + (R_AB * B_dt)
+        //    A_t_AC = A_t_AC + (R_AC * R_AC^-1) * (R_AB * B_dt) <- trick
+        //           = A_t_AC + R_AC * (R_CB * B_dt)
+        //           -> Jacobian = R_CB
+        H1->block<3,3>(0,0) = R_CB;
+        H1->block<3,3>(0,3) = - R_CB * skewSymmetric(v.head<3>());
+        H1->block<3,3>(3,0) = Z_3x3;
+        H1->block<3,3>(3,3) = g.getRotation().inverse().getRotationMatrix();
+      }
+      if(H2)
+      {
+        H2->block<3,3>(0,0) = R_CB;
+        H2->block<3,3>(0,3) = Z_3x3;
+        H2->block<3,3>(3,0) = Z_3x3;
+        H2->block<3,3>(3,3) = expmapDerivativeSO3(v.tail<3>());
+      }
+    }
+    return h;
+  }
+};
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_common/include/ze/common/math.hpp b/RWR/src/ze_oss/ze_common/include/ze/common/math.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..055a2804b9394d946b76effb8da1df38e1f297bc
--- /dev/null
+++ b/RWR/src/ze_oss/ze_common/include/ze/common/math.hpp
@@ -0,0 +1,47 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#pragma once
+
+#include <ze/common/types.hpp>
+
+//! @file math.hpp
+//! Common math utilities.
+
+namespace ze {
+
+template<typename Scalar>
+constexpr Scalar radToDeg(Scalar rad)
+{
+  return rad * 180.0 / M_PI;
+}
+
+template<typename Scalar>
+constexpr Scalar degToRad(Scalar deg)
+{
+  return deg * M_PI / 180.0;
+}
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_common/include/ze/common/matrix.hpp b/RWR/src/ze_oss/ze_common/include/ze/common/matrix.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..5090ea7fdbbf100a460b5579cc23160c9d03a694
--- /dev/null
+++ b/RWR/src/ze_oss/ze_common/include/ze/common/matrix.hpp
@@ -0,0 +1,143 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#pragma once
+
+#include <algorithm>
+#include <tuple>
+#include <ze/common/logging.hpp>
+
+#include <ze/common/types.hpp>
+
+//! @file matrix.hpp
+//! Common matrix utilities.
+
+namespace ze {
+
+// ----------------------------------------------------------------------------
+//! Skew symmetric matrix.
+inline Matrix3 skewSymmetric(const real_t w1,
+                             const real_t w2,
+                             const real_t w3)
+{
+  return (Matrix3() <<
+           0.0f, -w3,  w2,
+           w3,  0.0f, -w1,
+          -w2,  w1,  0.0f).finished();
+}
+
+inline Matrix3 skewSymmetric(const Eigen::Ref<const Vector3>& w)
+{
+  return skewSymmetric(w(0), w(1), w(2));
+}
+
+// ----------------------------------------------------------------------------
+//! Normalize a block of bearing vectors.
+inline void normalizeBearings(Bearings& bearings)
+{
+  bearings = bearings.array().rowwise() / bearings.colwise().norm().array();
+}
+
+// ----------------------------------------------------------------------------
+template<typename Derived>
+Eigen::Matrix<typename Derived::Scalar, 2, 1> project2(const Eigen::MatrixBase<Derived>& v)
+{
+  EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived, 3, 1);
+  return v.template head<2>() / v(2);
+}
+
+// ----------------------------------------------------------------------------
+inline Matrix2X project2Vectorized(const Matrix3X& v)
+{
+  Matrix2X m(2, v.cols());
+  for (int i = 0; i < v.cols(); ++i)
+  {
+    m.col(i) = v.block<2,1>(0,i) / v(2,i);
+  }
+  return m;
+}
+
+// ----------------------------------------------------------------------------
+//! Get element with max norm in a vector.
+inline real_t normMax(const VectorX& v)
+{
+  return v.lpNorm<Eigen::Infinity>();
+}
+
+// ----------------------------------------------------------------------------
+//! Get element with maximum norm on diagonal.
+template<typename Derived>
+real_t maxAbsDiagonalElement(const Eigen::MatrixBase<Derived>& M)
+{
+  real_t max_val = 0.0f;
+  CHECK_EQ(M.cols(), M.rows());
+  for (int i = 0; i < M.cols(); ++i)
+  {
+    max_val = std::max(max_val, std::abs(M(i,i)));
+  }
+  return max_val;
+}
+
+// ----------------------------------------------------------------------------
+//! Direct linear transform algorithm that calls svd to find a vector v that
+//! minimizes the algebraic error A*v
+//! @param A of size m*n, where m>=n (pad with zero rows if not!)
+//! @return Rank of A, minimum error (singular value), and corresponding
+//! eigenvector (column of V, with A=U*S*V')
+std::tuple<int, real_t, VectorX> directLinearTransform(
+    const MatrixX& A, real_t rank_tol = 1e-9);
+
+// ----------------------------------------------------------------------------
+//! Get a slice of vector X by the specified indices.
+template<typename Derived>
+Eigen::Matrix<typename Derived::Scalar, Eigen::Dynamic, 1> getVectorElements(
+    Eigen::MatrixBase<Derived>& X, const std::vector<uint32_t>& indices)
+{
+  EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived);
+  const uint32_t n = indices.size();
+  Eigen::Matrix<typename Derived::Scalar, Eigen::Dynamic, 1> Y(n);
+  for (uint32_t i = 0; i < n; ++i)
+  {
+    Y(i) = X(indices[i]);
+  }
+  return Y;
+}
+
+// ----------------------------------------------------------------------------
+//! Get a slice of the matrix X by the specified column indices.
+template<typename Derived>
+Eigen::Matrix<typename Derived::Scalar, Eigen::Dynamic, Eigen::Dynamic> getMatrixCols(
+    Eigen::MatrixBase<Derived>& X, const std::vector<uint32_t>& column_indices)
+{
+  const uint32_t n_col = column_indices.size();
+  Eigen::Matrix<typename Derived::Scalar, Eigen::Dynamic, Eigen::Dynamic> Y(X.rows(), n_col);
+  for (uint32_t i = 0; i < n_col; ++i)
+  {
+    Y.col(i) = X.col(column_indices[i]);
+  }
+  return Y;
+}
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_common/include/ze/common/nonassignable.hpp b/RWR/src/ze_oss/ze_common/include/ze/common/nonassignable.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..0aff37e5f6d18686d16c8cfaf3d059555b1d2d2f
--- /dev/null
+++ b/RWR/src/ze_oss/ze_common/include/ze/common/nonassignable.hpp
@@ -0,0 +1,43 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#pragma once
+
+namespace ze {
+
+//! Denotes a class that can be copied but may not be assigned to
+//! (e.g. because of reference member variables)
+class Nonassignable
+{
+public:
+  Nonassignable() = default;
+  Nonassignable(const Nonassignable& other) = default;
+  Nonassignable(Nonassignable&& other) = default;
+  ~Nonassignable() = default;
+  Nonassignable& operator=(const Nonassignable& other) = delete;
+  Nonassignable& operator=(Nonassignable&& other) = delete;
+};
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_common/include/ze/common/noncopyable.hpp b/RWR/src/ze_oss/ze_common/include/ze/common/noncopyable.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..2d15b8842ae272eeca0dd8d74da97e59974856b4
--- /dev/null
+++ b/RWR/src/ze_oss/ze_common/include/ze/common/noncopyable.hpp
@@ -0,0 +1,41 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#pragma once
+
+namespace ze {
+
+//! Denotes a class that cannot be copied.
+//! Depending on the class semantics it may be possible to move the object.
+class Noncopyable
+{
+public:
+  Noncopyable() = default;
+  ~Noncopyable() = default;
+  Noncopyable(const Noncopyable& other) = delete;
+  Noncopyable& operator=(const Noncopyable& other) = delete;
+};
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_common/include/ze/common/numerical_derivative.hpp b/RWR/src/ze_oss/ze_common/include/ze/common/numerical_derivative.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..560b105c207fecf37ae77e4cb21324badb730373
--- /dev/null
+++ b/RWR/src/ze_oss/ze_common/include/ze/common/numerical_derivative.hpp
@@ -0,0 +1,74 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#pragma once
+
+#include <iostream>
+#include <functional>
+#include <ze/common/types.hpp>
+#include <ze/common/manifold.hpp>
+
+namespace ze {
+
+//! Template function to compute numerical derivatives. See unit tests for examples.
+//! The traits used for this functions are defined in common/manifold.h
+template<class Y, class X>
+typename Eigen::Matrix<real_t, traits<Y>::dimension, traits<X>::dimension>
+numericalDerivative(std::function<Y(const X&)> h, const X& x, real_t delta = 1e-5)
+{
+  typedef typename Eigen::Matrix<real_t, traits<Y>::dimension, traits<X>::dimension> Jacobian;
+  typedef typename traits<Y>::TangentVector TangentY;
+  typedef typename traits<X>::TangentVector TangentX;
+
+  const int N_X = traits<X>::getDimension(x);
+
+  // Get value at x.
+  Y hx = h(x);
+
+  const int N_Y = traits<Y>::getDimension(hx);
+
+  // Prepare a tangent vector to perturb x.
+  TangentX dx(N_X, 1);
+  dx.setZero();
+
+  // Compute numerical Jacobian column by column.
+  Jacobian H(N_Y, N_X);
+  H.setZero();
+
+  real_t factor = 1.0 / (2.0 * delta);
+
+  for(int i = 0; i < N_X; ++i)
+  {
+    dx(i) = delta;
+    TangentY dy1 = traits<Y>::local(hx, h(traits<X>::retract(x, dx)));
+    dx(i) = -delta;
+    TangentY dy2 = traits<Y>::local(hx, h(traits<X>::retract(x, dx)));
+    dx(i) = 0;
+    H.col(i) << (dy1 - dy2) * factor;
+  }
+  return H;
+}
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_common/include/ze/common/path_utils.hpp b/RWR/src/ze_oss/ze_common/include/ze/common/path_utils.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..a84a553b044a797e6c6bc93ac9c33bcc2d4f8db3
--- /dev/null
+++ b/RWR/src/ze_oss/ze_common/include/ze/common/path_utils.hpp
@@ -0,0 +1,77 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#pragma once
+
+#include <sys/stat.h>
+#include <string>
+#include <fstream>
+
+#include <ze/common/string_utils.hpp>
+
+//! @file path_utils.hpp
+//! Utilities to check whether a path or file exists or to join paths.
+//! @todo: replace with std/boost::filesystem.
+namespace ze {
+
+inline bool fileExists(const std::string& filename)
+{
+  std::ifstream infile(filename);
+  return infile.good();
+}
+
+inline bool isDir(const std::string& filename)
+{
+  struct stat buf;
+  if(0 == stat(filename.c_str(), &buf))
+  {
+    return S_ISDIR(buf.st_mode);
+  }
+  return false;
+}
+
+inline std::string joinPath(const std::string& s1, const std::string& s2)
+{
+  return std::string(ensureRightSlash(s1) + ensureNoLeftSlash(s2));
+}
+
+inline std::string joinPath(const std::string& s1, const std::string& s2,
+                            const std::string& s3)
+{
+  return joinPath(joinPath(s1, s2), s3);
+}
+
+inline std::string joinPath(const std::string& s1, const std::string& s2,
+                            const std::string& s3, const std::string& s4)
+{
+  return joinPath(joinPath(s1, s2), joinPath(s3, s4));
+}
+
+inline std::string getFileName(const std::string& path)
+{
+  return path.substr(path.find_last_of('/') + 1);
+}
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_common/include/ze/common/random.hpp b/RWR/src/ze_oss/ze_common/include/ze/common/random.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..739af26bce76cb0fdd263b4268573f55edc7aeb7
--- /dev/null
+++ b/RWR/src/ze_oss/ze_common/include/ze/common/random.hpp
@@ -0,0 +1,176 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#pragma once
+
+#include <random>
+#include <ze/common/logging.hpp>
+#include <ze/common/types.hpp>
+
+//! @file random.hpp
+//! Sample integer and real-valued scalars from uniform or normal distributions.
+
+namespace ze {
+
+//------------------------------------------------------------------------------
+//! @return Sample from integer-valued distribution.
+template<typename T>
+T sampleUniformIntDistribution(
+    bool deterministic = false,
+    T from = std::numeric_limits<T>::lowest(),
+    T to   = std::numeric_limits<T>::max())
+{
+  static std::mt19937 gen_nondeterministic(std::random_device{}());
+  static std::mt19937 gen_deterministic(0);
+  auto dist = std::uniform_int_distribution<T>(from, to);
+  return deterministic ? dist(gen_deterministic) : dist(gen_nondeterministic);
+}
+
+//------------------------------------------------------------------------------
+//! @return Sample from uniform real-valued distribution.
+template<typename T>
+T sampleUniformRealDistribution(
+    bool deterministic = false,
+    T from = T{0.0},
+    T to   = T{1.0})
+{
+  static std::mt19937 gen_nondeterministic(std::random_device{}());
+  static std::mt19937 gen_deterministic(0);
+  auto dist = std::uniform_real_distribution<T>(from, to);
+  return deterministic ? dist(gen_deterministic) : dist(gen_nondeterministic);
+}
+
+//------------------------------------------------------------------------------
+//! @return Sample from normal distribution (real-valued).
+template<typename T>
+T sampleNormalDistribution(
+    bool deterministic = false,
+    T mean  = T{0.0},
+    T sigma = T{1.0})
+{
+  static std::mt19937 gen_nondeterministic(std::random_device{}());
+  static std::mt19937 gen_deterministic(0);
+  auto dist = std::normal_distribution<T>(mean, sigma);
+  return deterministic ? dist(gen_deterministic) : dist(gen_nondeterministic);
+}
+
+//------------------------------------------------------------------------------
+//! @return Return true with given probability. Samples the Bernoulli distribution.
+inline bool flipCoin(
+    bool deterministic = false,
+    real_t true_probability = real_t{0.5})
+{
+  DEBUG_CHECK_GE(true_probability, 0.0);
+  DEBUG_CHECK_LT(true_probability, 1.0);
+  static std::mt19937 gen_nondeterministic(std::random_device{}());
+  static std::mt19937 gen_deterministic(0);
+  auto dist = std::bernoulli_distribution(true_probability);
+  return deterministic ? dist(gen_deterministic) : dist(gen_nondeterministic);
+}
+
+//------------------------------------------------------------------------------
+// Sample manifolds:
+
+//! @return Random 3-dimensional unit vector.
+Vector3 randomDirection3D();
+
+//! @return Random 2-dimensional unit vector.
+Vector2 randomDirection2D();
+
+// -----------------------------------------------------------------------------
+// Get distributions, only slightly faster than the above functions when many
+// random numbers are desired.
+
+//! Usage: f = uniformDistribution<int>(); sample = f();
+//! @return Uniform integer distribution in interval [from, to].
+template<class T>
+typename std::enable_if<std::is_integral<T>::value, std::function<T()> >::type
+uniformDistribution(
+    bool deterministic = false,
+    T from = std::numeric_limits<T>::lowest(),
+    T to   = std::numeric_limits<T>::max())
+{
+  static std::mt19937 gen_nondeterministic(std::random_device{}());
+  static std::mt19937 gen_deterministic(0);
+  std::uniform_int_distribution<T> distribution(from, to);
+  auto fun = deterministic ?
+               std::bind(distribution, gen_deterministic) :
+               std::bind(distribution, gen_nondeterministic);
+  return fun;
+}
+
+// -----------------------------------------------------------------------------
+//! Usage: f = uniformDistribution<double>(); sample = f();
+//! @return Uniform real-valued distribution in interval [from, to].
+template<class T>
+typename std::enable_if<!std::is_integral<T>::value, std::function<T()> >::type
+uniformDistribution(
+    bool deterministic = false,
+    T from = T{0.0},
+    T to   = T{1.0})
+{
+  static std::mt19937 gen_nondeterministic(std::random_device{}());
+  static std::mt19937 gen_deterministic(0);
+  std::uniform_real_distribution<T> distribution(from, to);
+  auto fun = deterministic ?
+               std::bind(distribution, gen_deterministic) :
+               std::bind(distribution, gen_nondeterministic);
+  return fun;
+}
+
+// -----------------------------------------------------------------------------
+//! Usage: f = uniformDistribution<double>(); sample = f();
+//! @return Uniform real-valued distribution in interval [from, to].
+template<class T>
+std::function<T()>
+normalDistribution(
+    bool deterministic = false,
+    T mean  = T{0.0},
+    T sigma = T{1.0})
+{
+  static std::mt19937 gen_nondeterministic(std::random_device{}());
+  static std::mt19937 gen_deterministic(0);
+  std::normal_distribution<T> distribution(mean, sigma);
+  auto fun = deterministic ?
+               std::bind(distribution, gen_deterministic) :
+               std::bind(distribution, gen_nondeterministic);
+  return fun;
+}
+
+// ----------------------------------------------------------------------------
+//! Bernoulli distribution, returns true with probability `true_probability` and
+//! false with probability `1-true_probability`
+inline std::function<bool()> getRandomGeneratorBinary(
+    real_t true_probability)
+{
+  CHECK_GE(true_probability, real_t{0.0});
+  CHECK_LE(true_probability, real_t{1.0});
+  std::mt19937 generator(std::random_device{}());
+  std::bernoulli_distribution distribution(true_probability);
+  std::function<bool()> fun = std::bind(distribution, generator);
+  return fun;
+}
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_common/include/ze/common/random_matrix.hpp b/RWR/src/ze_oss/ze_common/include/ze/common/random_matrix.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..f222266576a03a98a18ff62a7950a6a7e6d1299c
--- /dev/null
+++ b/RWR/src/ze_oss/ze_common/include/ze/common/random_matrix.hpp
@@ -0,0 +1,168 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#pragma once
+
+#include <ze/common/random.hpp>
+#include <ze/common/types.hpp>
+#include <ze/common/macros.hpp>
+
+//! @file random_matrix.hpp
+//! Sample matrices and vectors from uniform or normal distributions.
+
+namespace ze {
+
+//------------------------------------------------------------------------------
+//! A sampler for uncorrelated noise vectors.
+template<size_t DIM>
+class RandomVectorSampler
+{
+public:
+  ZE_POINTER_TYPEDEFS(RandomVectorSampler);
+
+  typedef Eigen::Matrix<real_t, DIM, DIM> covariance_matrix_t;
+  typedef Eigen::Matrix<real_t, DIM, 1> covariance_vector_t;
+  typedef Eigen::Matrix<real_t, DIM, 1> sigma_vector_t;
+  typedef Eigen::Matrix<real_t, DIM, 1> noise_vector_t;
+
+  //! Get a noise sample.
+  noise_vector_t sample()
+  {
+    noise_vector_t noise;
+    for (size_t i = 0; i < DIM; ++i)
+    {
+      // The gaussian takes a standard deviation as input.
+      noise(i) = sampleNormalDistribution<real_t>(deterministic_, 0.0, sigma_(i));
+    }
+    return noise;
+  }
+
+  static Ptr sigmas(const sigma_vector_t& sigmas, bool deterministic = false)
+  {
+    Ptr noise(new RandomVectorSampler(deterministic));
+    noise->sigma_ = sigmas;
+    return noise;
+  }
+
+  static Ptr variances(const covariance_vector_t& variances, bool deterministic = false)
+  {
+    Ptr noise(new RandomVectorSampler(deterministic));
+    noise->sigma_ = variances.cwiseSqrt();
+    return noise;
+  }
+
+protected:
+  RandomVectorSampler(bool deteterministic)
+    : deterministic_(deteterministic)
+  {}
+
+private:
+  const bool deterministic_;
+  sigma_vector_t sigma_;
+};
+
+//------------------------------------------------------------------------------
+inline MatrixX randomMatrixUniformDistributed(
+    int rows,
+    int cols,
+    bool deterministic = false,
+    real_t from  = 0.0,
+    real_t to    = 1.0)
+{
+  DEBUG_CHECK_GT(rows, 0);
+  DEBUG_CHECK_GT(cols, 0);
+  MatrixX m(rows, cols);
+  for (int x = 0; x < cols; ++x)
+  {
+    for (int y = 0; y < rows; ++y)
+    {
+      m(y,x) = sampleUniformRealDistribution(deterministic, from, to);
+    }
+  }
+  return m;
+}
+
+template<int rows, int cols>
+Eigen::Matrix<real_t, rows, cols>
+randomMatrixUniformDistributed(
+    bool deterministic = false,
+    real_t from = 0.0,
+    real_t to   = 1.0)
+{
+  return randomMatrixUniformDistributed(rows, cols, deterministic, from, to);
+}
+
+template<int size>
+Eigen::Matrix<real_t, size, 1>
+randomVectorUniformDistributed(
+    bool deterministic = false,
+    real_t from = 0.0,
+    real_t to   = 1.0)
+{
+  return randomMatrixUniformDistributed<size, 1>(deterministic, from, to);
+}
+
+//------------------------------------------------------------------------------
+inline MatrixX randomMatrixNormalDistributed(
+    int rows,
+    int cols,
+    bool deterministic = false,
+    real_t mean  = 0.0,
+    real_t sigma = 1.0)
+{
+  DEBUG_CHECK_GT(rows, 0);
+  DEBUG_CHECK_GT(cols, 0);
+  MatrixX m(rows, cols);
+  for (int x = 0; x < cols; ++x)
+  {
+    for (int y = 0; y < rows; ++y)
+    {
+      m(y,x) = sampleNormalDistribution(deterministic, mean, sigma);
+    }
+  }
+  return m;
+}
+
+template<int rows, int cols>
+Eigen::Matrix<real_t, rows, cols>
+randomMatrixNormalDistributed(
+    bool deterministic = false,
+    real_t mean  = 0.0,
+    real_t sigma = 1.0)
+{
+  return randomMatrixNormalDistributed(rows, cols, deterministic, mean, sigma);
+}
+
+template<int size>
+Eigen::Matrix<real_t, size, 1>
+randomVectorNormalDistributed(
+    bool deterministic = false,
+    real_t mean  = 0.0,
+    real_t sigma = 1.0)
+{
+  return randomMatrixNormalDistributed<size, 1>(deterministic, mean, sigma);
+}
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_common/include/ze/common/ring_view.hpp b/RWR/src/ze_oss/ze_common/include/ze/common/ring_view.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..6b19a70c94a1ae3f35f12b17177d1198b3f3f442
--- /dev/null
+++ b/RWR/src/ze_oss/ze_common/include/ze/common/ring_view.hpp
@@ -0,0 +1,452 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#pragma once
+
+//! A non-owning ring-buffer class.
+//! Since it's non-owning, it's not allowed to construct or destroy elements
+//! of the underlying container. Push and pop are implemented as assignment and
+//! bookkeeping, respectively.
+//!
+//! Original: https://github.com/Quuxplusone/ring_view
+//! License: MIT
+//! On the subject of "API conventions for pushing into a fixed-size container",
+//! see http://www.open-std.org/jtc1/sc22/wg21/docs/papers/2015/n4416.pdf
+
+#include <cassert>
+#include <cstddef>
+#include <iterator>
+#include <type_traits>
+#include <utility>
+// detail
+namespace detail {
+template<class, bool> class ring_view_iterator;
+} // namespace detail
+
+template<class T>
+struct null_popper
+{
+    void operator()(T&) { }
+};
+
+template<class T>
+struct move_popper
+{
+    T operator()(T& t) { return std::move(t); }
+};
+
+// Will come with c++14
+namespace std {
+
+template< bool B, class T, class F >
+using conditional_t = typename std::conditional<B,T,F>::type;
+
+template< bool B, class T = void >
+using enable_if_t = typename enable_if<B,T>::type;
+
+} // namespace std
+
+template<bool B>
+using EnableIfB = typename std::enable_if<B, int>::type;
+
+template<class T, size_t Capacity = 0, class Popper = move_popper<T>>
+class ring_view
+{
+public:
+  using type = ring_view<T, Capacity, Popper>;
+  using value_type = T;
+  using pointer = T*;
+  using reference = T&;
+  using const_reference = const T&;
+  using size_type = std::size_t;
+  using iterator = detail::ring_view_iterator<ring_view, false>;  // exposition only
+  using const_iterator = detail::ring_view_iterator<ring_view, true>;  // exposition only
+  using reverse_iterator = std::reverse_iterator<iterator>;
+  using const_reverse_iterator = std::reverse_iterator<const_iterator>;
+
+  ring_view() = default;
+
+  // Dynamic Size Capacity Constructors
+  // Construct a full ring_view.
+  template<class ContiguousIterator, size_t C1 = Capacity, EnableIfB<C1 == 0> = 0>
+  ring_view(ContiguousIterator begin,
+            ContiguousIterator end,
+            Popper p = Popper()) noexcept :
+    data_(&*begin),
+    size_(end - begin),
+    capacity_(end - begin),
+    front_idx_(0),
+    popper_(std::move(p))
+  {}
+
+  // Construct a "partially full" ring_view.
+  template<class ContiguousIterator, size_t C1 = Capacity, EnableIfB<C1 == 0> = 0>
+  ring_view(ContiguousIterator begin,
+            ContiguousIterator end,
+            ContiguousIterator first,
+            size_type size,
+            Popper p = Popper()) noexcept :
+    data_(&*begin),
+    size_(size),
+    capacity_(end - begin),
+    front_idx_(first - begin),
+    popper_(std::move(p))
+  {}
+
+  // Fixed Size Capacity Constructors
+  template<class ContiguousIterator, size_t C1 = Capacity, EnableIfB<C1 != 0> = 0>
+  ring_view(ContiguousIterator begin,
+            ContiguousIterator end,
+            Popper p = Popper()) noexcept :
+    data_(&*begin),
+    size_(end - begin),
+    front_idx_(0),
+    popper_(std::move(p))
+  {
+    CHECK((end - begin) == Capacity) << "Fixed size capacity must match data structure capacity.";
+  }
+
+  // Construct a "partially full" ring_view.
+  template<class ContiguousIterator, size_t C1 = Capacity, EnableIfB<C1 != 0> = 0>
+  ring_view(ContiguousIterator begin,
+            ContiguousIterator end,
+            ContiguousIterator first,
+            size_type size,
+            Popper p = Popper()) noexcept :
+    data_(&*begin),
+    size_(size),
+    front_idx_(first - begin),
+    popper_(std::move(p))
+  {
+    CHECK((end - begin) == Capacity) << "Fixed size capacity must match data structure capacity.";
+  }
+
+  // Notice that an iterator contains a pointer to the ring_view itself.
+  // Destroying ring_view rv invalidates rv.begin(), just as with an owning
+  // container.
+  iterator begin() noexcept { return iterator(0, this); }
+  iterator end() noexcept { return iterator(size(), this); }
+  const_iterator begin() const noexcept { return cbegin(); }
+  const_iterator end() const noexcept { return cend(); }
+  const_iterator cbegin() const noexcept { return const_iterator(0, this); }
+  const_iterator cend() const noexcept { return const_iterator(size(), this); }
+
+  reverse_iterator rbegin() noexcept { return reverse_iterator(end()); }
+  reverse_iterator rend() noexcept { return reverse_iterator(begin()); }
+  const_reverse_iterator rbegin() const noexcept { return crbegin(); }
+  const_reverse_iterator rend() const noexcept { return crend(); }
+  const_reverse_iterator crbegin() const noexcept
+  {
+    return const_reverse_iterator(cend());
+  }
+  const_reverse_iterator crend() const noexcept
+  {
+    return const_reverse_iterator(cbegin());
+  }
+
+  reference front() noexcept { return *begin(); }
+  reference back() noexcept { return *(end() - 1); }
+  const_reference front() const noexcept { return *begin(); }
+  const_reference back() const noexcept { return *(end() - 1); }
+
+  // not in the spec:
+  // reset the view to empty starting conditions.
+  // Does not clear any data
+  void reset(size_type front_idx = 0, size_type size = 0) noexcept
+  {
+    front_idx_ = front_idx;
+    size_ = size;
+  }
+  void reset_front(size_type front_idx)
+  {
+    size_ = size_ + (front_idx_ - front_idx);
+    front_idx_ = front_idx;
+  }
+  void reset_size(size_type size_)
+  {
+    size_ = size;
+  }
+
+  // state indicators
+  bool empty() const noexcept { return size_ == 0; }
+  bool full() const noexcept { return size_ == capacity_; }
+  size_type size() const noexcept { return size_; }
+  size_type capacity() const noexcept { return capacity_; }
+
+  // pop_front() increments the index of the begin of the ring.
+  // Notice that it does not destroy anything.
+  // Calling pop_front() on an empty ring is undefined,
+  // in the same way as calling pop_front() on an empty vector or list is
+  // undefined.
+  // Without pop_front(), you can't use ring_view as a std::queue.
+  void pop_front()
+  {
+    assert(not empty());
+    auto& elt = front_();
+    increment_front_();
+    popper_(elt);
+  }
+
+  void pop_back()
+  {
+    assert(not empty());
+    auto& elt = back_();
+    decrement_back_();
+    return popper_(elt);
+  }
+
+  // push_back() assigns a new value to the element
+  // at the end of the ring, and makes that element the
+  // new back of the ring. If the ring is full before
+  // the call to push_back(), we rotate the indices and
+  // invalidate all iterators into the ring.
+  // Without push_back(), you can't use ring_view as a std::queue.
+  template<bool b=true,
+           typename=std::enable_if_t<b && std::is_copy_assignable<T>::value>>
+  void push_back(const T& value)
+    noexcept(std::is_nothrow_copy_assignable<T>::value)
+  {
+    if (full()) {
+      increment_front_and_back_();
+    } else {
+      increment_back_();
+    }
+    back_() = value;
+  }
+
+  template<bool b=true,
+           typename=std::enable_if_t<b && std::is_move_assignable<T>::value>>
+  void push_back(T&& value) noexcept(std::is_nothrow_move_assignable<T>::value)
+  {
+    if (full())
+    {
+        increment_front_and_back_();
+    }
+    else
+    {
+        increment_back_();
+    }
+    back_() = std::move(value);
+  }
+
+  void swap(ring_view& rhs) /*noexcept(std::__is_nothrow_swappable<Popper>::value) C++14 */
+  {
+    using std::swap;
+    swap(data_, rhs.data_);
+    swap(size_, rhs.size_);
+    swap(capacity_, rhs.capacity_);
+    swap(front_idx_, rhs.front_idx_);
+    swap(popper_, rhs.popper_);
+  }
+
+  friend void swap(ring_view& lhs, ring_view& rhs)
+  noexcept(noexcept(lhs.swap(rhs)))
+  {
+    lhs.swap(rhs);
+  }
+
+  // not public in proposal:
+  // Access the i'th element of the ring, not of the underlying datastructure
+  reference at(size_type i) noexcept
+  {
+    return data_[(front_idx_ + i) % capacity_];
+  }
+  const_reference at(size_type i) const noexcept
+  {
+    return data_[(front_idx_ + i) % capacity_];
+  }
+
+  // get the index after the last element of the buffer
+  size_type back_idx() const noexcept
+  {
+    return (front_idx_ + size_ - 1) % capacity_;
+  }
+
+  //! get the index in the datastructure given the index of in the ring
+  //! this function is not in the proposal.
+  inline size_type container_idx(size_type idx) const noexcept
+  {
+    return (front_idx_ + idx) % capacity_;
+  }
+
+private:
+  friend class detail::ring_view_iterator<ring_view, true>;
+  friend class detail::ring_view_iterator<ring_view, false>;
+
+  reference front_() noexcept { return *(data_ + front_idx_); }
+  const_reference front_() const noexcept { return *(data_ + front_idx_); }
+  reference back_() noexcept
+  {
+    return *(data_ + (front_idx_ + size_ - 1) % capacity_);
+  }
+  const_reference back_() const noexcept {
+    return *(data_ + (front_idx_ + size_ - 1) % capacity_);
+  }
+
+  void increment_front_() noexcept
+  {
+    front_idx_ = (front_idx_ + 1) % capacity_;
+    --size_;
+  }
+
+  void increment_back_() noexcept
+  {
+    ++size_;
+  }
+  void decrement_back_() noexcept
+  {
+    --size_;
+  }
+
+  void increment_front_and_back_() noexcept
+  {
+      front_idx_ = (front_idx_ + 1) % capacity_;
+  }
+
+  T *data_;
+  size_type size_;
+  std::conditional_t<Capacity == 0, const size_type, size_type> capacity_ =
+      Capacity;
+  size_type front_idx_;
+  Popper popper_;
+};
+
+namespace detail {
+
+//! the Iterator is 0-indexed, thus, relative to the starting point
+//! of the ring.
+template<class RV, bool is_const>
+class ring_view_iterator
+{
+public:
+  using type = ring_view_iterator<RV, is_const>;
+  using size_type = typename RV::size_type;
+
+  using value_type = typename RV::value_type;
+  using difference_type = std::ptrdiff_t;
+  using pointer = typename std::conditional_t<is_const,
+                                              const value_type, value_type>*;
+  using reference = typename std::conditional_t<is_const,
+                                                const value_type, value_type>&;
+  using iterator_category = std::random_access_iterator_tag;
+
+  ring_view_iterator() = default;
+
+  // not in spec:
+  // the index of the iterator in the container (this is relative to the
+  // starting point of the ring). Not the index in the ring
+  size_type index() const { return idx_; }
+  // get the index in the underlying container
+  size_type container_index() const {return rv_->container_idx(idx_); }
+
+  reference operator*() const noexcept { return rv_->at(idx_); }
+  ring_view_iterator& operator++() noexcept { ++idx_; return *this; }
+  ring_view_iterator operator++(int) noexcept
+  {
+    auto r(*this); ++*this; return r;
+  }
+  ring_view_iterator& operator--() noexcept { --idx_; return *this; }
+  ring_view_iterator operator--(int) noexcept
+  {
+    auto r(*this); --*this; return r;
+  }
+
+  friend ring_view_iterator& operator+=(ring_view_iterator& it, int i) noexcept
+  {
+    it.idx_ += i; return it;
+  }
+  friend ring_view_iterator& operator-=(ring_view_iterator& it, int i) noexcept
+  {
+    it.idx_ -= i; return it;
+  }
+
+  friend ring_view_iterator& operator+=(ring_view_iterator& it,
+                                        ring_view_iterator& it2) noexcept
+  {
+    it.idx_ += it2.idx_; return it;
+  }
+  friend ring_view_iterator& operator-=(ring_view_iterator& it,
+                                        ring_view_iterator& it2) noexcept
+  {
+    it.idx_ -= it2.idx_; return it;
+  }
+
+
+  friend ring_view_iterator operator+(ring_view_iterator it, int i) noexcept
+  {
+    it += i; return it;
+  }
+  friend ring_view_iterator operator-(ring_view_iterator it, int i) noexcept
+  {
+    it -= i; return it;
+  }
+
+  template<bool C>
+  bool operator==(const ring_view_iterator<RV,C>& rhs) const noexcept
+  {
+    return idx_ == rhs.idx_;
+  }
+  template<bool C>
+  bool operator!=(const ring_view_iterator<RV,C>& rhs) const noexcept
+  {
+    return idx_ != rhs.idx_;
+  }
+  template<bool C>
+  bool operator<(const ring_view_iterator<RV,C>& rhs) const noexcept
+  {
+    return idx_ < rhs.idx_;
+  }
+  template<bool C>
+  bool operator<=(const ring_view_iterator<RV,C>& rhs) const noexcept
+  {
+    return idx_ <= rhs.idx_;
+  }
+  template<bool C>
+  bool operator>(const ring_view_iterator<RV,C>& rhs) const noexcept
+  {
+    return idx_ > rhs.idx_;
+  }
+  template<bool C>
+  bool operator>=(const ring_view_iterator<RV,C>& rhs) const noexcept
+  {
+    return idx_ >= rhs.idx_;
+  }
+
+private:
+  friend RV;
+  // standard proposal does not declare these:
+  // allows us to apply operates across const_iterator and iterator
+  friend class ring_view_iterator<RV, true>;
+  friend class ring_view_iterator<RV, false>;
+
+  ring_view_iterator(size_type idx,
+                     std::conditional_t<is_const, const RV, RV> *rv) noexcept
+    : idx_(idx), rv_(rv) {}
+
+  size_type idx_;
+  std::conditional_t<is_const, const RV, RV> *rv_;
+};
+
+} // namespace detail
diff --git a/RWR/src/ze_oss/ze_common/include/ze/common/ringbuffer-inl.hpp b/RWR/src/ze_oss/ze_common/include/ze/common/ringbuffer-inl.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..f5bca1e08d4598bbcdf8880dc46dac89f720bb77
--- /dev/null
+++ b/RWR/src/ze_oss/ze_common/include/ze/common/ringbuffer-inl.hpp
@@ -0,0 +1,377 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#pragma once
+
+namespace ze {
+
+template <typename Scalar, size_t ValueDim, size_t Size>
+typename Ringbuffer<Scalar, ValueDim, Size>::TimeDataBoolTuple
+Ringbuffer<Scalar, ValueDim, Size>::getNearestValue(time_t stamp)
+{
+  CHECK_GE(stamp, 0u);
+
+  std::lock_guard<std::mutex> lock(mutex_);
+
+  if(times_.empty())
+  {
+    LOG(WARNING) << "Buffer is empty.";
+    return std::make_tuple(-1, DataType(), false);
+  }
+
+  auto it_before = iterator_equal_or_before(stamp);
+  //! @todo an approx equality could return the desired result immediately
+  if(*it_before == stamp)
+  {
+    return std::make_tuple(*it_before, dataAtTimeIterator(it_before), true);
+  }
+
+  auto it_after = iterator_equal_or_after(stamp);
+  //! @todo an approx equality could return the desired result immediately
+  if(*it_after == stamp)
+  {
+    return std::make_tuple(*it_after, dataAtTimeIterator(it_after), true);
+  }
+
+  // Compute time difference between stamp and closest entries.
+  time_t dt_after = -1, dt_before = -1;
+  if(it_after != times_.end())
+  {
+    dt_after = *it_after - stamp;
+  }
+  if(it_before != times_.end())
+  {
+    dt_before = stamp - *it_before;
+  }
+
+  // Select which entry is closest based on time difference.
+  if(dt_after < 0 && dt_before < 0)
+  {
+    CHECK(false) << "Should not occur.";
+    return std::make_tuple(-1, DataType(), false);
+  }
+  else if(dt_after < 0)
+  {
+    return std::make_tuple(*it_before, dataAtTimeIterator(it_before), true);
+  }
+  else if(dt_before < 0)
+  {
+    return std::make_tuple(*it_after, dataAtTimeIterator(it_after), true);
+  }
+  else if(dt_after > 0 && dt_before > 0 && dt_after < dt_before)
+  {
+    return std::make_tuple(*it_after, dataAtTimeIterator(it_after), true);
+  }
+
+  return std::make_tuple(*it_before, dataAtTimeIterator(it_before), true);
+}
+
+template <typename Scalar, size_t ValueDim, size_t Size>
+typename Ringbuffer<Scalar, ValueDim, Size>::DataBoolPair
+Ringbuffer<Scalar, ValueDim, Size>::getOldestValue() const
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  if(times_.empty())
+  {
+    return std::make_pair(DataType(), false);
+  }
+  return std::make_pair(dataAtTimeIterator(times_.begin()), true);
+}
+
+template <typename Scalar, size_t ValueDim, size_t Size>
+typename Ringbuffer<Scalar, ValueDim, Size>::DataBoolPair
+Ringbuffer<Scalar, ValueDim, Size>::getNewestValue() const
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  if(times_.empty())
+  {
+    return std::make_pair(DataType(), false);
+  }
+  return std::make_pair(dataAtTimeIterator((times_.end()-1)), true);
+}
+
+template <typename Scalar, size_t ValueDim, size_t Size>
+std::tuple<int64_t, int64_t, bool>
+Ringbuffer<Scalar, ValueDim, Size>::getOldestAndNewestStamp() const
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  if(times_.empty())
+  {
+    return std::make_tuple(-1, -1, false);
+  }
+  return std::make_tuple(times_.front(), times_.back(), true);
+}
+
+template <typename Scalar, size_t ValueDim, size_t Size>
+template <typename Interpolator>
+typename Ringbuffer<Scalar, ValueDim, Size>::TimeDataRangePair
+Ringbuffer<Scalar, ValueDim, Size>::getBetweenValuesInterpolated(
+    time_t stamp_from,
+    time_t stamp_to)
+{
+  CHECK_GE(stamp_from, 0u);
+  CHECK_LT(stamp_from, stamp_to);
+  times_dynamic_t stamps;
+  data_dynamic_t values;
+
+  std::lock_guard<std::mutex> lock(mutex_);
+  if(times_.size() < 2)
+  {
+    LOG(WARNING) << "Buffer has less than 2 entries.";
+    return std::make_pair(stamps, values); // return empty means unsuccessful.
+  }
+
+  const time_t oldest_stamp = times_.front();
+  const time_t newest_stamp = times_.back();
+  if(stamp_from < oldest_stamp)
+  {
+    LOG(WARNING) << "Requests older timestamp than in buffer.";
+    return std::make_pair(stamps, values); // return empty means unsuccessful.
+  }
+  if(stamp_to > newest_stamp)
+  {
+    LOG(WARNING) << "Requests newer timestamp than in buffer.";
+    return std::make_pair(stamps, values); // return empty means unsuccessful.
+  }
+
+  auto it_from_before = iterator_equal_or_before(stamp_from);
+  auto it_to_after = iterator_equal_or_after(stamp_to);
+  CHECK(it_from_before != times_.end());
+  CHECK(it_to_after != times_.end());
+  auto it_from_after = it_from_before + 1;
+  auto it_to_before = it_to_after - 1;
+  if(it_from_after == it_to_before)
+  {
+    LOG(WARNING) << "Not enough data for interpolation";
+    return std::make_pair(stamps, values); // return empty means unsuccessful.
+  }
+
+  // resize containers
+  size_t range = it_to_before.index() - it_from_after.index() + 3;
+  stamps.resize(range);
+  values.resize(ValueDim, range);
+
+  // first element interpolated:
+  stamps(0) = stamp_from;
+  values.col(0) = Interpolator::interpolate(this, stamp_from, it_from_before);
+
+  // this is a real edge case where we hit the two consecutive timestamps
+  //  with from and to.
+  if (range > 2)
+  {
+    // will we cross the boundaries of the ringbuffer?
+    if (it_to_before.container_index() < it_from_after.container_index())
+    {
+      // first batch at end of data structure
+      size_t end_block_size = times_raw_.size() - it_from_after.container_index();
+      stamps.segment(1, end_block_size) = times_raw_.segment(
+                                            it_from_after.container_index(),
+                                            end_block_size);
+
+      values.middleCols(1, end_block_size) =
+          data_.middleCols(it_from_after.container_index(), end_block_size);
+      // second batch at beginning
+      size_t begin_block_size = range - 2 - end_block_size;
+      stamps.segment(end_block_size + 1, begin_block_size) =
+          times_raw_.segment(0, begin_block_size);
+
+      values.middleCols(end_block_size + 1, begin_block_size) =
+          data_.middleCols(0, begin_block_size);
+    }
+    // copyable in a single block
+    else
+    {
+      stamps.segment(1, range - 2) = times_raw_.segment(
+                                       it_from_after.container_index(),
+                                       range - 2);
+
+      values.middleCols(1, range - 2) = data_.middleCols(
+                                                  it_from_after.container_index(),
+                                                  range - 2);
+    }
+  }
+
+  // last element interpolated
+  stamps(range - 1) = stamp_to;
+
+  values.col(range - 1) = Interpolator::interpolate(this, stamp_to, it_to_before);
+
+  return std::make_pair(stamps, values);
+}
+
+template <typename Scalar, size_t ValueDim, size_t Size>
+template <typename Interpolator>
+typename Ringbuffer<Scalar, ValueDim, Size>::data_dynamic_t
+Ringbuffer<Scalar, ValueDim, Size>::getValuesInterpolated(
+    times_dynamic_t stamps)
+{
+  CHECK_GT(stamps.size(), 0);
+
+  std::lock_guard<std::mutex> lock(mutex_);
+  time_t oldest_time = times_.front();
+  time_t newest_time = times_.back();
+
+  data_dynamic_t values(ValueDim, stamps.size());
+
+  // Starting point
+  auto it_before = iterator_equal_or_before(stamps(0));
+  values.col(0) = Interpolator::interpolate(this, stamps(0), it_before);
+
+  for (int i = 1; i < stamps.size(); ++i)
+  {
+    // ensure that we stay within the bounds of the buffer
+    CHECK_LT(stamps(i), newest_time);
+    CHECK_GT(stamps(i), oldest_time);
+
+    // advance to next value
+    while (*(it_before + 1) < stamps(i))
+    {
+      ++it_before;
+    }
+
+    values.col(i) = Interpolator::interpolate(this, stamps(i), it_before);
+  }
+
+  return values;
+}
+
+template <typename Scalar, size_t ValueDim, size_t Size>
+template <typename Interpolator>
+bool Ringbuffer<Scalar, ValueDim, Size>::getValueInterpolated(
+    time_t stamp,
+    Eigen::Ref<typename Ringbuffer<Scalar, ValueDim, Size>::data_dynamic_t> out)
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+
+  if (stamp > times_.back())
+  {
+    return false;
+  }
+
+  // Starting point
+  auto it_before = iterator_equal_or_before(stamp);
+  if (it_before == times_.end())
+  {
+    return false;
+  }
+
+  out = Interpolator::interpolate(this, stamp, it_before);
+
+  return true;
+}
+
+template <typename Scalar, size_t ValueDim, size_t Size>
+typename Ringbuffer<Scalar, ValueDim, Size>::timering_t::iterator
+Ringbuffer<Scalar, ValueDim, Size>::iterator_equal_or_before(time_t stamp)
+{
+  DEBUG_CHECK(!mutex_.try_lock()) << "Call lock() before accessing data.";
+  auto it = lower_bound(stamp);
+
+  if(*it == stamp)
+  {
+    return it; // Return iterator to key if exact key exists.
+  }
+  if(stamp > times_.back())
+  {
+    return (--times_.end()); // Pointer to last value.
+  }
+  if(it == times_.begin())
+  {
+    return times_.end(); // Invalid if data before first value.
+  }
+  --it;
+  return it;
+}
+
+template <typename Scalar, size_t ValueDim, size_t Size>
+typename Ringbuffer<Scalar, ValueDim, Size>::timering_t::iterator
+Ringbuffer<Scalar, ValueDim, Size>::iterator_equal_or_after(time_t stamp)
+{
+  DEBUG_CHECK(!mutex_.try_lock()) << "Call lock() before accessing data.";
+  return lower_bound(stamp);
+}
+
+template <typename Scalar, size_t ValueDim, size_t Size>
+typename Ringbuffer<Scalar, ValueDim, Size>::timering_t::iterator
+Ringbuffer<Scalar, ValueDim, Size>::lower_bound(time_t stamp)
+{
+  // implements a heuristic that assumes approx. equally spaced timestamps
+  time_t time_range = times_.back() - times_.front();
+
+  // stamp is out of range
+  if (stamp > times_.back())
+  {
+    return times_.end();
+  }
+
+  if (stamp < times_.front())
+  {
+    return times_.begin();
+  }
+
+  size_t offset = times_.size() * (stamp - times_.front()) / time_range;
+  // make sure we stay within the bounds
+  if (offset >= times_.size())
+  {
+    offset = times_.size() - 1;
+  }
+  typename timering_t::iterator candidate = times_.begin() + offset;
+
+  if (*candidate == stamp)
+  {
+    return candidate;
+  }
+
+  // iterate backwards
+  if (*candidate > stamp)
+  {
+    while (candidate > times_.begin())
+    {
+      if (*(candidate - 1) < stamp)
+      {
+        return candidate;
+      }
+      candidate = candidate - 1;
+    }
+    // no match
+  }
+  else
+  {
+    while (candidate < times_.end() - 1)
+    {
+      candidate++;
+      if (*candidate >= stamp)
+      {
+        return candidate;
+      }
+    }
+    // no match
+  }
+
+  // no match points to end
+  return times_.end();
+}
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_common/include/ze/common/ringbuffer.hpp b/RWR/src/ze_oss/ze_common/include/ze/common/ringbuffer.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..50db783e21b99550fa09f93b00936396feab61e9
--- /dev/null
+++ b/RWR/src/ze_oss/ze_common/include/ze/common/ringbuffer.hpp
@@ -0,0 +1,310 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#pragma once
+
+#include <map>
+#include <tuple>
+#include <thread>
+#include <utility>
+#include <mutex>
+#include <Eigen/Dense>
+#include <ze/common/logging.hpp>
+#include <ze/common/ring_view.hpp>
+
+#include <ze/common/types.hpp>
+#include <ze/common/time_conversions.hpp>
+
+namespace ze {
+
+//! @todo: move the interpolators somewhere where they make more sense?
+//!
+//! Interpolators have to implement:
+//! _ interpolate(Ringbuffer<...>*, int64_t time, Ringbuffer<...>timering_t::iterator);
+//! Passing the (optional) interator to the timestamp right before the to be
+//! interpolated value speeds up the process.
+//! The passed it_before is expected to be valid.
+//!
+//! A nearest neighbour "interpolator".
+struct InterpolatorNearest
+{
+  template<typename Ringbuffer_T>
+  static typename Ringbuffer_T::DataType interpolate(
+      Ringbuffer_T* buffer,
+      int64_t time,
+      typename Ringbuffer_T::timering_t::iterator it_before)
+  {
+    // the end value
+    auto it_after = it_before + 1;
+    if (it_after == buffer->times_.end())
+    {
+      LOG(WARNING) << "Interpolation hit end of buffer.";
+      return buffer->dataAtTimeIterator(it_before);
+    }
+
+    // The times are ordered, we can guarantee those differences to be positive
+    if ((time - *it_before) < (*it_after - time))
+    {
+      return buffer->dataAtTimeIterator(it_before);
+    }
+    return buffer->dataAtTimeIterator(it_after);
+  }
+
+  template<typename Ringbuffer_T>
+  static typename Ringbuffer_T::DataType interpolate(
+      Ringbuffer_T* buffer,
+      int64_t time)
+  {
+    auto it_before = buffer->iterator_equal_or_before(time);
+    // caller should check the bounds:
+    CHECK(it_before != buffer->times_.end());
+
+    return interpolate(buffer, time, it_before);
+  }
+};
+
+//! A simple linear interpolator
+struct InterpolatorLinear
+{
+  template<typename Ringbuffer_T>
+  static typename Ringbuffer_T::DataType interpolate(
+      Ringbuffer_T* buffer,
+      int64_t time,
+      typename Ringbuffer_T::timering_t::iterator it_before)
+  {
+    // the end value
+    auto it_after = it_before + 1;
+    if (it_after == buffer->times_.end())
+    {
+      LOG(WARNING) << "Interpolation hit end of buffer.";
+      return buffer->dataAtTimeIterator(it_before);
+    }
+
+    const real_t w1 =
+        static_cast<real_t>(time - *it_before) /
+        static_cast<real_t>(*it_after - *it_before);
+
+    return (real_t{1.0} - w1) * buffer->dataAtTimeIterator(it_before)
+        + w1 * buffer->dataAtTimeIterator(it_after);
+  }
+
+  template<typename Ringbuffer_T>
+  static typename Ringbuffer_T::DataType interpolate(
+      Ringbuffer_T* buffer,
+      int64_t time)
+  {
+    auto it_before = buffer->iterator_equal_or_before(time);
+    // caller should check the bounds:
+    CHECK(it_before != buffer->times_.end());
+
+    return interpolate(buffer, time, it_before);
+  }
+};
+using DefaultInterpolator = InterpolatorLinear;
+
+
+//! A fixed size timed buffer templated on the number of entries.
+//! Opposed to the `Buffer`, values are expected to be received ORDERED in
+//! TIME!
+// Oldest entry: buffer.begin(), newest entry: buffer.rbegin()
+template <typename Scalar, size_t ValueDim, size_t Size>
+class Ringbuffer
+{
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  //! Ringbuffer is friend with the interpolator types.
+  friend struct InterpolatorNearest;
+  friend struct InterpolatorLinear;
+
+  typedef int64_t time_t;
+  typedef Eigen::Matrix<time_t, Size, 1> times_t;
+  typedef Eigen::Matrix<time_t, Eigen::Dynamic, 1> times_dynamic_t;
+  typedef Eigen::Matrix<Scalar, ValueDim, Size> data_t;
+  typedef Eigen::Matrix<Scalar, ValueDim, Eigen::Dynamic> data_dynamic_t;
+
+  // time ring is used to keep track of the positions of the data
+  // in the dataring
+  // uses fixed size ring_view
+  typedef ring_view<time_t> timering_t;
+
+  using DataType = Eigen::Matrix<Scalar, ValueDim, 1>;
+  using DataTypeMap = Eigen::Map<DataType>;
+
+  // a series of return types
+  using DataBoolPair = std::pair<DataType, bool>;
+  using TimeDataBoolTuple = std::tuple<time_t, DataType, bool>;
+  using TimeDataRangePair = std::pair<times_dynamic_t, data_dynamic_t>;
+
+  Ringbuffer()
+    : times_(timering_t(times_raw_.data(),
+                        times_raw_.data() + Size,
+                        times_raw_.data(),
+                        0))
+  {}
+
+  //! no copy, no move as there is no way to track the mutex
+  Ringbuffer(const Ringbuffer& from) = delete;
+  Ringbuffer(const Ringbuffer&& from) = delete;
+
+  inline void insert(time_t stamp,
+                     const DataType& data)
+  {
+    std::lock_guard<std::mutex> lock(mutex_);
+    times_.push_back(stamp);
+    data_.col(times_.back_idx()) = data;
+  }
+
+  //! Get value with timestamp closest to stamp. Boolean returns if successful.
+  std::tuple<time_t, DataType, bool> getNearestValue(time_t stamp);
+
+  //! Get oldest value in buffer.
+  std::pair<DataType, bool> getOldestValue() const;
+
+  //! Get newest value in buffer.
+  std::pair<DataType, bool> getNewestValue() const;
+
+  //! Get timestamps of newest and oldest entry.
+  std::tuple<time_t, time_t, bool> getOldestAndNewestStamp() const;
+
+  /*! @brief Get Values between timestamps.
+   *
+   * If timestamps are not matched, the values
+   * are interpolated. Returns a vector of timestamps and a block matrix with
+   * values as columns. Returns empty matrices if not successful.
+   */
+  template <typename Interpolator = DefaultInterpolator>
+  TimeDataRangePair
+  getBetweenValuesInterpolated(time_t stamp_from, time_t stamp_to);
+
+  //! Get the values of the container at the given timestamps
+  //! The requested timestamps are expected to be in order!
+  template <typename Interpolator = DefaultInterpolator>
+  data_dynamic_t getValuesInterpolated(times_dynamic_t stamps);
+
+  //! Interpolate a single value
+  template <typename Interpolator = DefaultInterpolator>
+  bool getValueInterpolated(time_t t,  Eigen::Ref<data_dynamic_t> out);
+
+  inline void clear()
+  {
+    std::lock_guard<std::mutex> lock(mutex_);
+    times_.reset();
+  }
+
+  inline size_t size() const
+  {
+    std::lock_guard<std::mutex> lock(mutex_);
+    return times_.size();
+  }
+
+  inline bool empty() const
+  {
+    std::lock_guard<std::mutex> lock(mutex_);
+    return times_.empty();
+  }
+
+  //! technically does not remove but only moves the beginning of the ring
+  inline void removeDataBeforeTimestamp(time_t stamp)
+  {
+    std::lock_guard<std::mutex> lock(mutex_);
+    removeDataBeforeTimestamp_impl(stamp);
+  }
+
+  inline void removeDataOlderThan(real_t seconds)
+  {
+    std::lock_guard<std::mutex> lock(mutex_);
+    if(times_.empty())
+    {
+      return;
+    }
+
+    removeDataBeforeTimestamp_impl(
+          times_.back() - secToNanosec(seconds));
+  }
+
+  inline void lock() const
+  {
+    mutex_.lock();
+  }
+
+  inline void unlock() const
+  {
+    mutex_.unlock();
+  }
+
+  const data_t& data() const
+  {
+    CHECK(!mutex_.try_lock()) << "Call lock() before accessing data.";
+    return data_;
+  }
+
+  const timering_t& times() const
+  {
+    CHECK(!mutex_.try_lock()) << "Call lock() before accessing data.";
+    return times_;
+  }
+
+  typename timering_t::iterator iterator_equal_or_before(time_t stamp);
+  typename timering_t::iterator iterator_equal_or_after(time_t stamp);
+
+  //! returns an iterator to the first element in the times_ ring that
+  //! is greater or equal to stamp
+  inline typename timering_t::iterator lower_bound(time_t stamp);
+
+  inline std::mutex& mutex() {return mutex_;}
+
+protected:
+  mutable std::mutex mutex_;
+  data_t data_;
+  times_t times_raw_;
+  timering_t times_;
+
+  //! return the data at a given point in time
+  inline DataType dataAtTimeIterator(typename timering_t::iterator iter) const
+  {
+    //! @todo: i believe this is wrong.
+    return data_.col(iter.container_index());
+  }
+
+  //! return the data at a given point in time (const)
+  inline DataType dataAtTimeIterator(typename timering_t::const_iterator iter) const
+  {
+    //! @todo: i believe this is wrong.
+    return data_.col(iter.container_index());
+  }
+
+  //! shifts the starting point of the ringbuffer to the given timestamp
+  //! no resizing or deletion happens.
+  inline void removeDataBeforeTimestamp_impl(time_t stamp)
+  {
+    auto it = lower_bound(stamp);
+    times_.reset_front(it.container_index());
+  }
+};
+
+} // namespace ze
+
+#include <ze/common/ringbuffer-inl.hpp>
diff --git a/RWR/src/ze_oss/ze_common/include/ze/common/running_statistics.hpp b/RWR/src/ze_oss/ze_common/include/ze/common/running_statistics.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..0e18517d173a8324a5c009f1d8d29dcb19f33c79
--- /dev/null
+++ b/RWR/src/ze_oss/ze_common/include/ze/common/running_statistics.hpp
@@ -0,0 +1,107 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#pragma once
+
+#include <algorithm>
+#include <ze/common/types.hpp>
+
+namespace ze {
+
+//! Collects samples and incrementally computes statistical properties.
+//! http://www.johndcook.com/blog/standard_deviation/
+class RunningStatistics
+{
+public:
+
+  RunningStatistics() = default;
+  ~RunningStatistics() = default;
+
+  inline void addSample(real_t x)
+  {
+    ++n_;
+    min_ = std::min(min_, x);
+    max_ = std::max(max_, x);
+    sum_ += x;
+
+    // Online variance computation [Knuth TAOCP vol 2, 3rd edition, page 232].
+    if (n_ == 1u)
+    {
+      M_ = x;
+      S_ = 0.0;
+    }
+    else
+    {
+      real_t M_new = M_ + (x - M_) / n_;
+      S_ += (x - M_) * (x - M_new);
+      M_ = M_new;
+    }
+  }
+
+  inline real_t numSamples() const { return n_; }
+  inline real_t min()  const { return min_; }
+  inline real_t max()  const { return max_; }
+  inline real_t sum()  const { return sum_; }
+  inline real_t mean() const { return M_; }
+
+  // The use of (n-1) is due to https://en.wikipedia.org/wiki/Bessel's_correction
+  // that's why the result for small sample sizes in unit tests may not coincide
+  // with what you may expect.
+  inline real_t var()  const { return (n_ > 0u) ? S_ / (n_ - 1u) : 0.0; }
+  inline real_t std()  const { return std::sqrt(var()); }
+
+  inline void reset()
+  {
+    n_ = 0;
+    min_ = std::numeric_limits<real_t>::max();
+    max_ = 0.0;
+    sum_ = 0.0;
+    M_ = 0.0;
+    S_ = 0.0;
+  }
+
+private:
+  uint32_t n_ = 0u;
+  real_t min_ = std::numeric_limits<real_t>::max();
+  real_t max_ = 0.0;
+  real_t sum_ = 0.0;
+  real_t M_ = 0.0;
+  real_t S_ = 0.0;
+};
+
+//! Print statistics:
+inline std::ostream& operator<<(std::ostream& out, const RunningStatistics& stat)
+{
+  out << "  num_samples: " << stat.numSamples() << "\n"
+      << "  min: " << stat.min() << "\n"
+      << "  max: " << stat.max() << "\n"
+      << "  sum: " << stat.sum() << "\n"
+      << "  mean: " << stat.mean() << "\n"
+      << "  variance: " << stat.var() << "\n"
+      << "  standard_deviation: " << stat.std() << "\n";
+  return out;
+}
+
+} // end namespace ze
diff --git a/RWR/src/ze_oss/ze_common/include/ze/common/running_statistics_collection.hpp b/RWR/src/ze_oss/ze_common/include/ze/common/running_statistics_collection.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..ad4ffc610b9809e65ea931ea8a48e0fb41c63ced
--- /dev/null
+++ b/RWR/src/ze_oss/ze_common/include/ze/common/running_statistics_collection.hpp
@@ -0,0 +1,124 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#pragma once
+
+#include <array>
+#include <string>
+#include <sstream>
+
+#include <ze/common/file_utils.hpp>
+#include <ze/common/logging.hpp>
+#include <ze/common/string_utils.hpp>
+#include <ze/common/types.hpp>
+#include <ze/common/running_statistics.hpp>
+
+namespace ze {
+
+/*! Collect samples and iteratively compute statistics.
+ *
+ * Usage:
+\code{.cpp}
+  DECLARE_STATISTICS(StatisticsName, stats, foo, bar);
+  ze::StatisticsCollection stats;
+  stats[StatisticsName::foo].addSample(12);
+  stats[StatisticsName::foo].addSample(10);
+  real_t variance = stats[StatisticsName::foo].var();
+\endcode
+*/
+template<typename StatisticsEnum>
+class StatisticsCollection
+{
+public:
+  using Collection = std::array<RunningStatistics,
+                                static_cast<uint32_t>(StatisticsEnum::dimension)>;
+  using CollectionNames = std::vector<std::string>;
+
+  StatisticsCollection() = delete;
+
+  //! This constructor is used by the macro DECLARE_TIMER() below.
+  StatisticsCollection(const std::string& statistics_names_comma_separated)
+    : names_(splitString(statistics_names_comma_separated, ','))
+  {
+    CHECK_EQ(names_.size(), collection_.size());
+  }
+
+  StatisticsCollection(const std::vector<std::string>& timer_names)
+    : names_(timer_names)
+  {
+    CHECK_EQ(names_.size(), collection_.size());
+  }
+
+  ~StatisticsCollection() = default;
+
+  inline RunningStatistics& operator[](StatisticsEnum s)
+  {
+    return collection_[static_cast<uint32_t>(s)];
+  }
+
+  inline const RunningStatistics& operator[](StatisticsEnum s) const
+  {
+    return collection_[static_cast<uint32_t>(s)];
+  }
+
+  constexpr size_t size() const noexcept { return collection_.size(); }
+
+  //! Saves statistics to file in YAML format.
+  void saveToFile(const std::string& directory, const std::string& filename)
+  {
+    std::ofstream fs;
+    CHECK(isDir(directory));
+    openOutputFileStream(joinPath(directory, filename), &fs);
+    fs << *this;
+  }
+
+  inline const Collection& collection() const { return collection_; }
+
+  inline const CollectionNames& names() const { return names_; }
+
+private:
+  Collection collection_;
+  CollectionNames names_;
+};
+
+//! Print statistics collection.
+//! Print Timer Collection:
+template<typename StatisticsEnum>
+std::ostream& operator<<(std::ostream& out,
+                         const StatisticsCollection<StatisticsEnum>& statistics)
+{
+  for (size_t i = 0u; i < statistics.size(); ++i)
+  {
+    out << statistics.names().at(i) << ":\n"
+        << statistics.collection().at(i);
+  }
+  return out;
+}
+
+#define DECLARE_STATISTICS(classname, membername, ...)              \
+  enum class classname : uint32_t { __VA_ARGS__, dimension };  \
+  ze::StatisticsCollection<classname> membername { #__VA_ARGS__ }
+
+} // end namespace ze
diff --git a/RWR/src/ze_oss/ze_common/include/ze/common/signal_handler.hpp b/RWR/src/ze_oss/ze_common/include/ze/common/signal_handler.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..5514eb762f0b0c54366e18b0d87607e980d38d85
--- /dev/null
+++ b/RWR/src/ze_oss/ze_common/include/ze/common/signal_handler.hpp
@@ -0,0 +1,41 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#pragma once
+
+#include <ze/common/noncopyable.hpp>
+
+namespace ze {
+
+//! RAII-style signal handler that simply clears a flag when receiving SIGHUP,
+//! SIGINT or SIGTERM.
+class SimpleSigtermHandler : Noncopyable
+{
+public:
+  SimpleSigtermHandler(volatile bool& flag);
+  ~SimpleSigtermHandler();
+};
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_common/include/ze/common/statistics.hpp b/RWR/src/ze_oss/ze_common/include/ze/common/statistics.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..c938b74b0ce44eb32c4f98f615375a6040877f98
--- /dev/null
+++ b/RWR/src/ze_oss/ze_common/include/ze/common/statistics.hpp
@@ -0,0 +1,84 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#pragma once
+
+#include <utility>
+#include <vector>
+#include <algorithm>
+#include <Eigen/Dense>
+#include <ze/common/logging.hpp>
+
+#include <ze/common/stl_utils.hpp>
+
+//! @file statistics.hpp
+//! Various utilities, e.g. to compute statistical properties of vectors.
+
+namespace ze {
+
+//! Does not take const-ref because vector will be sorted.
+template <typename Scalar>
+std::pair<Scalar, bool> median(std::vector<Scalar>& v)
+{
+  if(v.size() == 0)
+  {
+    LOG(WARNING) << "Median computation of empty vector.";
+    return std::make_pair(Scalar{0}, false);
+  }
+  const size_t center = v.size() / 2;
+  std::nth_element(v.begin(), v.begin() + center, v.end());
+  Scalar median = v[center];
+  return std::make_pair(median, true);
+}
+
+template <typename DerivedVec>
+std::pair<typename DerivedVec::Scalar, bool> median(const Eigen::MatrixBase<DerivedVec>& v)
+{
+  EIGEN_STATIC_ASSERT_VECTOR_ONLY(DerivedVec);
+  auto w = eigenVectorToStlVector(v);
+  return median<typename DerivedVec::Scalar>(w);
+}
+
+template<class T>
+inline T normPdf(const T x, const T mean, const T sigma)
+{
+  T exponent = x - mean;
+  exponent *= -exponent;
+  exponent /= 2 * sigma * sigma;
+  T result = std::exp(exponent);
+  result /= sigma * std::sqrt(2 * M_PI);
+  return result;
+}
+
+//! Calculate the covariance of a matrix of measurements. The measurements are
+//! stacked column-wise in the matrix.
+inline const MatrixX measurementCovariance(const Eigen::Ref<MatrixX>& values)
+{
+  MatrixX zero_mean = values.colwise() - values.rowwise().mean();
+
+  return (zero_mean * zero_mean.adjoint()) / (zero_mean.cols() - 1);
+}
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_common/include/ze/common/stl_utils.hpp b/RWR/src/ze_oss/ze_common/include/ze/common/stl_utils.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..58c38b304a806ae86cff77dc2f4a656d2327a25b
--- /dev/null
+++ b/RWR/src/ze_oss/ze_common/include/ze/common/stl_utils.hpp
@@ -0,0 +1,71 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#pragma once
+
+#include <numeric>
+#include <vector>
+#include <ze/common/types.hpp>
+#include <ze/common/logging.hpp>
+
+//! @file stl_utils.hpp
+//! Various utilities to work with the standard template library.
+
+namespace ze {
+
+// -----------------------------------------------------------------------------
+//! Transform Eigen::Vector to std::vector.
+template <typename DerivedVec>
+std::vector<typename DerivedVec::Scalar> eigenVectorToStlVector(
+    const Eigen::MatrixBase<DerivedVec>& v)
+{
+  //! @todo: both data is continuous, can we do this more efficiently?
+  EIGEN_STATIC_ASSERT_VECTOR_ONLY(DerivedVec);
+  std::vector<typename DerivedVec::Scalar> rv(v.size());
+  for(int i = 0; i < v.size(); ++i)
+  {
+    rv[i] = v(i);
+  }
+  return rv;
+}
+
+// -----------------------------------------------------------------------------
+//! @return Returns a vector of indices form start to stop.
+inline std::vector<uint32_t> range(uint32_t start, uint32_t stop)
+{
+  DEBUG_CHECK_GE(stop, start);
+  std::vector<uint32_t> vec(stop - start);
+  std::iota(vec.begin(), vec.end(), start);
+  return vec;
+}
+
+// -----------------------------------------------------------------------------
+//! @return Returns a vector of indices form 0 to stop.
+inline std::vector<uint32_t> range(uint32_t stop)
+{
+  return range(0u, stop);
+}
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_common/include/ze/common/string_utils.hpp b/RWR/src/ze_oss/ze_common/include/ze/common/string_utils.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..186f6ccbadd9bf1d08f0c8247bf12acddcbb3833
--- /dev/null
+++ b/RWR/src/ze_oss/ze_common/include/ze/common/string_utils.hpp
@@ -0,0 +1,128 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#pragma once
+
+#include <sstream>
+#include <string>
+#include <algorithm>
+#include <vector>
+
+#include <ze/common/logging.hpp>
+
+//! @file string_utilties.hpp
+//! Various utilities to work with std::string.
+
+namespace ze {
+
+inline std::string& leftTrimString(std::string& s)
+{
+  s.erase(s.begin(), std::find_if(s.begin(), s.end(), std::not1(std::ptr_fun<int, int>(std::isspace))));
+  return s;
+}
+
+inline std::string& rightTrimString(std::string& s)
+{
+  s.erase(std::find_if(s.rbegin(), s.rend(), std::not1(std::ptr_fun<int, int>(std::isspace))).base(), s.end());
+  return s;
+}
+
+inline std::string& trimString(std::string& s)
+{
+  return leftTrimString(rightTrimString(s));
+}
+
+inline std::string& ensureLeftSlash(std::string& s)
+{
+  CHECK_GE(s.size(), 1u);
+  if(s[0] != '/')
+  {
+    s.insert(0, "/");
+  }
+  return s;
+}
+
+inline std::string ensureLeftSlash(const std::string& s)
+{
+  CHECK_GE(s.size(), 1u);
+  std::string s_copy = s;
+  if(s_copy[0] != '/')
+  {
+    s_copy.insert(0, "/");
+  }
+  return s_copy;
+}
+
+inline std::string ensureNoLeftSlash(const std::string& s)
+{
+  CHECK_GE(s.size(), 1u);
+  if(s[0] != '/')
+  {
+    return s;
+  }
+  std::string s_copy = s;
+  s_copy.erase(0, 1);
+  CHECK(s_copy[0] != '/');
+  return s_copy;
+}
+
+inline std::string ensureRightSlash(const std::string& s)
+{
+  if(s.size() == 0)
+  {
+    return "/";
+  }
+  if(s[s.size()-1] == '/')
+  {
+    return s;
+  }
+  return s + "/";
+}
+
+inline std::vector<std::string> splitString(const std::string& s, char delim)
+{
+  std::stringstream ss(s);
+  std::string item;
+  std::vector<std::string> items;
+  while (std::getline(ss, item, delim))
+  {
+    items.push_back(trimString(item));
+  }
+  return items;
+}
+
+inline bool replaceInString(
+    std::string& str, const std::string& from, const std::string& to)
+{
+    size_t start_pos = str.find(from);
+    if(start_pos == std::string::npos)
+    {
+      return false;
+    }
+    str.replace(start_pos, from.length(), to);
+    return true;
+}
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_common/include/ze/common/test_entrypoint.hpp b/RWR/src/ze_oss/ze_common/include/ze/common/test_entrypoint.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..2c76210482dfd6f974c5e679d6b951bdc1345dee
--- /dev/null
+++ b/RWR/src/ze_oss/ze_common/include/ze/common/test_entrypoint.hpp
@@ -0,0 +1,76 @@
+// Copyright (c) 2015, Autonomous Systems Lab, ETH Zurich
+// Copyright (C) 2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the <organization> nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#pragma once
+
+#include <gtest/gtest.h>
+#include <gflags/gflags.h>
+#include <ze/common/logging.hpp>
+#include <eigen-checks/gtest.h>
+#include <ze/common/types.hpp>
+
+//! @file test_entrypoint.hpp
+//! Macros for unit tests.
+
+// Floating-point precision checks.
+#ifdef ZE_SINGLE_PRECISION_FLOAT
+# define EXPECT_FLOATTYPE_EQ(a, b) EXPECT_FLOAT_EQ(a, b)
+#else
+# define EXPECT_FLOATTYPE_EQ(a, b) EXPECT_DOUBLE_EQ(a, b)
+#endif
+
+// Let the Eclipse parser see the macro.
+#ifndef TEST
+# define TEST(a, b) int Test_##a##_##b()
+#endif
+
+#ifndef TEST_F
+# define TEST_F(a, b) int Test_##a##_##b()
+#endif
+
+#ifndef TEST_P
+# define TEST_P(a, b) int Test_##a##_##b()
+#endif
+
+#ifndef TYPED_TEST_CASE
+# define TYPED_TEST_CASE(a, b) int Test_##a##_##b()
+#endif
+
+#ifndef TYPED_TEST
+# define TYPED_TEST(a, b) int Test_##a##_##b()
+#endif
+
+#define ZE_UNITTEST_ENTRYPOINT\
+  int main(int argc, char** argv) {\
+  ::testing::InitGoogleTest(&argc, argv);\
+  google::InitGoogleLogging(argv[0]);\
+  google::ParseCommandLineFlags(&argc, &argv, false);\
+  google::InstallFailureSignalHandler();\
+  ::testing::FLAGS_gtest_death_test_style = "threadsafe";\
+  FLAGS_alsologtostderr = true; \
+  FLAGS_colorlogtostderr = true; \
+  return RUN_ALL_TESTS();\
+}
diff --git a/RWR/src/ze_oss/ze_common/include/ze/common/test_manifold.hpp b/RWR/src/ze_oss/ze_common/include/ze/common/test_manifold.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..a486d12224f170c6c05a33d8b3b07c28bccbf550
--- /dev/null
+++ b/RWR/src/ze_oss/ze_common/include/ze/common/test_manifold.hpp
@@ -0,0 +1,78 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#pragma once
+
+#include <ze/common/logging.hpp>
+
+#include <ze/common/manifold.hpp>
+#include <ze/common/numerical_derivative.hpp>
+
+namespace ze {
+
+template<typename T>
+void testManifoldInvariants(const T& a, const T& b, real_t tol = 1e-9)
+{
+  CHECK(traits<T>::equals(a, a));
+  typename traits<T>::TangentVector v = traits<T>::local(a, b);
+  T c = traits<T>::retract(a, v);
+  CHECK(traits<T>::equals(b, c, tol));
+}
+
+template<typename T>
+void testRetractJacobians(const T& a, const T& b, real_t tol = 1e-9)
+{
+  using namespace std::placeholders; // for _1
+  typename traits<T>::Jacobian H1, H2;
+  typename traits<T>::TangentVector v = traits<T>::local(a, b);
+  T c = traits<T>::retract(a, v, &H1, &H2);
+  typename traits<T>::Jacobian H1_numerical =
+      numericalDerivative<T, T>(
+        std::bind(traits<T>::retract, _1, v, nullptr, nullptr), a);
+  CHECK(traits<typename traits<T>::Jacobian>::equals(H1, H1_numerical, tol));
+
+  typename traits<T>::Jacobian H2_numerical =
+      numericalDerivative<T, typename traits<T>::TangentVector>(
+        std::bind(traits<T>::retract, a, _1, nullptr, nullptr), v);
+  CHECK(traits<typename traits<T>::Jacobian>::equals(H2, H2_numerical, tol));
+}
+
+template<typename T>
+void testLocalJacobians(const T& a, const T& b, real_t tol = 1e-9)
+{
+  using namespace std::placeholders; // for _1
+  typename traits<T>::Jacobian H1, H2;
+  typename traits<T>::TangentVector v = traits<T>::local(a, b, &H1, &H2);
+  typename traits<T>::Jacobian H1_numerical =
+      numericalDerivative<typename traits<T>::TangentVector, T>(
+        std::bind(traits<T>::local, _1, b, nullptr, nullptr), a);
+  CHECK(traits<typename traits<T>::Jacobian>::equals(H1, H1_numerical, tol));
+
+  typename traits<T>::Jacobian H2_numerical =
+      numericalDerivative<typename traits<T>::TangentVector, T>(
+        std::bind(traits<T>::local, a, _1, nullptr, nullptr), b);
+  CHECK(traits<typename traits<T>::Jacobian>::equals(H2, H2_numerical, tol));
+}
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_common/include/ze/common/test_thread_blocking.hpp b/RWR/src/ze_oss/ze_common/include/ze/common/test_thread_blocking.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..aa2dce24132df09ed2387c3c0e93837ad4e9e939
--- /dev/null
+++ b/RWR/src/ze_oss/ze_common/include/ze/common/test_thread_blocking.hpp
@@ -0,0 +1,48 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#pragma once
+
+namespace ze {
+
+//! Helper class to perform blocking tests.
+class BlockingTest
+{
+public:
+  BlockingTest() = default;
+  virtual ~BlockingTest() = default;
+  virtual void performBlockingAction(unsigned testId) = 0;
+  virtual void performUnblockingAction(unsigned testId) = 0;
+  void runBlockingTest(unsigned testId, unsigned timeout);
+  unsigned testId() const { return test_id_; }
+  void setFinished() { finished_ = true; }
+
+private:
+  void runThread();
+  unsigned test_id_ = 0;
+  bool finished_ = false;
+};
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_common/include/ze/common/test_utils.hpp b/RWR/src/ze_oss/ze_common/include/ze/common/test_utils.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..b0af281a901e7f81a0112ad7b677ed84da3ac3f5
--- /dev/null
+++ b/RWR/src/ze_oss/ze_common/include/ze/common/test_utils.hpp
@@ -0,0 +1,49 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#pragma once
+
+#include <float.h>
+#include <limits>
+#include <map>
+#include <string>
+#include <ze/common/transformation.hpp>
+#include <ze/common/types.hpp>
+
+namespace ze {
+
+//! Get path to the "data" folder in the ze_test_data repository. Therefore,
+//! the environment variable ZE_TEST_DATA_PATH must be set.
+std::string getTestDataDir(const std::string& dataset_name);
+
+//! Load poses from .csv file. Returns a map { Image-Index / Stamp -> Pose }
+std::map<int64_t, Transformation> loadIndexedPosesFromCsv(const std::string& filename);
+
+//! Load depthmap from .depth file. Copy into raw datapointer of size data_size
+//! because ze_common does not depend on imp_core. data must be preallocated!
+void loadDepthmapFromFile(
+    const std::string& filename, const size_t data_size, float* data);
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_common/include/ze/common/thread_pool.hpp b/RWR/src/ze_oss/ze_common/include/ze/common/thread_pool.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..433af08f4e707b41fc8181f7f536d4010e6c0189
--- /dev/null
+++ b/RWR/src/ze_oss/ze_common/include/ze/common/thread_pool.hpp
@@ -0,0 +1,103 @@
+// Copyright (c) 2012 Jakob Progsch, Václav Zeman
+// From: https://github.com/progschj/ThreadPool
+// Copyright (C) 2016 ETH Zurich, Wyss Zurich, Zurich Eye
+//
+// This software is provided 'as-is', without any express or implied
+// warranty. In no event will the authors be held liable for any damages
+// arising from the use of this software.
+//
+// Permission is granted to anyone to use this software for any purpose,
+// including commercial applications, and to alter it and redistribute it
+// freely, subject to the following restrictions:
+//
+//    1. The origin of this software must not be misrepresented; you must not
+//    claim that you wrote the original software. If you use this software
+//    in a product, an acknowledgment in the product documentation would be
+//    appreciated but is not required.
+//
+//    2. Altered source versions must be plainly marked as such, and must not be
+//    misrepresented as being the original software.
+//
+//    3. This notice may not be removed or altered from any source
+//    distribution.
+
+
+
+#pragma once
+
+#include <vector>
+#include <queue>
+#include <memory>
+#include <thread>
+#include <mutex>
+#include <condition_variable>
+#include <future>
+#include <functional>
+#include <stdexcept>
+#include <glog/logging.h>
+
+namespace ze {
+
+class ThreadPool
+{
+public:
+  ThreadPool() = default;
+
+  //! Creates a thread pool and starts the amount of specified worker threads.
+  ThreadPool(size_t n_threads)
+  {
+    startThreads(n_threads);
+  }
+
+  //! The destructor joins all threads.
+  ~ThreadPool();
+
+  //! Launches the amount of specified worker threads.
+  void startThreads(size_t n_threads);
+
+  //! Add task to threadpool. See for example usage in unit-test.
+  template<class F, class... Args>
+  auto enqueue(F&& f, Args&&... args)
+  -> std::future<typename std::result_of<F(Args...)>::type>;
+
+private:
+  //! need to keep track of threads so we can join them
+  std::vector<std::thread> workers_;
+
+  //! the task queue
+  std::queue<std::function<void()>> tasks_;
+
+  //! synchronization
+  std::mutex queue_mutex_;
+  std::condition_variable condition_;
+  bool stop_ = false;
+};
+ 
+// add new work item to the pool
+template<class F, class... Args>
+auto ThreadPool::enqueue(F&& f, Args&&... args)
+  -> std::future<typename std::result_of<F(Args...)>::type>
+{
+  using return_type = typename std::result_of<F(Args...)>::type;
+
+  auto task = std::make_shared<std::packaged_task<return_type()>>(
+    std::bind(std::forward<F>(f), std::forward<Args>(args)...)
+  );
+
+  std::future<return_type> res = task->get_future();
+  {
+    std::unique_lock<std::mutex> lock(queue_mutex_);
+
+    // don't allow enqueueing after stopping the pool
+    if (stop_)
+    {
+      LOG(FATAL) << "Enqueue on stopped ThreadPool";
+    }
+
+    tasks_.emplace([task](){ (*task)(); });
+  }
+  condition_.notify_one();
+  return res;
+}
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_common/include/ze/common/thread_safe_fifo.hpp b/RWR/src/ze_oss/ze_common/include/ze/common/thread_safe_fifo.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..fa6a9275c0df2bad2db4d78df86da8137505b892
--- /dev/null
+++ b/RWR/src/ze_oss/ze_common/include/ze/common/thread_safe_fifo.hpp
@@ -0,0 +1,566 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+
+#pragma once
+
+#include <mutex>
+#include <condition_variable>
+#include <ze/common/noncopyable.hpp>
+
+namespace ze {
+
+/*!
+ * @brief Thread-safe FIFO for an arbitrary number of writer and reader threads.
+ *
+ * The object class must be <default constructible> and <assignable>.
+ *
+ * The implementation blocks on read() and timedRead() if the queue is empty.
+ * For timedRead() and nonBlockingRead(), a default object is returned if the
+ * queue is empty.
+ *
+ * write() will block until the queue no longer is full.
+ * nonBlockingWrite() will fail if the queue is full.
+ * timedWrite() will fail if the queue is full and no object is read until the
+ * timeout occurs.
+ *
+ * Capacity defines the buffer capacity. Note that this only defines the raw
+ * buffer capacity. The actual capacity is one item less, as one sentinel slot
+ * is used to differentiate between full and empty.
+ *
+ * If Erase is true, the element in the buffer will be overwritten with a null
+ * object when the element is read.
+ * If MoveSemantics is true, elements can be moved out of the buffer without
+ * explicit need to clear the buffer content afterwards.
+ **/
+template <class T, unsigned Capacity, bool Erase=true, bool MoveSemantics=true>
+class ThreadSafeFifo : Noncopyable
+{
+public:
+
+  typedef std::mutex Mutex;
+  typedef std::lock_guard<Mutex> LockGuard;
+  typedef std::unique_lock<Mutex> UniqueLock;
+  typedef std::condition_variable ConditionVariable;
+
+  ThreadSafeFifo();
+  ~ThreadSafeFifo() = default;
+
+  /*!
+   * Creates a lock for the queue and returns it.
+   * Useful for external synchronization schemes.
+   **/
+  UniqueLock getLock() const { return UniqueLock(mutex_); }
+
+  /*!
+   * Returns the reader condition variable.
+   * Useful for external synchronization schemes.
+   **/
+  ConditionVariable& readerConditionVariable() const { return read_cond_; }
+
+  /*!
+   * Returns the writer condition variable.
+   * Useful for external synchronization schemes.
+   **/
+  ConditionVariable& writerConditionVariable() const { return write_cond_; }
+
+  /*!
+   * @name Status
+   **/
+  //@{
+
+  bool empty() const;
+  bool empty(UniqueLock& lock) const;
+
+  bool full() const;
+  bool full(UniqueLock& lock) const;
+
+  unsigned size() const;
+  unsigned size(UniqueLock& lock) const;
+
+  //@} // Status
+
+  /*!
+   * @name Data Access
+   **/
+  //@{
+
+  /*!
+   * Writes the data element to the buffer.
+   * This method will block while the buffer is full.
+   **/
+  //@{
+  void write(const T& data);
+  void write(const T& data, UniqueLock& lock);
+  void write(T&& data);
+  void write(T&& data, UniqueLock& lock);
+  //@}
+
+  /*!
+   * Writes the data element to the buffer if the buffer is not full.
+   * Returns whether the data was written or not.
+   **/
+  //@{
+  bool nonBlockingWrite(const T& data);
+  bool nonBlockingWrite(const T& data, UniqueLock& lock);
+  bool nonBlockingWrite(T&& data);
+  bool nonBlockingWrite(T&& data, UniqueLock& lock);
+  //@}
+
+  /*!
+   * Writes the data element to the buffer.
+   * This method will block for the specified timeout if the buffer is full.
+   * Returns whether data was written or not.
+   **/
+  //@{
+  bool timedWrite(const T& data, unsigned timeout);
+  bool timedWrite(const T& data, unsigned timeout, UniqueLock& lock);
+  bool timedWrite(T&& data, unsigned timeout);
+  bool timedWrite(T&& data, unsigned timeout, UniqueLock& lock);
+  //@}
+
+  /*!
+   * Returns the next element from the queue.
+   * The method blocks until data is available.
+   **/
+  //@{
+  T read();
+  T read(UniqueLock& lock);
+  //@}
+
+  /*!
+   * Reads the next element (if available) into the provided variable.
+   * Returns whether data was read or not.
+   **/
+  //@{
+  bool nonBlockingRead(T& data);
+  bool nonBlockingRead(T& data, UniqueLock& lock);
+  //@}
+
+  /*!
+   * Reads the next element (if available) into the provided variable.
+   * This method will block for the specified timeout if no data is available.
+   * Returns whether data was read or not.
+   **/
+  //@{
+  bool timedRead(T& data, unsigned timeout);
+  bool timedRead(T& data, unsigned timeout, UniqueLock& lock);
+  //@}
+
+  /*!
+   * Clears the content of the queue.
+   **/
+  //@{
+  void clear();
+  void clear(UniqueLock& lock);
+  //@}
+
+  //@} // Data Access
+
+private:
+
+  bool _empty() const;
+  bool _full() const;
+  unsigned _size() const;
+  void _write(const T& data);
+  void _write(T&& data);
+  T _read();
+  void _clear();
+  unsigned _nextIndex(unsigned index) const;
+  bool _notEmpty() const;
+  bool _notFull() const;
+
+  mutable Mutex mutex_;
+  ConditionVariable read_cond_;
+  ConditionVariable write_cond_;
+
+  std::array<T, Capacity> buf_;
+  unsigned tail_; // writer end
+  unsigned head_; // reader end
+
+}; // ThreadSafeFifo
+
+//------------------------------------------------------------------------------
+// implementation
+//
+
+template <typename T, unsigned Capacity, bool Erase, bool MoveSemantics>
+ThreadSafeFifo<T, Capacity, Erase, MoveSemantics>::ThreadSafeFifo()
+  : mutex_()
+  , read_cond_()
+  , write_cond_()
+  , buf_()
+  , tail_(0)
+  , head_(0)
+{
+}
+
+//------------------------------------------------------------------------------
+template <typename T, unsigned Capacity, bool Erase, bool MoveSemantics>
+bool ThreadSafeFifo<T, Capacity, Erase, MoveSemantics>::empty() const
+{
+  LockGuard lock(mutex_);
+  return _empty();
+}
+
+//------------------------------------------------------------------------------
+template <typename T, unsigned Capacity, bool Erase, bool MoveSemantics>
+bool
+ThreadSafeFifo<T, Capacity, Erase, MoveSemantics>::empty(UniqueLock& lock) const
+{
+  return _empty();
+}
+
+//------------------------------------------------------------------------------
+template <typename T, unsigned Capacity, bool Erase, bool MoveSemantics>
+bool ThreadSafeFifo<T, Capacity, Erase, MoveSemantics>::full() const
+{
+  LockGuard lock(mutex_);
+  return _full();
+}
+
+//------------------------------------------------------------------------------
+template <typename T, unsigned Capacity, bool Erase, bool MoveSemantics>
+bool
+ThreadSafeFifo<T, Capacity, Erase, MoveSemantics>::full(UniqueLock& lock) const
+{
+  return _full();
+}
+
+//------------------------------------------------------------------------------
+template <typename T, unsigned Capacity, bool Erase, bool MoveSemantics>
+unsigned ThreadSafeFifo<T, Capacity, Erase, MoveSemantics>::size() const
+{
+  LockGuard lock(mutex_);
+  return _size();
+}
+
+//------------------------------------------------------------------------------
+template <typename T, unsigned Capacity, bool Erase, bool MoveSemantics>
+unsigned
+ThreadSafeFifo<T, Capacity, Erase, MoveSemantics>::size(UniqueLock& lock) const
+{
+  return _size();
+}
+
+//------------------------------------------------------------------------------
+template<typename T, unsigned Capacity, bool Erase, bool MoveSemantics>
+void ThreadSafeFifo<T, Capacity, Erase, MoveSemantics>::write(const T& data)
+{
+  UniqueLock lock(mutex_);
+  write(data, lock);
+}
+
+//------------------------------------------------------------------------------
+template<typename T, unsigned Capacity, bool Erase, bool MoveSemantics>
+void
+ThreadSafeFifo<T, Capacity, Erase, MoveSemantics>::write(const T& data,
+                                                         UniqueLock& lock)
+{
+  write_cond_.wait(lock, [this]{ return _notFull(); });
+  _write(data);
+}
+
+//------------------------------------------------------------------------------
+template<typename T, unsigned Capacity, bool Erase, bool MoveSemantics>
+void ThreadSafeFifo<T, Capacity, Erase, MoveSemantics>::write(T&& data)
+{
+  UniqueLock lock(mutex_);
+  write(std::forward<T>(data), lock);
+}
+
+//------------------------------------------------------------------------------
+template<typename T, unsigned Capacity, bool Erase, bool MoveSemantics>
+void ThreadSafeFifo<T, Capacity, Erase, MoveSemantics>::write(T&& data,
+                                                              UniqueLock& lock)
+{
+  write_cond_.wait(lock, [this]{ return _notFull(); });
+  _write(std::forward<T>(data));
+}
+
+//------------------------------------------------------------------------------
+template<typename T, unsigned Capacity, bool Erase, bool MoveSemantics>
+bool ThreadSafeFifo<T, Capacity, Erase, MoveSemantics>
+::nonBlockingWrite(const T& data)
+{
+  UniqueLock lock(mutex_);
+  return nonBlockingWrite(data, lock);
+}
+
+//------------------------------------------------------------------------------
+template<typename T, unsigned Capacity, bool Erase, bool MoveSemantics>
+bool ThreadSafeFifo<T, Capacity, Erase, MoveSemantics>
+::nonBlockingWrite(const T& data, UniqueLock& lock)
+{
+  if (_full())
+  {
+    return false;
+  }
+  _write(data);
+  return true;
+}
+
+//------------------------------------------------------------------------------
+template<typename T, unsigned Capacity, bool Erase, bool MoveSemantics>
+bool ThreadSafeFifo<T, Capacity, Erase, MoveSemantics>
+::nonBlockingWrite(T&& data)
+{
+  UniqueLock lock(mutex_);
+  return nonBlockingWrite(std::forward<T>(data), lock);
+}
+
+//------------------------------------------------------------------------------
+template<typename T, unsigned Capacity, bool Erase, bool MoveSemantics>
+bool ThreadSafeFifo<T, Capacity, Erase, MoveSemantics>
+::nonBlockingWrite(T&& data, UniqueLock& lock)
+{
+  if (_full())
+  {
+    return false;
+  }
+
+  _write(std::forward<T>(data));
+  return true;
+}
+
+//------------------------------------------------------------------------------
+template<typename T, unsigned Capacity, bool Erase, bool MoveSemantics>
+bool ThreadSafeFifo<T, Capacity, Erase, MoveSemantics>
+::timedWrite(const T& data, unsigned timeout)
+{
+  UniqueLock lock(mutex_);
+  return timedWrite(data, timeout, lock);
+}
+
+//------------------------------------------------------------------------------
+template<typename T, unsigned Capacity, bool Erase, bool MoveSemantics>
+bool ThreadSafeFifo<T, Capacity, Erase, MoveSemantics>
+::timedWrite(const T& data, unsigned timeout, UniqueLock& lock)
+{
+  if (!write_cond_.wait_for(lock, std::chrono::milliseconds(timeout),
+                                     [this]{ return _notFull(); }))
+  {
+    return false;
+  }
+  _write(data);
+  return true;
+}
+
+//------------------------------------------------------------------------------
+template<typename T, unsigned Capacity, bool Erase, bool MoveSemantics>
+bool ThreadSafeFifo<T, Capacity, Erase, MoveSemantics>
+::timedWrite(T&& data, unsigned timeout)
+{
+  UniqueLock lock(mutex_);
+  return timedWrite(std::forward<T>(data), timeout, lock);
+}
+
+//------------------------------------------------------------------------------
+template<typename T, unsigned Capacity, bool Erase, bool MoveSemantics>
+bool ThreadSafeFifo<T, Capacity, Erase, MoveSemantics>
+::timedWrite(T&& data, unsigned timeout, UniqueLock& lock)
+{
+  if (!write_cond_.wait_for(lock, std::chrono::milliseconds(timeout),
+                              [this]{ return _notFull(); }))
+  {
+    return false;
+  }
+
+  _write(std::forward<T>(data));
+  return true;
+}
+
+//------------------------------------------------------------------------------
+template<typename T, unsigned Capacity, bool Erase, bool MoveSemantics>
+T ThreadSafeFifo<T, Capacity, Erase, MoveSemantics>::read()
+{
+  UniqueLock lock(mutex_);
+  return read(lock);
+}
+
+//------------------------------------------------------------------------------
+template<typename T, unsigned Capacity, bool Erase, bool MoveSemantics>
+T ThreadSafeFifo<T, Capacity, Erase, MoveSemantics>::read(UniqueLock& lock)
+{
+  read_cond_.wait(lock, [this]{ return _notEmpty(); });
+
+  return _read();
+}
+
+//------------------------------------------------------------------------------
+template<typename T, unsigned Capacity, bool Erase, bool MoveSemantics>
+bool ThreadSafeFifo<T, Capacity, Erase, MoveSemantics>::nonBlockingRead(T& data)
+{
+  UniqueLock lock(mutex_);
+  return nonBlockingRead(data, lock);
+}
+
+//------------------------------------------------------------------------------
+template<typename T, unsigned Capacity, bool Erase, bool MoveSemantics>
+bool ThreadSafeFifo<T, Capacity, Erase, MoveSemantics>
+::nonBlockingRead(T& data, UniqueLock& lock)
+{
+  if (_empty())
+  {
+    return false;
+  }
+
+  data = _read();
+  return true;
+}
+
+//------------------------------------------------------------------------------
+template<typename T, unsigned Capacity, bool Erase, bool MoveSemantics>
+bool ThreadSafeFifo<T, Capacity, Erase, MoveSemantics>
+::timedRead(T& data, unsigned timeout)
+{
+  UniqueLock lock(mutex_);
+  return timedRead(data, timeout, lock);
+}
+
+//------------------------------------------------------------------------------
+template<typename T, unsigned Capacity, bool Erase, bool MoveSemantics>
+bool ThreadSafeFifo<T, Capacity, Erase, MoveSemantics>
+::timedRead(T& data, unsigned timeout, UniqueLock& lock)
+{
+  if (!read_cond_.wait_for(lock, std::chrono::milliseconds(timeout),
+                           [this]{ return _notEmpty(); }))
+  {
+    return false;
+  }
+
+  data = _read();
+  return true;
+}
+
+//------------------------------------------------------------------------------
+template<typename T, unsigned Capacity, bool Erase, bool MoveSemantics>
+void ThreadSafeFifo<T, Capacity, Erase, MoveSemantics>::clear()
+{
+  LockGuard lock(mutex_);
+  _clear();
+}
+
+//------------------------------------------------------------------------------
+template<typename T, unsigned Capacity, bool Erase, bool MoveSemantics>
+void ThreadSafeFifo<T, Capacity, Erase, MoveSemantics>::clear(UniqueLock& lock)
+{
+  _clear();
+}
+
+//------------------------------------------------------------------------------
+template<typename T, unsigned Capacity, bool Erase, bool MoveSemantics>
+bool ThreadSafeFifo<T, Capacity, Erase, MoveSemantics>::_empty() const
+{
+  return (tail_ == head_);
+}
+
+//------------------------------------------------------------------------------
+template<typename T, unsigned Capacity, bool Erase, bool MoveSemantics>
+bool ThreadSafeFifo<T, Capacity, Erase, MoveSemantics>::_full() const
+{
+  return (_nextIndex(tail_) == head_);
+}
+
+//------------------------------------------------------------------------------
+template<typename T, unsigned Capacity, bool Erase, bool MoveSemantics>
+unsigned ThreadSafeFifo<T, Capacity, Erase, MoveSemantics>::_size() const
+{
+  return (tail_ < head_) ? ((tail_ + Capacity) - head_) : (tail_ - head_);
+}
+
+//------------------------------------------------------------------------------
+template<typename T, unsigned Capacity, bool Erase, bool MoveSemantics>
+void ThreadSafeFifo<T, Capacity, Erase, MoveSemantics>::_write(const T& data)
+{
+  buf_[tail_] = data;
+  tail_ = _nextIndex(tail_);
+  read_cond_.notify_one();
+}
+
+//------------------------------------------------------------------------------
+template<typename T, unsigned Capacity, bool Erase, bool MoveSemantics>
+void ThreadSafeFifo<T, Capacity, Erase, MoveSemantics>::_write(T&& data)
+{
+  buf_[tail_] = std::forward<T>(data);
+  tail_ = _nextIndex(tail_);
+  read_cond_.notify_one();
+}
+
+//------------------------------------------------------------------------------
+template<typename T, unsigned Capacity, bool Erase, bool MoveSemantics>
+T ThreadSafeFifo<T, Capacity, Erase, MoveSemantics>::_read()
+{
+  T data = std::forward<T>(buf_[head_]);
+  if (Erase and not MoveSemantics)
+  {
+    buf_[head_] = T();
+  }
+  head_ = _nextIndex(head_);
+
+  write_cond_.notify_one();
+  return data;
+}
+
+//------------------------------------------------------------------------------
+template<typename T, unsigned Capacity, bool Erase, bool MoveSemantics>
+void ThreadSafeFifo<T, Capacity, Erase, MoveSemantics>::_clear()
+{
+  if (Erase) {
+    buf_.fill(T());
+  }
+
+  tail_ = 0;
+  head_ = 0;
+}
+
+//------------------------------------------------------------------------------
+template<typename T, unsigned Capacity, bool Erase, bool MoveSemantics>
+unsigned ThreadSafeFifo<T, Capacity, Erase, MoveSemantics>
+::_nextIndex(unsigned index) const
+{
+  index++;
+  if (index == Capacity)
+  {
+    index = 0;
+  }
+  return index;
+}
+
+//------------------------------------------------------------------------------
+template<typename T, unsigned Capacity, bool Erase, bool MoveSemantics>
+bool ThreadSafeFifo<T, Capacity, Erase, MoveSemantics>::_notEmpty() const
+{
+  return !_empty();
+}
+
+//------------------------------------------------------------------------------
+template<typename T, unsigned Capacity, bool Erase, bool MoveSemantics>
+bool ThreadSafeFifo<T, Capacity, Erase, MoveSemantics>::_notFull() const
+{
+  return !_full();
+}
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_common/include/ze/common/time_conversions.hpp b/RWR/src/ze_oss/ze_common/include/ze/common/time_conversions.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..ed714f531dd7dc362aa78986ecfc757661467ca7
--- /dev/null
+++ b/RWR/src/ze_oss/ze_common/include/ze/common/time_conversions.hpp
@@ -0,0 +1,76 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#pragma once
+
+#include <ze/common/types.hpp>
+
+namespace ze {
+
+//! Utilities for working with timestamps.
+//!
+//! Important: Always store int64_t nanosecond timestamps! We use signed type
+//!            to avoid errors when taking differences and we use nanoseconds
+//!            when saving to file to have a unique type for lookups in
+//!            dictionaries/maps.
+
+//! Seconds to nanoseconds.
+inline constexpr int64_t secToNanosec(real_t seconds)
+{
+  return static_cast<int64_t>(seconds * 1e9);
+}
+
+//! Milliseconds to nanoseconds.
+inline constexpr int64_t millisecToNanosec(real_t milliseconds)
+{
+  return static_cast<int64_t>(milliseconds * 1e6);
+}
+
+//! Nanoseconds to seconds.
+//! WARNING: Don't pass very large or small numbers to this function as the
+//!          representability of the float value does not capture nanoseconds
+//!          resolution. The resulting accuracy will be in the order of
+//!          hundreds of nanoseconds.
+inline constexpr real_t nanosecToSecTrunc(int64_t nanoseconds)
+{
+  return static_cast<real_t>(nanoseconds) / 1e9;
+}
+
+//! Nanoseconds to milliseconds.
+//! WARNING: Don't pass very large or very small numbers to this function as the
+//!          representability of the float value does not capture nanoseconds
+//!          resolution.
+inline constexpr real_t nanosecToMillisecTrunc(int64_t nanoseconds)
+{
+  return static_cast<real_t>(nanoseconds) / 1e6;
+}
+
+//! Return total nanoseconds from seconds and nanoseconds pair.
+inline constexpr int64_t nanosecFromSecAndNanosec(int32_t sec, int32_t nsec)
+{
+  return static_cast<int64_t>(sec) * 1000000000ll + static_cast<int64_t>(nsec);
+}
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_common/include/ze/common/timer.hpp b/RWR/src/ze_oss/ze_common/include/ze/common/timer.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..79c627c71d65e6efc4fe814c9c59ebb5ab0e4a1c
--- /dev/null
+++ b/RWR/src/ze_oss/ze_common/include/ze/common/timer.hpp
@@ -0,0 +1,75 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#pragma once
+
+#include <chrono>
+
+#include <ze/common/time_conversions.hpp>
+#include <ze/common/types.hpp>
+
+namespace ze {
+
+//! Simple timing utilty.
+class Timer
+{
+public:
+  using Clock = std::chrono::high_resolution_clock;
+  using TimePoint = std::chrono::time_point<Clock>;
+  using ns = std::chrono::nanoseconds;
+  using ms = std::chrono::milliseconds;
+
+  //! The constructor directly starts the timer.
+  Timer()
+    : start_time_(Clock::now())
+  {}
+
+  inline void start()
+  {
+    start_time_ = Clock::now();
+  }
+
+  inline int64_t stopAndGetNanoseconds()
+  {
+    const TimePoint end_time(Clock::now());
+    ns duration = std::chrono::duration_cast<ns>(end_time - start_time_);
+    return duration.count();
+  }
+
+  inline real_t stopAndGetMilliseconds()
+  {
+    return nanosecToMillisecTrunc(stopAndGetNanoseconds());
+  }
+
+  inline real_t stopAndGetSeconds()
+  {
+    return nanosecToSecTrunc(stopAndGetNanoseconds());
+  }
+
+private:
+  TimePoint start_time_;
+};
+
+} // end namespace ze
diff --git a/RWR/src/ze_oss/ze_common/include/ze/common/timer_collection.hpp b/RWR/src/ze_oss/ze_common/include/ze/common/timer_collection.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..e92a387348058469e75d1c4c894f55d5abb3edd5
--- /dev/null
+++ b/RWR/src/ze_oss/ze_common/include/ze/common/timer_collection.hpp
@@ -0,0 +1,131 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#pragma once
+
+#include <array>
+#include <string>
+#include <sstream>
+
+#include <ze/common/file_utils.hpp>
+#include <ze/common/logging.hpp>
+#include <ze/common/string_utils.hpp>
+#include <ze/common/time_conversions.hpp>
+#include <ze/common/timer_statistics.hpp>
+#include <ze/common/types.hpp>
+#include <ze/common/running_statistics.hpp>
+
+namespace ze {
+
+/*! Collect statistics over multiple timings in milliseconds.
+ *
+ * Usage with explicit start and stop:
+\code{.cpp}
+  // Use the macro to declare the timer:
+  DECLARE_TIMER(TimerName, timers, foo, bar);
+  timers[TimerName::foo].start();
+  ...
+  timers[TimerName::foo].stop();
+\endcode
+ * Or RAII-style, use a TimedScope object that stops the timer when it
+ * goes out of scope.
+\code{.cpp}
+  {
+    auto t = timers[TimerName::foo].timeScope();
+    ...
+  }
+\endcode
+*/
+template<typename TimerEnum>
+class TimerCollection
+{
+public:
+  using Timers = std::array<TimerStatistics, static_cast<uint32_t>(TimerEnum::dimension)>;
+  using TimerNames = std::vector<std::string>;
+
+  TimerCollection() = delete;
+
+  //! This constructor is used by the macro DECLARE_TIMER() below.
+  TimerCollection(const std::string& timer_names_comma_separated)
+    : names_(splitString(timer_names_comma_separated, ','))
+  {
+    CHECK_EQ(names_.size(), timers_.size());
+  }
+
+  TimerCollection(const std::vector<std::string>& timer_names)
+    : names_(timer_names)
+  {
+    CHECK_EQ(names_.size(), timers_.size());
+  }
+
+  ~TimerCollection() = default;
+
+  inline TimerStatistics& operator[](TimerEnum t)
+  {
+    return timers_[static_cast<uint32_t>(t)];
+  }
+
+  inline const TimerStatistics& operator[](TimerEnum t) const
+  {
+    return timers_[static_cast<uint32_t>(t)];
+  }
+
+  constexpr size_t size() const noexcept { return timers_.size(); }
+
+  //! Saves timings to file in YAML format.
+  inline void saveToFile(const std::string& directory, const std::string& filename) const
+  {
+    std::ofstream fs;
+    CHECK(isDir(directory));
+    openOutputFileStream(joinPath(directory, filename), &fs);
+    fs << *this;
+  }
+
+  inline const Timers& timers() const { return timers_; }
+
+  inline const TimerNames& names() const { return names_; }
+
+private:
+  Timers timers_;
+  std::vector<std::string> names_;
+};
+
+//! Print Timer Collection:
+template<typename TimerEnum>
+std::ostream& operator<<(std::ostream& out, const TimerCollection<TimerEnum>& timers)
+{
+  for (size_t i = 0u; i < timers.size(); ++i)
+  {
+    out << timers.names().at(i) << ":\n"
+        << timers.timers().at(i).statistics();
+  }
+  return out;
+}
+
+} // end namespace ze
+
+#define DECLARE_TIMER(classname, membername, ...)              \
+  enum class classname : uint32_t { __VA_ARGS__, dimension };  \
+  ze::TimerCollection<classname> membername { #__VA_ARGS__ }
diff --git a/RWR/src/ze_oss/ze_common/include/ze/common/timer_statistics.hpp b/RWR/src/ze_oss/ze_common/include/ze/common/timer_statistics.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..917cadb66335d4f49ee9eaba108446106d5e8c24
--- /dev/null
+++ b/RWR/src/ze_oss/ze_common/include/ze/common/timer_statistics.hpp
@@ -0,0 +1,101 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#pragma once
+
+#include <ze/common/running_statistics.hpp>
+#include <ze/common/timer.hpp>
+#include <ze/common/types.hpp>
+
+namespace ze {
+
+// fwd
+class TimedScope;
+
+//! Collect statistics over multiple timings in milliseconds.
+class TimerStatistics
+{
+public:
+  inline void start()
+  {
+    t_.start();
+  }
+
+  //! Using the concept of "Initialization is Resource Acquisition" idiom, this
+  //! function returns a timer object. When this timer object is destructed,
+  //! the timer is stopped.
+  inline TimedScope timeScope();
+
+  inline real_t stop()
+  {
+    real_t t = t_.stopAndGetMilliseconds();
+    stat_.addSample(t);
+    return t;
+  }
+
+  inline real_t numTimings() const { return stat_.numSamples(); }
+  inline real_t accumulated() const { return stat_.sum(); }
+  inline real_t min() const { return stat_.min(); }
+  inline real_t max() const { return stat_.max(); }
+  inline real_t mean() const { return stat_.mean(); }
+  inline real_t variance() const { return stat_.var(); }
+  inline real_t standarDeviation() const { return stat_.std(); }
+  inline void reset() { stat_.reset(); }
+  inline const RunningStatistics& statistics() const { return stat_; }
+
+private:
+  Timer t_;
+  RunningStatistics stat_;
+};
+
+//! This object is return from TimerStatistics::timeScope()
+class TimedScope
+{
+public:
+  TimedScope() = delete;
+
+  TimedScope(TimerStatistics* timer)
+    : timer_(timer)
+  {
+    timer_->start();
+  }
+
+  ~TimedScope()
+  {
+    timer_->stop();
+  }
+private:
+  TimerStatistics* timer_;
+};
+
+inline TimedScope TimerStatistics::timeScope()
+{
+  // Returning a pointer to this should be safe, as inserting more elements in
+  // the unordered map does not invalidate any references to other elements in
+  // the unordered map (http://stackoverflow.com/questions/16781886).
+  return TimedScope(this);
+}
+
+} // end namespace ze
diff --git a/RWR/src/ze_oss/ze_common/include/ze/common/transformation.hpp b/RWR/src/ze_oss/ze_common/include/ze/common/transformation.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..a2d401a0ad893e9a0ed4f4e0649687a8abe39e43
--- /dev/null
+++ b/RWR/src/ze_oss/ze_common/include/ze/common/transformation.hpp
@@ -0,0 +1,111 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#pragma once
+
+#include <limits>
+#include <vector>
+#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
+// Eigen 3.2.7 uses std::binder1st and std::binder2nd which are deprecated since c++11
+// Fix is in 3.3 devel (http://eigen.tuxfamily.org/bz/show_bug.cgi?id=872).
+#include <kindr/minimal/quat-transformation.h>
+#pragma diagnostic pop
+#include <ze/common/types.hpp>
+#include <ze/common/matrix.hpp>
+
+namespace ze {
+
+using Transformation = kindr::minimal::QuatTransformationTemplate<real_t>;
+using Quaternion = kindr::minimal::RotationQuaternionTemplate<real_t>;
+using AngleAxis = kindr::minimal::AngleAxisTemplate<real_t>;
+
+using TransformationVector = std::vector<Transformation, Eigen::aligned_allocator<Transformation>>;
+using QuaternionVector = std::vector<Quaternion, Eigen::aligned_allocator<Quaternion>>;
+
+using StampedTransformation = std::pair<int64_t, Transformation>;
+using StampedTransformationVector = std::vector<StampedTransformation,
+                                                Eigen::aligned_allocator<StampedTransformation>>;
+
+// -----------------------------------------------------------------------------
+// Transformation utils
+
+// Right Jacobian for Exponential map in SO(3)
+inline Matrix3 expmapDerivativeSO3(const Vector3& omega)
+{
+  real_t theta2 = omega.dot(omega);
+  if (theta2 <= std::numeric_limits<real_t>::epsilon())
+  {
+    return I_3x3;
+  }
+  real_t theta = std::sqrt(theta2);  // rotation angle
+  // element of Lie algebra so(3): X = omega^, normalized by normx
+  const Matrix3 Y = skewSymmetric(omega) / theta;
+  return I_3x3 - ((real_t{1} - std::cos(theta)) / (theta)) * Y
+               + (real_t{1} - std::sin(theta) / theta) * Y * Y;
+}
+
+// Right Jacobian for Log map in SO(3)
+inline Matrix3 logmapDerivativeSO3(const Vector3& omega)
+{
+  real_t theta2 = omega.dot(omega);
+  if (theta2 <= std::numeric_limits<real_t>::epsilon())
+  {
+    return I_3x3;
+  }
+  real_t theta = std::sqrt(theta2);  // rotation angle
+  const Matrix3 X = skewSymmetric(omega); // element of Lie algebra so(3): X = omega^
+  return I_3x3 + real_t{0.5} * X
+               + (real_t{1} / (theta * theta)
+               - (real_t{1} + std::cos(theta)) / (real_t{2} * theta * std::sin(theta))) * X * X;
+}
+
+// -----------------------------------------------------------------------------
+// Quaternion utils
+
+//! Plus matrix for a quaternion. q_AB x q_BC = plus(q_AB) * q_BC.coeffs().
+inline Matrix4 quaternionPlusMatrix(const Eigen::Quaternion<real_t>& q_AB)
+{
+  const Vector4& q = q_AB.coeffs();
+  Matrix4 Q;
+  Q(0,0) =  q[3]; Q(0,1) = -q[2]; Q(0,2) =  q[1]; Q(0,3) =  q[0];
+  Q(1,0) =  q[2]; Q(1,1) =  q[3]; Q(1,2) = -q[0]; Q(1,3) =  q[1];
+  Q(2,0) = -q[1]; Q(2,1) =  q[0]; Q(2,2) =  q[3]; Q(2,3) =  q[2];
+  Q(3,0) = -q[0]; Q(3,1) = -q[1]; Q(3,2) = -q[2]; Q(3,3) =  q[3];
+  return Q;
+}
+
+//! Opposite-Plus matrix for a quaternion q_AB x q_BC = oplus(q_BC) * q_AB.coeffs().
+inline Matrix4 quaternionOplusMatrix(const Eigen::Quaternion<real_t>& q_BC)
+{
+  const Vector4& q = q_BC.coeffs();
+  Matrix4 Q;
+  Q(0,0) =  q[3]; Q(0,1) =  q[2]; Q(0,2) = -q[1]; Q(0,3) =  q[0];
+  Q(1,0) = -q[2]; Q(1,1) =  q[3]; Q(1,2) =  q[0]; Q(1,3) =  q[1];
+  Q(2,0) =  q[1]; Q(2,1) = -q[0]; Q(2,2) =  q[3]; Q(2,3) =  q[2];
+  Q(3,0) = -q[0]; Q(3,1) = -q[1]; Q(3,2) = -q[2]; Q(3,3) =  q[3];
+  return Q;
+}
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_common/include/ze/common/types.hpp b/RWR/src/ze_oss/ze_common/include/ze/common/types.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..7faf6c30af04d5e7c0469bfdf70f0e2c310fd582
--- /dev/null
+++ b/RWR/src/ze_oss/ze_common/include/ze/common/types.hpp
@@ -0,0 +1,139 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#pragma once
+
+#include <cstdint>
+#pragma diagnostic push
+#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
+// Eigen 3.2.7 uses std::binder1st and std::binder2nd which are deprecated since c++11
+// Fix is in 3.3 devel (http://eigen.tuxfamily.org/bz/show_bug.cgi?id=872).
+#include <Eigen/Core>
+#pragma diagnostic pop
+#include <Eigen/StdVector>
+#include <ze/common/config.hpp>
+
+namespace ze {
+
+//------------------------------------------------------------------------------
+// Scalars and fp precision.
+using size_t    = std::size_t;
+using int8_t    = std::int8_t;
+using int16_t   = std::int16_t;
+using int64_t   = std::int64_t;
+using uint8_t   = std::uint8_t;
+using uint16_t  = std::uint16_t;
+using uint32_t  = uint32_t;
+using uint64_t  = std::uint64_t;
+#ifdef ZE_SINGLE_PRECISION_FLOAT
+using real_t = float;
+#else
+using real_t = double;
+#endif
+
+//------------------------------------------------------------------------------
+// Typedefs of commonly used Eigen matrices and vectors.
+
+// MatrixMN, MatrixN = MatrixNN, I_NxN, and Z_NxN, for M,N=1..9.
+#define ZE_MAKE_EIGEN_MATRIX_TYPEDEFS(SIZE, SUFFIX)            \
+  using Matrix##SUFFIX = Eigen::Matrix<real_t, SIZE, SIZE>; \
+  using Matrix1##SUFFIX = Eigen::Matrix<real_t, 1, SIZE>;   \
+  using Matrix2##SUFFIX = Eigen::Matrix<real_t, 2, SIZE>;   \
+  using Matrix3##SUFFIX = Eigen::Matrix<real_t, 3, SIZE>;   \
+  using Matrix4##SUFFIX = Eigen::Matrix<real_t, 4, SIZE>;   \
+  using Matrix5##SUFFIX = Eigen::Matrix<real_t, 5, SIZE>;   \
+  using Matrix6##SUFFIX = Eigen::Matrix<real_t, 6, SIZE>;   \
+  using Matrix7##SUFFIX = Eigen::Matrix<real_t, 7, SIZE>;   \
+  using Matrix8##SUFFIX = Eigen::Matrix<real_t, 8, SIZE>;   \
+  using Matrix9##SUFFIX = Eigen::Matrix<real_t, 9, SIZE>;   \
+  using Matrix##SUFFIX##X = Eigen::Matrix<real_t, SIZE, Eigen::Dynamic>; \
+  using MatrixX##SUFFIX = Eigen::Matrix<real_t, Eigen::Dynamic, SIZE>;   \
+  static const Eigen::MatrixBase<Matrix##SUFFIX>::IdentityReturnType I_##SUFFIX##x##SUFFIX = Matrix##SUFFIX::Identity(); \
+  static const Eigen::MatrixBase<Matrix##SUFFIX>::ConstantReturnType Z_##SUFFIX##x##SUFFIX = Matrix##SUFFIX::Zero()
+
+ZE_MAKE_EIGEN_MATRIX_TYPEDEFS(1,1);
+ZE_MAKE_EIGEN_MATRIX_TYPEDEFS(2,2);
+ZE_MAKE_EIGEN_MATRIX_TYPEDEFS(3,3);
+ZE_MAKE_EIGEN_MATRIX_TYPEDEFS(4,4);
+ZE_MAKE_EIGEN_MATRIX_TYPEDEFS(5,5);
+ZE_MAKE_EIGEN_MATRIX_TYPEDEFS(6,6);
+ZE_MAKE_EIGEN_MATRIX_TYPEDEFS(7,7);
+ZE_MAKE_EIGEN_MATRIX_TYPEDEFS(8,8);
+ZE_MAKE_EIGEN_MATRIX_TYPEDEFS(9,9);
+
+// Typedef arbitary length vector and arbitrary sized matrix.
+using VectorX = Eigen::Matrix<real_t, Eigen::Dynamic, 1>;
+using MatrixX = Eigen::Matrix<real_t, Eigen::Dynamic, Eigen::Dynamic>;
+using VectorXi = Eigen::VectorXi;
+
+// Commonly used fixed size vectors.
+using Vector1 = Eigen::Matrix<real_t, 1, 1>;
+using Vector2 = Eigen::Matrix<real_t, 2, 1>;
+using Vector3 = Eigen::Matrix<real_t, 3, 1>;
+using Vector4 = Eigen::Matrix<real_t, 4, 1>;
+using Vector5 = Eigen::Matrix<real_t, 5, 1>;
+using Vector6 = Eigen::Matrix<real_t, 6, 1>;
+using Vector7 = Eigen::Matrix<real_t, 7, 1>;
+using Vector8 = Eigen::Matrix<real_t, 8, 1>;
+using Vector9 = Eigen::Matrix<real_t, 9, 1>;
+using Vector2i = Eigen::Vector2i;
+
+//------------------------------------------------------------------------------
+// Feature containers.
+using Keypoint    = Vector2;
+using Bearing     = Vector3;
+using Position    = Vector3;
+using HomPosition = Vector4;
+using Gradient    = Vector2;
+using Seed        = Vector4;
+using LineMeasurement = Vector3;
+using Keypoints   = Matrix2X;
+using Bearings    = Matrix3X;
+using Positions   = Matrix3X;
+using HomPositions = Matrix4X;
+using Gradients   = Matrix2X;
+using Seeds       = Matrix4X;
+//! Normal vector on line end-points bearings, as explained in
+//! ze_geometry/doc/line_parametrization.pdf
+using LineMeasurements = Matrix3X;
+using KeypointLevel = int8_t;
+using KeypointType  = int8_t;
+using KeypointIndex = uint16_t;
+using KeypointLevels = Eigen::Matrix<KeypointLevel, Eigen::Dynamic, 1>;
+using KeypointTypes  = Eigen::Matrix<KeypointType, Eigen::Dynamic, 1>;
+using KeypointAngles = VectorX;
+using KeypointScores = VectorX;
+using KeypointSizes  = VectorX;
+using KeypointIndices  = Eigen::Matrix<KeypointIndex, Eigen::Dynamic, 1>;
+using Descriptors = Eigen::Matrix<uint8_t, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>;
+
+//------------------------------------------------------------------------------
+// Inertial containers.
+using ImuStamps = Eigen::Matrix<int64_t, Eigen::Dynamic, 1>;
+using ImuAccGyrContainer = Matrix6X;
+// Order: Accelerometer, Gyroscope
+using ImuAccGyr = Vector6;
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_common/include/ze/common/versioned_slot_handle.hpp b/RWR/src/ze_oss/ze_common/include/ze/common/versioned_slot_handle.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..366a544b66bfb10c020769c186f793fdb883c983
--- /dev/null
+++ b/RWR/src/ze_oss/ze_common/include/ze/common/versioned_slot_handle.hpp
@@ -0,0 +1,94 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#pragma once
+
+#include <ze/common/types.hpp>
+
+namespace ze {
+
+//! A template for a data handle that is composed of the slot index (in an array)
+//! plus a version number. The indexed array stores the version number of each
+//! object and can therefore tell if the object at the slot is at the same version we
+//! request.
+//! http://seanmiddleditch.com/data-structures-for-game-developers-the-slot-map/
+//! http://blog.molecular-matters.com/2013/05/17/adventures-in-data-oriented-design-part-3b-internal-references/
+template <typename T, int NumSlotBits, int NumVersionBits>
+union VersionedSlotHandle
+{
+  using value_t = T;
+
+  struct
+  {
+    // The first NumSlotBits in this bit-field store the slot, the next
+    // NumVersionBits store the version number of the handle.
+    T slot    : NumSlotBits;
+    T version : NumVersionBits;
+  };
+  T handle;
+
+  // Always initialize handle first to zero to make sure that all bits are really 0.
+  VersionedSlotHandle() : handle(0) {}
+  VersionedSlotHandle(T handle) : handle(handle) {}
+  VersionedSlotHandle(T _slot, T _version) : handle(0) { slot = _slot; version = _version; }
+
+  static constexpr T maxSlot() { return (1 << NumSlotBits) - 1; }
+  static constexpr T maxVersion() { return (1 << NumVersionBits) - 1; }
+
+  void reset() { handle = T{0}; }
+};
+
+template <typename T, int NumSlotBits, int NumVersionBits>
+inline bool operator==(const VersionedSlotHandle<T, NumSlotBits, NumVersionBits> lhs,
+                       const VersionedSlotHandle<T, NumSlotBits, NumVersionBits> rhs)
+{
+  return lhs.handle == rhs.handle;
+}
+
+template <typename T, int NumSlotBits, int NumVersionBits>
+inline bool operator!=(const VersionedSlotHandle<T, NumSlotBits, NumVersionBits> lhs,
+                       const VersionedSlotHandle<T, NumSlotBits, NumVersionBits> rhs)
+{
+  return !(lhs.handle == rhs.handle);
+}
+
+template <typename T, int NumSlotBits, int NumVersionBits>
+inline std::ostream& operator<<(
+    std::ostream& out, const VersionedSlotHandle<T, NumSlotBits, NumVersionBits> handle)
+{
+  out << handle.handle
+      << " (slot: " << handle.slot << ", version: " << handle.version << ")";
+  return out;
+}
+
+//! Compares the handle as an integer. I.e. Compare slot number first, then version.
+template <typename T, int NumSlotBits, int NumVersionBits>
+inline bool operator<(const VersionedSlotHandle<T, NumSlotBits, NumVersionBits> lhs,
+                      const VersionedSlotHandle<T, NumSlotBits, NumVersionBits> rhs)
+{
+  return lhs.handle < rhs.handle;
+}
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_common/include/ze/common/yaml_serialization.hpp b/RWR/src/ze_oss/ze_common/include/ze/common/yaml_serialization.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..40eee2efe452f8d55e88991e0473bf13e7ba3c36
--- /dev/null
+++ b/RWR/src/ze_oss/ze_common/include/ze/common/yaml_serialization.hpp
@@ -0,0 +1,118 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#pragma once
+
+#include <Eigen/Core>
+
+#include <ze/common/logging.hpp>
+#include <yaml-cpp/yaml.h>
+
+namespace YAML {
+
+template <typename T>
+T extractChild(const YAML::Node& node, const std::string& key)
+{
+  if (!node.IsMap())
+  {
+    throw YAML::Exception(node.Mark(), "Node is not a map");
+  }
+
+  const YAML::Node child = node[key];
+  if (!child)
+  {
+    throw YAML::Exception(node.Mark(), "key '" + key + "' does not exist");
+  }
+
+  return child.as<T>();
+}
+
+/**
+ * yaml serialization helper function for the Eigen3 Matrix object.
+ * The matrix is a base class for dense matrices.
+ * http://eigen.tuxfamily.org/dox-devel/TutorialMatrixClass.html
+ * 
+ * see yaml-cpp for how the extraction of complex types works
+ *
+ * Inspired by ETHZ-ASL ASLAM_CV_COMMON
+ */
+template <class Scalar, int A, int B, int C, int D, int E>
+struct convert<Eigen::Matrix<Scalar, A, B, C, D, E> >
+{
+  static bool decode(const Node& node,
+                     Eigen::Matrix<Scalar, A, B, C, D, E>& M)
+  {
+    if(!node.IsMap())
+    {
+      LOG(ERROR) << "Unable to parse the matrix because the node is not a map.";
+      return false;
+    }
+
+    typedef typename Eigen::Matrix<Scalar, A, B, C, D, E>::Index IndexType;
+    IndexType rows = node["rows"].as<IndexType>();
+    IndexType cols = node["cols"].as<IndexType>();
+
+    if((A != Eigen::Dynamic && rows != A) || 
+       (B != Eigen::Dynamic && cols != B))
+    {
+      LOG(ERROR) << "The matrix is the wrong size (rows, cols). Wanted: (" << (A==Eigen::Dynamic?rows:A) << ","
+          << (B==Eigen::Dynamic?cols:B) << "), got (" << rows << ", " << cols << ")";
+      return false;
+    }
+
+    M.resize(rows, cols);
+
+    size_t expected_size = M.rows() * M.cols();
+    if (!node["data"].IsSequence())
+    {
+      LOG(ERROR) << "The matrix data is not a sequence.";
+      return false;
+    }
+    if(node["data"].size() != expected_size)
+    {
+      LOG(ERROR) << "The data sequence is the wrong size. Wanted: " << expected_size <<
+          ", got: " << node["data"].size();
+      return false;
+    }
+
+    YAML::const_iterator it = node["data"].begin();
+    YAML::const_iterator it_end = node["data"].end();
+    if(rows > 0 && cols > 0)
+    {
+      for(IndexType i = 0; i < rows; ++i)
+      {
+        for(IndexType j = 0; j < cols; ++j)
+        {
+          CHECK(it != it_end);
+          M(i, j) = it->as<Scalar>();
+          ++it;
+        }
+      }
+    }
+    return true;
+  }
+};
+
+}  // namespace YAML
diff --git a/RWR/src/ze_oss/ze_common/package.xml b/RWR/src/ze_oss/ze_common/package.xml
new file mode 100644
index 0000000000000000000000000000000000000000..4db1c7608fe183c9c500e6849ea43529506a20be
--- /dev/null
+++ b/RWR/src/ze_oss/ze_common/package.xml
@@ -0,0 +1,23 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>ze_common</name>
+  <version>0.1.4</version>
+  <description>
+    Common utilities for all ZE projects.
+  </description>
+  <maintainer email="christian.forster@WyssZurich.ch">Christian Forster</maintainer>
+  <license>ZE</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <buildtool_depend>catkin_simple</buildtool_depend>
+
+  <depend>glog_catkin</depend>
+  <depend>gflags_catkin</depend>
+  <depend>eigen_catkin</depend>
+  <depend>eigen_checks</depend>
+  <depend>minkindr</depend>
+  <depend>ze_cmake</depend>
+  <depend>yaml_cpp_catkin</depend>
+
+  <test_depend>gtest</test_depend>
+</package>
diff --git a/RWR/src/ze_oss/ze_common/src/csv_trajectory.cpp b/RWR/src/ze_oss/ze_common/src/csv_trajectory.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..72b197f2594e90551397bc4a7a5333c189afb9b9
--- /dev/null
+++ b/RWR/src/ze_oss/ze_common/src/csv_trajectory.cpp
@@ -0,0 +1,217 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include <ze/common/csv_trajectory.hpp>
+
+namespace ze {
+
+int64_t CSVTrajectory::getTimeStamp(const std::string& ts_str) const
+{
+  return std::stoll(ts_str);
+}
+
+void CSVTrajectory::readHeader(const std::string& in_file_path)
+{
+  in_str_.open(in_file_path);
+  CHECK(in_str_.is_open());
+  if(!header_.empty())
+  {
+    std::string line;
+    getline(in_str_, line);
+    CHECK_EQ(line.substr(0, header_.size()), header_);
+  }
+}
+
+Vector3 CSVTrajectory::readTranslation(const std::vector<std::string>& items)
+{
+  return Vector3(
+        std::stod(items[order_.find("tx")->second]),
+        std::stod(items[order_.find("ty")->second]),
+        std::stod(items[order_.find("tz")->second]));
+}
+
+Vector4 CSVTrajectory::readOrientation(const std::vector<std::string>& items)
+{
+  Vector4 q(
+        std::stod(items[order_.find("qx")->second]),
+        std::stod(items[order_.find("qy")->second]),
+        std::stod(items[order_.find("qz")->second]),
+        std::stod(items[order_.find("qw")->second]));
+  if(std::abs(q.squaredNorm() - 1.0) > 1e-4)
+  {
+    LOG(WARNING) << "Quaternion norm is = " << q.norm();
+    CHECK_NEAR(q.norm(), 1.0, 0.01);
+    q.normalize(); // This is only good up to some point.
+  }
+  return q;
+}
+
+Vector7 CSVTrajectory::readPose(const std::vector<std::string>& items)
+{
+  Vector7 pose;
+  pose << readTranslation(items), readOrientation(items);
+  return pose;
+}
+
+PositionSeries::PositionSeries()
+{
+  order_["ts"] = 0;
+  order_["tx"] = 1;
+  order_["ty"] = 2;
+  order_["tz"] = 3;
+
+  header_ = "# timestamp, x, y, z";
+  num_tokens_in_line_ = 4u;
+}
+
+void PositionSeries::load(const std::string& in_file_path)
+{
+  readHeader(in_file_path);
+  std::string line;
+  while(getline(in_str_, line))
+  {
+    if('%' != line.at(0) && '#' != line.at(0))
+    {
+      std::vector<std::string> items = ze::splitString(line, delimiter_);
+      CHECK_GE(items.size(), num_tokens_in_line_);
+      int64_t stamp = getTimeStamp(items[order_.find("ts")->second]);
+      Vector3 position = readTranslation(items);
+      position_buf_.insert(stamp, position);
+    }
+  }
+}
+
+const Buffer<real_t, 3>& PositionSeries::getBuffer() const
+{
+  return position_buf_;
+}
+
+Buffer<real_t, 3>& PositionSeries::getBuffer()
+{
+  return position_buf_;
+}
+
+PoseSeries::PoseSeries()
+{
+  order_["ts"] = 0;
+  order_["tx"] = 1;
+  order_["ty"] = 2;
+  order_["tz"] = 3;
+  order_["qx"] = 4;
+  order_["qy"] = 5;
+  order_["qz"] = 6;
+  order_["qw"] = 7;
+
+  header_ = "# timestamp, x, y, z, qx, qy, qz, qw";
+  num_tokens_in_line_ = 8u;
+}
+
+void PoseSeries::load(const std::string& in_file_path)
+{
+  readHeader(in_file_path);
+  std::string line;
+  while(getline(in_str_, line))
+  {
+    if('%' != line.at(0) && '#' != line.at(0) && 't' != line.at(0))
+    {
+      std::vector<std::string> items = ze::splitString(line, delimiter_);
+      CHECK_GE(items.size(), num_tokens_in_line_);
+      int64_t stamp = getTimeStamp(items[order_.find("ts")->second]);
+      Vector7 pose = readPose(items);
+      pose_buf_.insert(stamp, pose);
+    }
+  }
+}
+
+const Buffer<real_t, 7>& PoseSeries::getBuffer() const
+{
+  return pose_buf_;
+}
+
+Buffer<real_t, 7>& PoseSeries::getBuffer()
+{
+  return pose_buf_;
+}
+
+StampedTransformationVector PoseSeries::getStampedTransformationVector()
+{
+  StampedTransformationVector vec;
+  pose_buf_.lock();
+  auto& data = pose_buf_.data();
+  vec.reserve(data.size());
+  for(const auto& it : data)
+  {
+    vec.push_back(std::make_pair(it.first, getTransformationFromVec7(it.second)));
+  }
+  pose_buf_.unlock();
+  return vec;
+}
+
+Transformation PoseSeries::getTransformationFromVec7(const Vector7& data)
+{
+  Vector3 p = data.head<3>();
+  Eigen::Quaternion<real_t> q(data(6), data(3), data(4), data(5));
+  CHECK_NEAR(q.squaredNorm(), 1.0, 1e-4);
+  q.normalize();
+  return Transformation(q, p);
+}
+
+SWEResultSeries::SWEResultSeries()
+  : PoseSeries()
+{
+  header_ = "timestamp, x, y, z, qx, qy, qz, qw, vx, vy, vz, bgx, bgy, bgz, bax, bay, baz";
+}
+
+SWEGlobalSeries::SWEGlobalSeries()
+{
+  order_["ts"] = 0;
+  order_["tx"] = 2;
+  order_["ty"] = 3;
+  order_["tz"] = 4;
+  order_["qx"] = 5;
+  order_["qy"] = 6;
+  order_["qz"] = 7;
+  order_["qw"] = 8;
+
+  header_ = "timestamp, state, x_GB, y_GB, z_GB, qx_GB, qy_GB, qz_GB, qw_GB, x_GM, y_GM, z_GM, qx_GM, qy_GM, qz_GM, qw_GM";
+  num_tokens_in_line_ = 16u;
+}
+
+EurocResultSeries::EurocResultSeries()
+  : PoseSeries()
+{
+  order_["ts"] = 0;
+  order_["tx"] = 1;
+  order_["ty"] = 2;
+  order_["tz"] = 3;
+  order_["qx"] = 5;
+  order_["qy"] = 6;
+  order_["qz"] = 7;
+  order_["qw"] = 4;
+
+  header_ = "#timestamp, p_RS_R_x [m], p_RS_R_y [m], p_RS_R_z [m], q_RS_w [], q_RS_x [], q_RS_y [], q_RS_z [], v_RS_R_x [m s^-1], v_RS_R_y [m s^-1], v_RS_R_z [m s^-1], b_w_RS_S_x [rad s^-1], b_w_RS_S_y [rad s^-1], b_w_RS_S_z [rad s^-1], b_a_RS_S_x [m s^-2], b_a_RS_S_y [m s^-2], b_a_RS_S_z [m s^-2]";
+}
+
+} // ze namespace
diff --git a/RWR/src/ze_oss/ze_common/src/matrix.cpp b/RWR/src/ze_oss/ze_common/src/matrix.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..16d6f97e4dbb8e995bac886708b75257a91e506a
--- /dev/null
+++ b/RWR/src/ze_oss/ze_common/src/matrix.cpp
@@ -0,0 +1,57 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include <ze/common/matrix.hpp>
+
+#include <Eigen/SVD>
+#include <Eigen/LU>
+
+namespace ze {
+
+std::tuple<int, real_t, VectorX> directLinearTransform(const MatrixX& A, real_t rank_tol)
+{
+  int n = A.rows();
+  int p = A.cols();
+  int m = std::min(n,p);
+  Eigen::JacobiSVD<MatrixX> svd(A, Eigen::ComputeFullV);
+  VectorX s = svd.singularValues();
+  MatrixX V = svd.matrixV();
+
+  // Find rank
+  int rank = 0;
+  for (int i = 0; i < m; ++i)
+  {
+    if (s(i) > rank_tol)
+    {
+      ++rank;
+    }
+  }
+
+  // Return rank, error, and corresponding column of V
+  real_t error = (m < p) ? 0.0 : s(m-1);
+  return std::make_tuple(rank, error, V.col(p-1));
+}
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_common/src/random.cpp b/RWR/src/ze_oss/ze_common/src/random.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..833d8a72089c23c6156fb56f60405002473cb499
--- /dev/null
+++ b/RWR/src/ze_oss/ze_common/src/random.cpp
@@ -0,0 +1,51 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include <ze/common/random.hpp>
+
+namespace ze {
+
+Vector3 randomDirection3D(bool deterministic)
+{
+  // equal-area projection according to:
+  // https://math.stackexchange.com/questions/44689/how-to-find-a-random-axis-or-unit-vector-in-3d
+  const real_t z =
+      sampleUniformRealDistribution(deterministic, 0.0, 1.0) * 2.0 - 1.0;
+  const real_t t =
+      sampleUniformRealDistribution(deterministic, 0.0, 1.0) * 2.0 * M_PI;
+  const real_t r = std::sqrt(1.0 - z * z);
+  const real_t x = r * std::cos(t);
+  const real_t y = r * std::sin(t);
+  return Vector3(x, y, z);
+}
+
+Vector2 randomDirection2D(bool deterministic)
+{
+  const real_t theta =
+      sampleUniformRealDistribution(deterministic, 0.0, 1.0) * 2.0 * M_PI;
+  return Vector2(std::cos(theta), std::sin(theta));
+}
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_common/src/signal_handler.cpp b/RWR/src/ze_oss/ze_common/src/signal_handler.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..8aed3aa5ca5f2c8f8c6c35ca3adb4095bc6dd0a4
--- /dev/null
+++ b/RWR/src/ze_oss/ze_common/src/signal_handler.cpp
@@ -0,0 +1,100 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include <ze/common/signal_handler.hpp>
+
+#include <cstdint>
+#include <cstring>
+#include <iostream>
+#include <signal.h>
+#include <sys/wait.h>
+#include <unistd.h>
+
+#include <ze/common/logging.hpp>
+
+namespace ze {
+
+// enforce local linkage
+namespace {
+
+volatile bool* s_simple_flag = nullptr;
+
+void handleSignalSimple(int signal)
+{
+  LOG(WARNING) << "Signal handler was called with signal " << signal;
+  *s_simple_flag = false;
+}
+
+void installSignalHandlerSimple(int sig)
+{
+  struct sigaction handler;
+  handler.sa_handler = handleSignalSimple;
+  sigfillset(&handler.sa_mask); // block all pending signals
+  handler.sa_flags = 0;
+  if (sigaction(sig, &handler, NULL) < 0)
+  {
+    LOG(FATAL) << "cannot install the signal handler for signal " << sig
+               << " : " << std::strerror(errno);
+  }
+}
+
+void clearSignalHandlerSimple(int sig)
+{
+  struct sigaction handler;
+  handler.sa_handler = SIG_DFL;
+  sigemptyset(&handler.sa_mask);
+  handler.sa_flags = 0;
+  if (sigaction(sig, &handler, NULL) < 0)
+  {
+    LOG(ERROR) << "failed to uninstall the signal handler for signal "
+               << sig << ": " << std::strerror(errno);
+  }
+}
+
+} // unnamed namespace
+
+SimpleSigtermHandler::SimpleSigtermHandler(volatile bool &flag)
+{
+  CHECK(s_simple_flag == nullptr) << "Signal handler already installed";
+  s_simple_flag = &flag;
+
+  // note: if one of the functions below throws,
+  // s_simple_flag will remain set since the destructor will not be called
+  // however, an error here will lead to process termination anyway
+  installSignalHandlerSimple(SIGHUP);
+  installSignalHandlerSimple(SIGTERM);
+  installSignalHandlerSimple(SIGINT);
+}
+
+SimpleSigtermHandler::~SimpleSigtermHandler()
+{
+  clearSignalHandlerSimple(SIGINT);
+  clearSignalHandlerSimple(SIGTERM);
+  clearSignalHandlerSimple(SIGHUP);
+
+  s_simple_flag = nullptr;
+}
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_common/src/test_thread_blocking.cpp b/RWR/src/ze_oss/ze_common/src/test_thread_blocking.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..01dbc44164d7f4a54688a59196ecdd43ab4ce8bb
--- /dev/null
+++ b/RWR/src/ze_oss/ze_common/src/test_thread_blocking.cpp
@@ -0,0 +1,55 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include <ze/common/test_thread_blocking.hpp>
+#include <thread>
+#include <chrono>
+#include <ze/common/logging.hpp>
+
+namespace ze {
+
+//------------------------------------------------------------------------------
+void BlockingTest::runBlockingTest(unsigned testId, unsigned timeout)
+{
+  test_id_ = testId;
+  std::thread thread(&BlockingTest::runThread, this);
+
+  std::this_thread::sleep_for(std::chrono::milliseconds(timeout));
+  CHECK(!finished_) << "Thread was not blocked";
+  performUnblockingAction(testId);
+
+  std::this_thread::sleep_for(std::chrono::milliseconds(timeout));
+  CHECK(finished_) << "Thread was not unblocked";
+  thread.join();
+}
+
+//------------------------------------------------------------------------------
+void BlockingTest::runThread()
+{
+  performBlockingAction(testId());
+  setFinished();
+}
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_common/src/test_utils.cpp b/RWR/src/ze_oss/ze_common/src/test_utils.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..3342984968c8c05d86f98d5d66a40107a88beda8
--- /dev/null
+++ b/RWR/src/ze_oss/ze_common/src/test_utils.cpp
@@ -0,0 +1,85 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include <ze/common/test_utils.hpp>
+
+#include <cstdlib>
+#include <ze/common/logging.hpp>
+#include <ze/common/types.hpp>
+#include <ze/common/path_utils.hpp>
+#include <ze/common/string_utils.hpp>
+#include <ze/common/file_utils.hpp>
+
+namespace ze {
+
+std::string getTestDataDir(const std::string& dataset_name)
+{
+  const char* datapath_dir = std::getenv("ZE_TEST_DATA_PATH");
+  CHECK(datapath_dir != nullptr)
+      << "Did you download the ze_test_data repository and set "
+      << "the ZE_TEST_DATA_PATH environment variable?";
+
+  std::string path(datapath_dir);
+  CHECK(isDir(path)) << "Folder does not exist: " << path;
+  path = path + "/data/" + dataset_name;
+  CHECK(isDir(path)) << "Dataset does not exist: " << path;
+  return path;
+}
+
+std::map<int64_t, Transformation> loadIndexedPosesFromCsv(const std::string& filename)
+{
+  std::map<int64_t, Transformation> poses;
+  std::ifstream fs;
+  openFileStream(filename, &fs);
+  std::string line;
+  while(std::getline(fs, line))
+  {
+    std::vector<std::string> items = splitString(line, ',');
+    CHECK_EQ(items.size(), 8u);
+    int64_t stamp = std::stoll(items[0]);
+    Vector3 t(std::stod(items[1]), std::stod(items[2]), std::stod(items[3]));
+    Quaternion q(std::stod(items[7]), std::stod(items[4]), std::stod(items[5]), std::stod(items[6]));
+    poses[stamp] = Transformation(q, t);
+  }
+  return poses;
+}
+
+void loadDepthmapFromFile(
+    const std::string& filename, const size_t data_size, float* data)
+{
+  CHECK_NOTNULL(data);
+  std::ifstream fs;
+  openFileStream(filename, &fs);
+  float* data_ptr = data;
+  for(size_t i = 0; i < data_size; ++i, ++data_ptr)
+  {
+    fs >> (*data_ptr);
+    CHECK(fs.peek() != '\n' || i == data_size - 1)
+        << "Did not read full depthmap. Num elements read = " <<  i
+        << " of expected " << data_size;
+  }
+}
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_common/src/thread_pool.cpp b/RWR/src/ze_oss/ze_common/src/thread_pool.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..59ea3f8b33111e778d674d6286dc364658b6db42
--- /dev/null
+++ b/RWR/src/ze_oss/ze_common/src/thread_pool.cpp
@@ -0,0 +1,75 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include <ze/common/thread_pool.hpp>
+
+namespace ze {
+
+void ThreadPool::startThreads(size_t threads)
+{
+  for(size_t i = 0;i<threads;++i)
+  {
+    workers_.emplace_back(
+      [this] {
+
+        // Thread loop:
+        while (true)
+        {
+          std::function<void()> task;
+
+          // Wait for next task.
+          {
+            std::unique_lock<std::mutex> lock(this->queue_mutex_);
+            this->condition_.wait(lock, [this]{ return this->stop_ || !this->tasks_.empty(); });
+            if (this->stop_ && this->tasks_.empty())
+            {
+              return;
+            }
+            task = std::move(this->tasks_.front());
+            this->tasks_.pop();
+          }
+
+          // Execute task.
+          task();
+        }
+      }
+    );
+  }
+}
+
+ThreadPool::~ThreadPool()
+{
+  {
+    std::unique_lock<std::mutex> lock(queue_mutex_);
+    stop_ = true;
+  }
+  condition_.notify_all();
+  for (std::thread& worker : workers_)
+  {
+    worker.join();
+  }
+}
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_common/test/test_benchmark.cpp b/RWR/src/ze_oss/ze_common/test/test_benchmark.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f5c1a38dd846774214b1de9635d8ae5f37f49b7e
--- /dev/null
+++ b/RWR/src/ze_oss/ze_common/test/test_benchmark.cpp
@@ -0,0 +1,52 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include <iostream>
+#include <string>
+
+#include <ze/common/test_entrypoint.hpp>
+#include <ze/common/benchmark.hpp>
+
+int foo(int x, int y)
+{
+  return x*x + y*y;
+}
+
+void printOut(const std::string& s)
+{
+  VLOG(10) << "Run dummy benchmark";
+}
+
+TEST(BenchmarkTest, testInterface)
+{
+  using namespace ze;
+  auto fun1 = std::bind(printOut, "a");
+  runTimingBenchmark(fun1, 5, 2);
+
+  auto fun2 = std::bind(foo, 1, 2);
+  int64_t duration_ns = runTimingBenchmark(fun2, 1000, 100, "foo", true);
+}
+
+ZE_UNITTEST_ENTRYPOINT
diff --git a/RWR/src/ze_oss/ze_common/test/test_buffer.cpp b/RWR/src/ze_oss/ze_common/test/test_buffer.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..39da900cdb9f5a95860a8e4252222bd829891380
--- /dev/null
+++ b/RWR/src/ze_oss/ze_common/test/test_buffer.cpp
@@ -0,0 +1,158 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include <string>
+#include <vector>
+
+#include <ze/common/buffer.hpp>
+#include <ze/common/test_entrypoint.hpp>
+
+
+TEST(BufferTest, testRemoveOlderThanTimestamp)
+{
+  ze::Buffer<double, 2> buffer;
+  for(int i = 1; i < 10; ++i)
+  {
+    buffer.insert(i, Eigen::Vector2d(i, i));
+  }
+
+  buffer.removeDataBeforeTimestamp(3);
+  buffer.lock();
+  EXPECT_EQ(buffer.data().begin()->first, 3);
+  EXPECT_EQ(buffer.data().rbegin()->first, 9);
+  buffer.unlock();
+}
+
+TEST(BufferTest, testRemoveOlderThan)
+{
+  ze::Buffer<double, 2> buffer;
+  for(int i = 1; i < 10; ++i)
+  {
+    buffer.insert(ze::secToNanosec(i), Eigen::Vector2d(i, i));
+  }
+
+  buffer.removeDataOlderThan(3.0);
+  buffer.lock();
+  EXPECT_EQ(buffer.data().begin()->first, ze::secToNanosec(6));
+  EXPECT_EQ(buffer.data().rbegin()->first, ze::secToNanosec(9));
+  buffer.unlock();
+}
+
+TEST(BufferTest, testIterator)
+{
+  ze::Buffer<double, 2> buffer;
+  for(int i = 1; i < 10; ++i)
+  {
+    buffer.insert(ze::secToNanosec(i), Eigen::Vector2d(i, i));
+  }
+
+  buffer.lock();
+
+  // Check before/after
+  EXPECT_EQ(buffer.iterator_equal_or_before(ze::secToNanosec(3.5))->first,
+            ze::secToNanosec(3));
+  EXPECT_EQ(buffer.iterator_equal_or_after(ze::secToNanosec(3.5))->first,
+            ze::secToNanosec(4));
+
+  // Check equal
+  EXPECT_EQ(buffer.iterator_equal_or_before(ze::secToNanosec(3))->first,
+            ze::secToNanosec(3));
+  EXPECT_EQ(buffer.iterator_equal_or_after(ze::secToNanosec(4))->first,
+            ze::secToNanosec(4));
+
+  // Expect out of range:
+  EXPECT_EQ(buffer.iterator_equal_or_before(ze::secToNanosec(0.8)),
+            buffer.data().end());
+  EXPECT_EQ(buffer.iterator_equal_or_before(ze::secToNanosec(9.1)),
+            (--buffer.data().end()));
+  EXPECT_EQ(buffer.iterator_equal_or_after(ze::secToNanosec(9.1)),
+            buffer.data().end());
+  EXPECT_EQ(buffer.iterator_equal_or_after(ze::secToNanosec(0.8)),
+            buffer.data().begin());
+
+  buffer.unlock();
+}
+
+TEST(BufferTest, testNearestValue)
+{
+  ze::Buffer<double, 2> buffer;
+  EXPECT_FALSE(std::get<2>(buffer.getNearestValue(ze::secToNanosec(1))));
+
+  for(int i = 1; i < 10; ++i)
+  {
+    buffer.insert(ze::secToNanosec(i), Eigen::Vector2d(i, i));
+  }
+
+  EXPECT_EQ(std::get<1>(buffer.getNearestValue(ze::secToNanosec(1)))[0], 1);
+  EXPECT_EQ(std::get<1>(buffer.getNearestValue(ze::secToNanosec(0.4)))[0], 1);
+  EXPECT_EQ(std::get<1>(buffer.getNearestValue(ze::secToNanosec(1.4)))[0], 1);
+  EXPECT_EQ(std::get<1>(buffer.getNearestValue(ze::secToNanosec(11.0)))[0], 9);
+}
+
+TEST(BufferTest, testOldestNewestValue)
+{
+  ze::Buffer<double, 2> buffer;
+  EXPECT_FALSE(buffer.getOldestValue().second);
+  EXPECT_FALSE(buffer.getNewestValue().second);
+
+  for(int i = 1; i < 10; ++i)
+  {
+    buffer.insert(ze::secToNanosec(i), Eigen::Vector2d(i, i));
+  }
+
+  EXPECT_EQ(buffer.getNewestValue().first[0], 9);
+  EXPECT_EQ(buffer.getOldestValue().first[0], 1);
+}
+
+TEST(BufferTest, testInterpolation)
+{
+  using namespace ze;
+
+  Buffer<real_t, 2> buffer;
+
+  for(int i = 0; i < 10; ++i)
+    buffer.insert(secToNanosec(i), Vector2(i, i));
+
+  Eigen::Matrix<int64_t, Eigen::Dynamic, 1> stamps;
+  Eigen::Matrix<real_t, 2, Eigen::Dynamic> values;
+  std::tie(stamps, values) = buffer.getBetweenValuesInterpolated(
+        secToNanosec(1.2), secToNanosec(5.4));
+
+  EXPECT_EQ(stamps.size(), values.cols());
+  EXPECT_EQ(stamps.size(), 6);
+  EXPECT_EQ(stamps(0), secToNanosec(1.2));
+  EXPECT_EQ(stamps(stamps.size()-1), secToNanosec(5.4));
+  EXPECT_FLOATTYPE_EQ(values(0, 0), 1.2);
+  EXPECT_FLOATTYPE_EQ(values(0, stamps.size()-1), 5.4);
+
+  std::tie(stamps, values) = buffer.getBetweenValuesInterpolated(
+        secToNanosec(0), secToNanosec(9));
+  EXPECT_EQ(stamps(0), secToNanosec(0));
+  EXPECT_EQ(stamps(stamps.size()-1), secToNanosec(9));
+  EXPECT_FLOATTYPE_EQ(values(0, 0), 0);
+  EXPECT_FLOATTYPE_EQ(values(0, stamps.size()-1), 9);
+}
+
+ZE_UNITTEST_ENTRYPOINT
diff --git a/RWR/src/ze_oss/ze_common/test/test_combinatorics.cpp b/RWR/src/ze_oss/ze_common/test/test_combinatorics.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..4ccd3dafd386ae4a7bcd2e06aadc8be19bab09b2
--- /dev/null
+++ b/RWR/src/ze_oss/ze_common/test/test_combinatorics.cpp
@@ -0,0 +1,87 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include <iostream>
+#include <ze/common/test_entrypoint.hpp>
+#include <ze/common/types.hpp>
+#include <ze/common/combinatorics.hpp>
+
+TEST(CombinatoricsTests, testGetMatchingIndices)
+{
+  using namespace ze;
+
+  // Create test data.
+  using LandmarkIds = Eigen::Matrix<int32_t, Eigen::Dynamic, 1>;
+  LandmarkIds ids_cur(10), ids_ref(7);
+  ids_cur << -1, 2, 3, 7, 9, -1, 10, 12, 4, 5;
+  ids_ref << -1, 1, 2, 3, 12, 9, -1;
+
+  // Find matches.
+  auto matches_cur_ref = getMatchIndices<int32_t>(
+        ids_cur, ids_ref, [](int32_t id) { return id != -1; });
+  EXPECT_EQ(matches_cur_ref.size(), 4u);
+  for(auto it : matches_cur_ref)
+  {
+    EXPECT_EQ(ids_cur(it.first), ids_ref(it.second));
+  }
+}
+
+TEST(CombinatoricsTests, tetstGetUnmatchedIndices)
+{
+  using namespace ze;
+
+  // Create test data.
+  using LandmarkIds = Eigen::Matrix<int32_t, Eigen::Dynamic, 1>;
+  LandmarkIds ids_cur(10), ids_ref(7);
+  ids_cur << -1, 2, 3, 7, 9, -1, 10, 12, 4, 5;
+  ids_ref << -1, 1, 2, 3, 12, 9, -1;
+
+  // Find unmached matches.
+  auto unmatched_cur = getUnmatchedIndices<int32_t>(
+        ids_cur, ids_ref, [](int32_t id) { return id != -1; });
+  EXPECT_EQ(unmatched_cur[0], 3u);
+  EXPECT_EQ(unmatched_cur[1], 6u);
+  EXPECT_EQ(unmatched_cur[2], 8u);
+  EXPECT_EQ(unmatched_cur[3], 9u);
+}
+
+TEST(CombinatoricsTests, testGetOutliersFromInliers1)
+{
+  std::vector<int> inliers { 0, 1, 5, 4 };
+  std::vector<int> outliers = ze::getOutlierIndicesFromInlierIndices<int>(inliers, 7);
+  ASSERT_EQ(outliers.size(), 3u);
+  EXPECT_EQ(outliers[0], 2);
+  EXPECT_EQ(outliers[1], 3);
+  EXPECT_EQ(outliers[2], 6);
+}
+
+TEST(CombinatoricsTests, testGetOutliersFromInliers2)
+{
+  std::vector<size_t> inliers { 0, 1, 2 };
+  auto outliers = ze::getOutlierIndicesFromInlierIndices<size_t>(inliers, 3);
+  ASSERT_TRUE(outliers.empty());
+}
+
+ZE_UNITTEST_ENTRYPOINT
diff --git a/RWR/src/ze_oss/ze_common/test/test_csv_trajectory.cpp b/RWR/src/ze_oss/ze_common/test/test_csv_trajectory.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..4167e159ba884f96cd614c927c346db6f47a5566
--- /dev/null
+++ b/RWR/src/ze_oss/ze_common/test/test_csv_trajectory.cpp
@@ -0,0 +1,92 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include <ze/common/buffer.hpp>
+#include <ze/common/csv_trajectory.hpp>
+#include <ze/common/file_utils.hpp>
+#include <ze/common/path_utils.hpp>
+#include <ze/common/test_entrypoint.hpp>
+#include <ze/common/test_utils.hpp>
+#include <ze/common/time_conversions.hpp>
+#include <ze/common/types.hpp>
+
+using namespace ze;
+
+TEST(TimeStampMatcher, matchTest)
+{
+  std::string test_data_dir = getTestDataDir("ze_ts_matching");
+  constexpr real_t offset_sec = 0.0;
+  constexpr real_t max_difference_sec = 0.02;
+
+  // Load groundtruth.
+  PoseSeries gt_poses;
+  gt_poses.load(joinPath(test_data_dir, "traj_gt.csv"));
+
+  // Load estimated trajectory.
+  SWEResultSeries es_poses;
+  es_poses.load(joinPath(test_data_dir, "traj_es.csv"));
+
+  // Load matches to compare against
+  std::map<int64_t, int64_t> matches;
+  std::ifstream fs;
+  openFileStream(joinPath(test_data_dir, "python_matches.csv"), &fs);
+  std::string line;
+  while (std::getline(fs, line))
+  {
+    if ('%' != line.at(0) && '#' != line.at(0))
+    {
+      std::vector<std::string> items = splitString(line, ',');
+      EXPECT_GE(items.size(), 2u);
+      int64_t stamp_es = std::stoll(items[0]);
+      int64_t stamp_gt = std::stoll(items[1]);
+      matches[stamp_es] = stamp_gt;
+    }
+  }
+
+  // Now loop through all estimated poses and find closest groundtruth-stamp.
+  auto& gt_buffer = gt_poses.getBuffer();
+  auto es_poses_data = es_poses.getStampedTransformationVector();
+  int n_skipped = 0, n_checked = 0;
+  for (const std::pair<int64_t, Transformation>& stampd_pose : es_poses_data)
+  {
+    Vector7 matched_gt_pose;
+    int64_t matched_gt_stamp;
+    if (!findNearestTimeStamp(gt_buffer, stampd_pose.first, matched_gt_stamp,
+                              matched_gt_pose, max_difference_sec, offset_sec))
+    {
+      ++n_skipped;
+    }
+    else
+    {
+      EXPECT_LE(std::abs(stampd_pose.first - matched_gt_stamp),
+                secToNanosec(max_difference_sec));
+      EXPECT_EQ(matches.find(stampd_pose.first)->second, matched_gt_stamp);
+      ++n_checked;
+    }
+  }
+  EXPECT_EQ(es_poses_data.size(), n_checked+n_skipped);
+}
+
+ZE_UNITTEST_ENTRYPOINT
diff --git a/RWR/src/ze_oss/ze_common/test/test_manifold.cpp b/RWR/src/ze_oss/ze_common/test/test_manifold.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ac1fd0dfe6c3adbffa36f0761d3ac0c36103e529
--- /dev/null
+++ b/RWR/src/ze_oss/ze_common/test/test_manifold.cpp
@@ -0,0 +1,140 @@
+#include <ze/common/manifold.hpp>
+#include <ze/common/numerical_derivative.hpp>
+#include <ze/common/test_entrypoint.hpp>
+
+using namespace ze;
+
+// -----------------------------------------------------------------------------
+// Test manifold concepts.
+template<typename T>
+void testManifoldInvariants(const T& a, const T& b, real_t tol = 1e-9)
+{
+  EXPECT_TRUE(traits<T>::equals(a, a));
+  typename traits<T>::TangentVector v = traits<T>::local(a, b);
+  T c = traits<T>::retract(a, v);
+  EXPECT_TRUE(traits<T>::equals(b, c, tol));
+}
+
+template<typename T>
+void testRetractJacobians(const T& a, const T& b, real_t tol = 1e-9)
+{
+  using namespace std::placeholders; // for _1
+  typename traits<T>::Jacobian H1, H2;
+  typename traits<T>::TangentVector v = traits<T>::local(a, b);
+  T c = traits<T>::retract(a, v, &H1, &H2);
+  ASSERT_TRUE(traits<T>::equals(b, c, tol));
+
+  typename traits<T>::Jacobian H1_numerical =
+      numericalDerivative<T, T>(
+        std::bind(traits<T>::retract, _1, v, nullptr, nullptr), a);
+  EXPECT_TRUE(EIGEN_MATRIX_NEAR(H1, H1_numerical, tol));
+
+  typename traits<T>::Jacobian H2_numerical =
+      numericalDerivative<T, typename traits<T>::TangentVector>(
+        std::bind(traits<T>::retract, a, _1, nullptr, nullptr), v);
+  EXPECT_TRUE(EIGEN_MATRIX_NEAR(H2, H2_numerical, tol));
+}
+
+template<typename T>
+void testLocalJacobians(const T& a, const T& b, real_t tol = 1e-9)
+{
+  using namespace std::placeholders; // for _1
+  typename traits<T>::Jacobian H1, H2;
+  typename traits<T>::TangentVector v = traits<T>::local(a, b, &H1, &H2);
+  typename traits<T>::Jacobian H1_numerical =
+      numericalDerivative<typename traits<T>::TangentVector, T>(
+        std::bind(traits<T>::local, _1, b, nullptr, nullptr), a);
+  EXPECT_TRUE(EIGEN_MATRIX_NEAR(H1, H1_numerical, tol));
+
+  typename traits<T>::Jacobian H2_numerical =
+      numericalDerivative<typename traits<T>::TangentVector, T>(
+        std::bind(traits<T>::local, a, _1, nullptr, nullptr), b);
+  EXPECT_TRUE(EIGEN_MATRIX_NEAR(H2, H2_numerical, tol));
+}
+
+#ifndef ZE_SINGLE_PRECISION_FLOAT
+constexpr real_t tol = 1e-9;
+#else
+constexpr real_t tol = 1e-7;
+#endif
+
+TEST(ManifoldTests, testScalarTraits)
+{
+  EXPECT_EQ(traits<real_t>::dimension, 1);
+  testManifoldInvariants<real_t>(1.0, 1.5);
+#ifndef ZE_SINGLE_PRECISION_FLOAT
+  testRetractJacobians<real_t>(1.0, 1.5);
+  testLocalJacobians<real_t>(1.0, 1.5);
+#else
+  LOG(WARNING) << "Numerical derivative test ignored for single precision float.";
+#endif
+}
+
+TEST(ManifoldTests, testEigenTraits)
+{
+  EXPECT_EQ(traits<Vector3>::dimension, 3);
+  testManifoldInvariants<Vector3>(
+        Vector3(1.0, 1.2, 1.3), Vector3(2.0, 1.0, 0.0));
+#ifndef ZE_SINGLE_PRECISION_FLOAT
+  testRetractJacobians<Vector3>(
+        Vector3(1.0, 1.2, 1.3), Vector3(2.0, 1.0, 0.0));
+  testLocalJacobians<Vector3>(
+        Vector3(1.0, 1.2, 1.3), Vector3(2.0, 1.0, 0.0));
+#else
+  LOG(WARNING) << "Numerical derivative test ignored for single precision float.";
+#endif
+}
+
+TEST(ManifoldTests, testManifoldSO3Invariants)
+{
+  EXPECT_EQ(traits<Quaternion>::dimension, 3);
+  testManifoldInvariants<Quaternion>(
+        Quaternion(Vector3(0.1, 0.2, 0.3)),
+        Quaternion(Vector3(0.2, 0.3, 0.4)), tol);
+}
+
+#ifndef ZE_SINGLE_PRECISION_FLOAT
+TEST(ManifoldTests, testManifoldSO3Retract)
+{
+  testRetractJacobians<Quaternion>(
+        Quaternion(Vector3(0.1, 0.2, 0.3)),
+        Quaternion(Vector3(0.2, 0.3, 0.4)));
+}
+
+TEST(ManifoldTests, testManifoldSO3Local)
+{
+  testLocalJacobians<Quaternion>(
+        Quaternion(Vector3(0.1, 0.2, 0.3)),
+        Quaternion(Vector3(0.2, 0.3, 0.4)));
+}
+#endif
+
+TEST(ManifoldTests, testManifoldSE3Invariants)
+{
+  EXPECT_EQ(traits<Transformation>::dimension, 6);
+  testManifoldInvariants<Transformation>(
+        Transformation().setRandom(),
+        Transformation().setRandom(), tol);
+}
+
+#ifndef ZE_SINGLE_PRECISION_FLOAT
+TEST(ManifoldTests, testManifoldSE3LocalJacobians)
+{
+  Transformation origin;
+  origin.setRandom();
+  Transformation other = origin * Transformation().setRandom(0.2, 0.2);
+  testLocalJacobians<Transformation>(origin, other);
+}
+
+TEST(ManifoldTests, testManifoldSE3RetractJacobians)
+{
+  Transformation origin, other;
+  origin.setRandom(1.2, 0.8);
+  other.setRandom();
+  testRetractJacobians<Transformation>(origin, other);
+  Vector6 v = Transformation::log(origin.inverse() * other);
+  EXPECT_TRUE(traits<Vector6>::equals(v, traits<Transformation>::local(origin, other)));
+}
+#endif
+
+ZE_UNITTEST_ENTRYPOINT
diff --git a/RWR/src/ze_oss/ze_common/test/test_matrix.cpp b/RWR/src/ze_oss/ze_common/test/test_matrix.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e13b3e1ce5dac4827ba4e15de0ddcb176808c70f
--- /dev/null
+++ b/RWR/src/ze_oss/ze_common/test/test_matrix.cpp
@@ -0,0 +1,63 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include <cmath>
+
+#include <ze/common/test_entrypoint.hpp>
+#include <ze/common/matrix.hpp>
+#include <ze/common/types.hpp>
+
+TEST(MatrixTests, testVectorSlice)
+{
+  using namespace ze;
+
+  VectorX M(5);
+  M << 1, 2, 3, 4, 6;
+  std::vector<uint32_t> indices { 0, 2, 3 };
+
+  M = getVectorElements(M, indices);
+
+  VectorX A_expected(3);
+  A_expected << 1, 3, 4;
+  EXPECT_TRUE(EIGEN_MATRIX_EQUAL_DOUBLE(M, A_expected));
+}
+
+TEST(MatrixTests, testColumnSlice)
+{
+  using namespace ze;
+
+  Matrix2X M(2, 5);
+  M << 1, 2, 3, 4, 6,
+       7, 8, 9, 10, 11;
+  std::vector<uint32_t> indices { 0, 2, 3 };
+
+  M = getMatrixCols(M, indices);
+
+  Matrix2X A_expected(2,3);
+  A_expected << 1, 3, 4, 7, 9, 10;
+  EXPECT_TRUE(EIGEN_MATRIX_EQUAL_DOUBLE(M, A_expected));
+}
+
+ZE_UNITTEST_ENTRYPOINT
diff --git a/RWR/src/ze_oss/ze_common/test/test_numerical_derivative.cpp b/RWR/src/ze_oss/ze_common/test/test_numerical_derivative.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..537e9267cdafd2ae7c427e8bcea903f0ae936853
--- /dev/null
+++ b/RWR/src/ze_oss/ze_common/test/test_numerical_derivative.cpp
@@ -0,0 +1,48 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include <cmath>
+
+#include <ze/common/types.hpp>
+#include <ze/common/test_entrypoint.hpp>
+#include <ze/common/numerical_derivative.hpp>
+
+TEST(NumericalDerivativeTests, testLinearVector)
+{
+#ifndef ZE_SINGLE_PRECISION_FLOAT
+  using namespace ze;
+
+  Matrix2 J = numericalDerivative<Vector2, Vector2>(
+        [](const Vector2& x) { return x * 1.2; }, Vector2(1, 2));
+  EXPECT_NEAR(J(0,0), 1.2, 1e-8);
+  EXPECT_NEAR(J(1,1), 1.2, 1e-8);
+  EXPECT_NEAR(J(0,1), 0.0, 1e-8);
+  EXPECT_NEAR(J(1,0), 0.0, 1e-8);
+#else
+  LOG(WARNING) << "Test ignored for single precision float.";
+#endif
+}
+
+ZE_UNITTEST_ENTRYPOINT
diff --git a/RWR/src/ze_oss/ze_common/test/test_random.cpp b/RWR/src/ze_oss/ze_common/test/test_random.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..855dc0e56bf8933463a5fc737f8e8a9e32ad93d5
--- /dev/null
+++ b/RWR/src/ze_oss/ze_common/test/test_random.cpp
@@ -0,0 +1,149 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include <ze/common/benchmark.hpp>
+#include <ze/common/test_entrypoint.hpp>
+#include <ze/common/random.hpp>
+#include <ze/common/running_statistics.hpp>
+
+TEST(RandomTests, testRandomSampling)
+{
+  using namespace ze;
+
+  // Deterministic sampling results always the same series of random numbers.
+  EXPECT_EQ(sampleUniformIntDistribution<uint8_t>(true), 140u);
+  EXPECT_EQ(sampleUniformIntDistribution<uint8_t>(true), 151u);
+  EXPECT_EQ(sampleUniformIntDistribution<uint8_t>(true), 183u);
+
+  EXPECT_EQ(sampleUniformIntDistribution<int>(true), 209652396);
+  EXPECT_EQ(sampleUniformIntDistribution<int>(true), 398764591);
+  EXPECT_EQ(sampleUniformIntDistribution<int>(true), 924231285);
+
+  EXPECT_NEAR(sampleUniformRealDistribution<double>(true), 0.592844, 1e-5);
+  EXPECT_NEAR(sampleUniformRealDistribution<double>(true), 0.844265, 1e-5);
+  EXPECT_NEAR(sampleUniformRealDistribution<double>(true), 0.857945, 1e-5);
+
+  EXPECT_NEAR(sampleNormalDistribution<double>(true, 1.0, 4.0), 5.4911797, 1e-5);
+  EXPECT_NEAR(sampleNormalDistribution<double>(true, 1.0, 4.0), 1.2834369, 1e-5);
+  EXPECT_NEAR(sampleNormalDistribution<double>(true, 1.0, 4.0), -4.689303, 1e-5);
+
+  EXPECT_TRUE (flipCoin(true, 0.7));
+  EXPECT_FALSE(flipCoin(true, 0.7));
+  EXPECT_FALSE(flipCoin(true, 0.7));
+  EXPECT_FALSE(flipCoin(true, 0.7));
+  EXPECT_TRUE (flipCoin(true, 0.7));
+  EXPECT_TRUE (flipCoin(true, 0.7));
+
+  // Non-deterministic sampling, always results in different numbers:
+  EXPECT_NE(sampleUniformIntDistribution<int>(false), 209652396);
+  EXPECT_NE(sampleUniformIntDistribution<int>(false), 398764591);
+  EXPECT_NE(sampleUniformIntDistribution<int>(false), 924231285);
+
+  // Test mean and standard deviation of normal distribution.
+  {
+    RunningStatistics statistics;
+    for (int i = 0; i < 10000; ++i)
+    {
+      statistics.addSample(sampleNormalDistribution<double>(false, 2.0, 5.0));
+    }
+    EXPECT_NEAR(statistics.mean(), 2.0, 0.2);
+    EXPECT_NEAR(statistics.std(),  5.0, 0.2);
+  }
+
+  // Test coin flips.
+  {
+    RunningStatistics statistics;
+    for (int i = 0; i < 10000; ++i)
+    {
+      statistics.addSample(static_cast<int>(flipCoin(false, 0.2)));
+    }
+    EXPECT_NEAR(statistics.mean(), 0.2, 0.2);
+  }
+}
+
+TEST(RandomTests, testDistribution)
+{
+  using namespace ze;
+
+  // Deterministic sampling results always the same series of random numbers.
+  {
+    auto f = uniformDistribution<uint8_t>(true);
+    EXPECT_EQ(f(), 140u);
+    EXPECT_EQ(f(), 151u);
+    EXPECT_EQ(f(), 183u);
+  }
+
+  // Deterministic sampling results always the same series of random numbers.
+  {
+    auto f = uniformDistribution<double>(true, 1.0, 2.0);
+    EXPECT_NEAR(f(), 1.59284, 1e-5);
+    EXPECT_NEAR(f(), 1.84427, 1e-5);
+    EXPECT_NEAR(f(), 1.85795, 1e-5);
+  }
+
+  // Deterministic sampling results always the same series of random numbers.
+  {
+    auto f = normalDistribution<float>(true, 3.0, 5.0);
+    EXPECT_NEAR(f(), 14.06103, 1e-5);
+    EXPECT_NEAR(f(), 8.81539, 1e-5);
+    EXPECT_NEAR(f(), 6.87001, 1e-5);
+  }
+}
+
+TEST(RandomTests, benchmark)
+{
+  using namespace ze;
+
+  auto lambda1 = [&]()
+  {
+    int sum = 0;
+    for (int i = 0; i < 100000; ++i)
+      sum += sampleUniformIntDistribution<uint8_t>(false);
+  };
+  runTimingBenchmark(lambda1, 10, 10, "sampleSeparately", true);
+
+  auto lambda2 = [&]()
+  {
+    int sum = 0;
+    auto dist = uniformDistribution<uint8_t>(false);
+    for (int i = 0; i < 100000; ++i)
+      sum += dist();
+  };
+  runTimingBenchmark(lambda2, 10, 10, "sampleFromDistribution", true);
+
+  auto lambda3 = [&]()
+  {
+    int sum = 0;
+    static std::mt19937 gen_deterministic(0);
+    std::uniform_int_distribution<uint8_t> distribution(0, 255);
+    for (int i = 0; i < 100000; ++i)
+    {
+      sum += distribution(gen_deterministic);
+    }
+  };
+  runTimingBenchmark(lambda3, 10, 10, "Using std interface", true);
+}
+
+ZE_UNITTEST_ENTRYPOINT
diff --git a/RWR/src/ze_oss/ze_common/test/test_random_matrix.cpp b/RWR/src/ze_oss/ze_common/test/test_random_matrix.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..2931c796717d442c989b18176cbe1596e778ce86
--- /dev/null
+++ b/RWR/src/ze_oss/ze_common/test/test_random_matrix.cpp
@@ -0,0 +1,67 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include <ze/common/benchmark.hpp>
+#include <ze/common/test_entrypoint.hpp>
+#include <ze/common/random_matrix.hpp>
+#include <ze/common/running_statistics.hpp>
+
+TEST(RandomMatrixTests, testRandomVectorSampler)
+{
+  using namespace ze;
+
+  Vector2 var_vector;
+  var_vector << 2, 3;
+  RandomVectorSampler<2>::Ptr sampler = RandomVectorSampler<2>::variances(var_vector, true);
+  RandomVectorSampler<3>::Ptr sampler2 = RandomVectorSampler<3>::sigmas(Vector3(1, 2, 3));
+
+  Vector2 sample = sampler->sample();
+  Vector3 sample2 = sampler2->sample();
+}
+
+TEST(RandomMatrixTests, testRandomVector)
+{
+  using namespace ze;
+
+  VLOG(1) << "Deterministic:";
+  Vector2 v1 = randomVectorUniformDistributed<2>(true);
+  EXPECT_NEAR(v1(0), 0.592845, 1e-5);
+  EXPECT_NEAR(v1(1), 0.844266, 1e-5);
+  Vector2 v2 = randomVectorUniformDistributed<2>(true);
+  EXPECT_NEAR(v2(0), 0.857946, 1e-5);
+  EXPECT_NEAR(v2(1), 0.847252, 1e-5);
+
+  VLOG(1) << "\n" << randomMatrixUniformDistributed<2,3>(true);
+  VLOG(1) << "\n" << randomMatrixNormalDistributed<2,3>(true);
+  VLOG(1) << "\n" << randomVectorNormalDistributed<4>(true).transpose();
+  VLOG(1) << "\n" << randomMatrixNormalDistributed(2, 3, true);
+  VLOG(1) << "Nondeterministic:";
+  VLOG(1) << "\n" << randomMatrixUniformDistributed<2,3>();
+  VLOG(1) << "\n" << randomMatrixNormalDistributed<2,3>();
+  VLOG(1) << "\n" << randomVectorNormalDistributed<4>().transpose();
+  VLOG(1) << "\n" << randomMatrixNormalDistributed(2, 3);
+}
+
+ZE_UNITTEST_ENTRYPOINT
diff --git a/RWR/src/ze_oss/ze_common/test/test_ring_view.cpp b/RWR/src/ze_oss/ze_common/test/test_ring_view.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a9fc9e1adcfe0e75a1bbb197b0aad40b884c12c5
--- /dev/null
+++ b/RWR/src/ze_oss/ze_common/test/test_ring_view.cpp
@@ -0,0 +1,175 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include <cmath>
+
+#include <ze/common/types.hpp>
+#include <ze/common/test_entrypoint.hpp>
+#include <ze/common/benchmark.hpp>
+#include <ze/common/ring_view.hpp>
+
+DEFINE_bool(run_benchmark, false, "Benchmark the buffer vs. ringbuffer");
+
+TEST(RingViewTest, testFullRingScalar)
+{
+  using namespace ze;
+
+  std::vector<int> vec(100);
+  ring_view<int> rv(vec.begin(), vec.end());
+  ASSERT_EQ(100, rv.capacity());
+  ASSERT_EQ(100, rv.size());
+  ASSERT_EQ(99, rv.back_idx());
+  rv.push_back(1);
+  ASSERT_EQ(100, rv.capacity());
+  ASSERT_EQ(100, rv.size());
+  ASSERT_EQ(0, rv.back_idx());
+  rv.pop_front();
+  ASSERT_EQ(100, rv.capacity());
+  ASSERT_EQ(99, rv.size());
+  ASSERT_EQ(0, rv.back_idx());
+}
+
+TEST(RingViewTest, testFullRingScalarFixedSIze)
+{
+  using namespace ze;
+
+  std::vector<int> vec(100);
+  ring_view<int, 100> rv(vec.begin(), vec.end());
+  EXPECT_EQ(100, rv.capacity());
+}
+
+TEST(RingViewTest, testEmptyRingScalar)
+{
+  using namespace ze;
+
+  std::vector<int> vec(100);
+  ring_view<int> rv(vec.begin(), vec.end(), vec.begin(), 0);
+  ASSERT_EQ(100, rv.capacity());
+  ASSERT_EQ(0, rv.size());
+  rv.push_back(1);
+  ASSERT_EQ(1, rv.at(0));
+  ASSERT_EQ(100, rv.capacity());
+  ASSERT_EQ(1, rv.size());
+  rv.pop_front();
+  ASSERT_EQ(100, rv.capacity());
+  ASSERT_EQ(0, rv.size());
+}
+
+TEST(RingViewTest, testRingEigenVector)
+{
+  using namespace ze;
+
+  Eigen::Matrix<real_t, 1, 4> data;
+  data << 1, 2, 3, 4;
+
+  ring_view<real_t> rv(data.data(), data.data()+4);
+  ASSERT_EQ(1, rv.at(0));
+  ASSERT_EQ(2, rv.at(1));
+  ASSERT_EQ(3, rv.at(2));
+  ASSERT_EQ(4, rv.at(3));
+
+  rv.push_back(7);
+  ASSERT_EQ(2, rv.at(0));
+  ASSERT_EQ(7, rv.at(3));
+}
+
+TEST(RingViewTest, testIteratorOperators)
+{
+  using namespace ze;
+  Eigen::Matrix<real_t, 1, 4> data;
+  data << 1, 2, 3, 4;
+
+  ring_view<real_t> rv(data.data(), data.data()+4);
+
+  auto iter1 = rv.begin();
+  auto iter2 = rv.end();
+
+  EXPECT_TRUE(iter1 < iter2);
+  EXPECT_TRUE(iter1 <= iter2);
+  EXPECT_TRUE(iter2 > iter1);
+  EXPECT_TRUE(iter2 >= iter1);
+  EXPECT_FALSE(iter1 == iter2);
+  EXPECT_TRUE(iter1 == iter1);
+  EXPECT_TRUE(iter1 != iter2);
+
+  iter1 += 2;
+  iter2 -= 2;
+
+  EXPECT_TRUE(iter1 == iter2);
+}
+
+TEST(RingViewTest, testResetFront)
+{
+  using namespace ze;
+
+  std::vector<int> vec = {1, 2, 3, 4};
+  ring_view<int> rv(vec.begin(), vec.end());
+
+  EXPECT_EQ(4, rv.size());
+  EXPECT_EQ(0, rv.begin().index());
+  EXPECT_EQ(4, rv.end().index());
+
+  rv.reset_front(2);
+  EXPECT_EQ(2, rv.size());
+  EXPECT_EQ(3, rv.at(0));
+
+  rv.reset_front(0);
+  EXPECT_EQ(4, rv.size());
+  EXPECT_EQ(1, rv.at(0));
+}
+
+TEST(RingViewTest, benchmarkFixedVsDynamicSize)
+{
+  if (!FLAGS_run_benchmark) {
+    return;
+  }
+
+  using namespace ze;
+
+  std::vector<int> vec(100);
+  ring_view<int> rv1(vec.begin(), vec.end());
+  ring_view<int, 100> rv2(vec.begin(), vec.end());
+
+  std::vector<int> vec2(128);
+  ring_view<int> rv3(vec2.begin(), vec2.end());
+  ring_view<int, 128> rv4(vec2.begin(), vec2.end());
+
+  //////
+  // access
+  auto atFixed = [&]() { rv2.at(26); };
+  real_t atFixed_r = runTimingBenchmark(atFixed, 1000, 20, "At Fixed", true);
+  auto atDynamic = [&]() { rv1.at(26); };
+  real_t atDynamic_r = runTimingBenchmark(atDynamic, 1000, 20, "At Fixed", true);
+  auto atFixed_128 = [&]() { rv4.at(26); };
+  real_t atFixed128_r = runTimingBenchmark(atFixed_128, 1000, 20, "At Fixed", true);
+  auto atDynamic_128 = [&]() { rv3.at(26); };
+  real_t atDynamic128_r = runTimingBenchmark(atDynamic_128, 1000, 20, "At Fixed", true);
+
+  VLOG(1) << "Fixed: " << atFixed_r << ", Fixed128: " << atFixed128_r << "\n";
+  VLOG(1) << "Dynamic: " << atDynamic_r << ", Dynamic128: " << atDynamic128_r << "\n";
+
+}
+ZE_UNITTEST_ENTRYPOINT
+
diff --git a/RWR/src/ze_oss/ze_common/test/test_ringbuffer.cpp b/RWR/src/ze_oss/ze_common/test/test_ringbuffer.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..4ddd78e646c3701e80ad4b7d18a36b0cbfaeafb4
--- /dev/null
+++ b/RWR/src/ze_oss/ze_common/test/test_ringbuffer.cpp
@@ -0,0 +1,493 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include <string>
+#include <vector>
+#include <iostream>
+
+#include <ze/common/benchmark.hpp>
+#include <ze/common/types.hpp>
+#include <ze/common/ringbuffer.hpp>
+#include <ze/common/buffer.hpp>
+#include <ze/common/test_entrypoint.hpp>
+
+DEFINE_bool(run_benchmark, false, "Benchmark the buffer vs. ringbuffer");
+
+using ze::real_t;
+
+TEST(RingBufferTest, testTimeAndDataSync)
+{
+  ze::Ringbuffer<real_t, 3, 10> buffer;
+  for(int i = 1; i < 8; ++i)
+  {
+    buffer.insert(i, Eigen::Vector3d(i, i, i));
+  }
+
+  buffer.lock();
+
+  // check data order
+  auto data = buffer.data();
+  auto times = buffer.times();
+
+  for (int i = 1; i < 8; ++i)
+  {
+    EXPECT_EQ(i, times.at(i - 1));
+    EXPECT_EQ(i, data(0, i - 1));
+  }
+  buffer.unlock();
+
+  // close the circle
+  for (int i = 8; i < 15; ++i)
+  {
+    buffer.insert(i, Eigen::Vector3d(i, i, i));
+  }
+
+  buffer.lock();
+  data = buffer.data();
+
+  EXPECT_EQ(9, times.at(8)); EXPECT_EQ(9, data(0, 8));
+  EXPECT_EQ(10, times.at(9)); EXPECT_EQ(10, data(0, 9));
+  EXPECT_EQ(11, times.at(0)); EXPECT_EQ(11, data(0, 0));
+  EXPECT_EQ(12, times.at(1)); EXPECT_EQ(12, data(0, 1));
+  EXPECT_EQ(13, times.at(2)); EXPECT_EQ(13, data(0, 2));
+  EXPECT_EQ(14, times.at(3)); EXPECT_EQ(14, data(0, 3));
+  EXPECT_EQ(5, times.at(4)); EXPECT_EQ(5, data(0, 4));
+
+  buffer.unlock();
+}
+
+TEST(RingBufferTest, testLowerBound)
+{
+  ze::Ringbuffer<real_t, 2, 10> buffer;
+  for(int i = 1; i < 10; ++i)
+  {
+    buffer.insert(i, Eigen::Vector2d(i, i));
+  }
+  buffer.lock();
+
+  EXPECT_EQ(buffer.lower_bound(2), buffer.times().begin() + 1);
+  EXPECT_EQ(buffer.lower_bound(11), buffer.times().end());
+  EXPECT_EQ(buffer.lower_bound(10), buffer.times().end());
+  EXPECT_EQ(buffer.lower_bound(9), buffer.times().end() - 1);
+  EXPECT_EQ(buffer.lower_bound(0), buffer.times().begin());
+}
+
+TEST(RingBufferTest, testRemoveOlderThanTimestamp)
+{
+  ze::Ringbuffer<real_t, 3, 10> buffer;
+  for(int i = 1; i < 10; ++i)
+  {
+    buffer.insert(i, Eigen::Vector3d(i, i, i));
+  }
+
+  buffer.lock();
+  EXPECT_EQ(9, buffer.times().size());
+  buffer.unlock();
+
+  buffer.removeDataBeforeTimestamp(3);
+  buffer.lock();
+  EXPECT_EQ(3, buffer.times().front());
+  EXPECT_EQ(7, buffer.times().size());
+  buffer.unlock();
+}
+
+TEST(RingBufferTest, testRemoveOlderThan)
+{
+  ze::Ringbuffer<real_t, 2, 10> buffer;
+  for(int i = 1; i < 10; ++i)
+  {
+    buffer.insert(ze::secToNanosec(i), Eigen::Vector2d(i, i));
+  }
+
+  buffer.removeDataOlderThan(3.0);
+  buffer.lock();
+  EXPECT_EQ(ze::secToNanosec(6), buffer.times().front());
+  EXPECT_EQ(ze::secToNanosec(9), buffer.times().back());
+  buffer.unlock();
+}
+
+TEST(RingBufferTest, testIterator)
+{
+  ze::Ringbuffer<real_t, 2, 10> buffer;
+  for(int i = 1; i < 10; ++i)
+  {
+    buffer.insert(ze::secToNanosec(i), Eigen::Vector2d(i, i));
+  }
+
+  buffer.lock();
+
+  // Check before/after
+  EXPECT_EQ(*buffer.iterator_equal_or_before(ze::secToNanosec(3.5)),
+            ze::secToNanosec(3));
+  EXPECT_EQ(*buffer.iterator_equal_or_after(ze::secToNanosec(3.5)),
+            ze::secToNanosec(4));
+
+  // Check equal
+  EXPECT_EQ(*buffer.iterator_equal_or_before(ze::secToNanosec(3)),
+            ze::secToNanosec(3));
+  EXPECT_EQ(*buffer.iterator_equal_or_after(ze::secToNanosec(4)),
+            ze::secToNanosec(4));
+
+  // Expect out of range:
+  EXPECT_EQ(buffer.iterator_equal_or_before(ze::secToNanosec(0.8)),
+            buffer.times().end());
+  EXPECT_EQ(buffer.iterator_equal_or_before(ze::secToNanosec(9.1)),
+            (--buffer.times().end()));
+  EXPECT_EQ(buffer.iterator_equal_or_after(ze::secToNanosec(9.1)),
+            buffer.times().end());
+  EXPECT_EQ(buffer.iterator_equal_or_after(ze::secToNanosec(0.8)),
+            buffer.times().begin());
+
+  buffer.unlock();
+}
+
+TEST(RingBufferTest, testNearestValue)
+{
+  ze::Ringbuffer<real_t, 2, 10> buffer;
+  EXPECT_FALSE(std::get<2>(buffer.getNearestValue(ze::secToNanosec(1))));
+
+  for(int i = 1; i < 10; ++i)
+  {
+    buffer.insert(ze::secToNanosec(i), Eigen::Vector2d(i, i));
+  }
+
+  EXPECT_EQ(std::get<1>(buffer.getNearestValue(ze::secToNanosec(1)))[0], 1);
+  EXPECT_EQ(std::get<1>(buffer.getNearestValue(ze::secToNanosec(0.4)))[0], 1);
+  EXPECT_EQ(std::get<1>(buffer.getNearestValue(ze::secToNanosec(1.4)))[0], 1);
+  EXPECT_EQ(std::get<1>(buffer.getNearestValue(ze::secToNanosec(11.0)))[0], 9);
+}
+
+TEST(RingBufferTest, testOldestNewestValue)
+{
+  ze::Ringbuffer<real_t, 2, 10> buffer;
+  EXPECT_FALSE(buffer.getOldestValue().second);
+  EXPECT_FALSE(buffer.getNewestValue().second);
+
+  for(int i = 1; i < 10; ++i)
+  {
+    buffer.insert(ze::secToNanosec(i), Eigen::Vector2d(i, i));
+  }
+
+  EXPECT_EQ(buffer.getNewestValue().first[0], 9);
+  EXPECT_EQ(buffer.getOldestValue().first[0], 1);
+}
+
+TEST(RingBufferTest, testInterpolation)
+{
+  using namespace ze;
+
+  ze::Ringbuffer<real_t, 2, 10> buffer;
+
+  for(int i = 0; i < 10; ++i)
+  {
+    buffer.insert(secToNanosec(i), Vector2(i, i));
+  }
+
+  Eigen::Matrix<int64_t, Eigen::Dynamic, 1> stamps;
+  Eigen::Matrix<real_t, 2, Eigen::Dynamic> values;
+  std::tie(stamps, values) = buffer.getBetweenValuesInterpolated(
+        secToNanosec(1.2), secToNanosec(5.4));
+
+  EXPECT_EQ(stamps.size(), values.cols());
+  EXPECT_EQ(stamps.size(), 6);
+  EXPECT_EQ(stamps(0), secToNanosec(1.2));
+  EXPECT_EQ(stamps(stamps.size()-1), secToNanosec(5.4));
+  EXPECT_DOUBLE_EQ(values(0, 0), 1.2);
+  EXPECT_DOUBLE_EQ(values(0, stamps.size()-1), 5.4);
+
+  std::tie(stamps, values) = buffer.getBetweenValuesInterpolated(
+        secToNanosec(0), secToNanosec(9));
+  EXPECT_EQ(stamps(0), secToNanosec(0));
+  EXPECT_EQ(stamps(stamps.size()-1), secToNanosec(9));
+  EXPECT_DOUBLE_EQ(values(0, 0), 0);
+  EXPECT_DOUBLE_EQ(values(0, stamps.size()-1), 9);
+
+  // "overfill" the ring
+  for(int i = 10; i < 15; ++i)
+  {
+    buffer.insert(secToNanosec(i), Vector2(i, i));
+  }
+  // cross the buffer boundaries
+  std::tie(stamps, values) = buffer.getBetweenValuesInterpolated(
+        secToNanosec(8), secToNanosec(12));
+
+  // ensure consistency
+  for (int i = 8; i <= 12; ++i)
+  {
+    EXPECT_EQ(secToNanosec(i), stamps(i - 8));
+    EXPECT_EQ(i, values(0, i - 8));
+  }
+
+  // cross the buffer boundaries
+  std::tie(stamps, values) = buffer.getBetweenValuesInterpolated(
+        secToNanosec(7.5), secToNanosec(12.5));
+
+  for (int i = 8; i <= 12; ++i)
+  {
+    EXPECT_EQ(secToNanosec(i), stamps(i - 8 + 1));
+    EXPECT_EQ(i, values(0, i - 8 + 1));
+  }
+  // interpolated boundaries
+  EXPECT_EQ(secToNanosec(7.5), stamps(0));
+  EXPECT_EQ(secToNanosec(12.5), stamps(6));
+  EXPECT_EQ(7.5, values(0, 0));
+  EXPECT_EQ(12.5, values(0, 6));
+}
+
+TEST(RingBufferTest, testInterpolationTimestamps)
+{
+  using namespace ze;
+  ze::Ringbuffer<real_t, 2, 10> buffer;
+  for(int i = 1; i < 10; ++i)
+  {
+    buffer.insert(secToNanosec(i), Vector2(i, i));
+  }
+
+  Eigen::Matrix<int64_t, 1, 3> times;
+  times << secToNanosec(1.5), secToNanosec(2.5), secToNanosec(3.5);
+
+  MatrixX values = buffer.getValuesInterpolated(times);
+
+  EXPECT_DOUBLE_EQ(values(0, 0), 1.5);
+  EXPECT_DOUBLE_EQ(values(0, 1), 2.5);
+  EXPECT_DOUBLE_EQ(values(0, 2), 3.5);
+  EXPECT_DOUBLE_EQ(values(1, 0), 1.5);
+  EXPECT_DOUBLE_EQ(values(1, 1), 2.5);
+  EXPECT_DOUBLE_EQ(values(1, 2), 3.5);  
+}
+
+TEST(RingBufferTest, testGetValueInterpolated)
+{
+  using namespace ze;
+  ze::Ringbuffer<real_t, 2, 10> buffer;
+  for(int i = 1; i < 10; ++i)
+  {
+    buffer.insert(secToNanosec(i), Vector2(i, i));
+  }
+
+  Vector2 out;
+  EXPECT_TRUE(buffer.getValueInterpolated(secToNanosec(1), out));
+  EXPECT_TRUE(EIGEN_MATRIX_NEAR(out, Vector2(1, 1), 1e-8));
+
+  EXPECT_TRUE(buffer.getValueInterpolated(secToNanosec(5), out));
+  EXPECT_TRUE(EIGEN_MATRIX_NEAR(out, Vector2(5, 5), 1e-8));
+
+  EXPECT_TRUE(buffer.getValueInterpolated(secToNanosec(9), out));
+  EXPECT_TRUE(EIGEN_MATRIX_NEAR(out, Vector2(9, 9), 1e-8));
+
+  EXPECT_TRUE(buffer.getValueInterpolated(secToNanosec(8.5), out));
+  EXPECT_TRUE(EIGEN_MATRIX_NEAR(out, Vector2(8.5, 8.5), 1e-8));
+
+  EXPECT_FALSE(buffer.getValueInterpolated(0, out));
+  EXPECT_FALSE(buffer.getValueInterpolated(secToNanosec(10), out));
+}
+
+TEST(RingBufferTest, testInterpolationBounds)
+{
+  using namespace ze;
+
+  ze::Ringbuffer<real_t, 2, 10> buffer;
+
+  for(int i = 1; i < 10; ++i)
+  {
+    buffer.insert(secToNanosec(i), Vector2(i, i));
+  }
+
+  Eigen::Matrix<int64_t, Eigen::Dynamic, 1> stamps;
+  Eigen::Matrix<real_t, 2, Eigen::Dynamic> values;
+  std::tie(stamps, values) = buffer.getBetweenValuesInterpolated(
+        secToNanosec(0), secToNanosec(2));
+  EXPECT_EQ(stamps.size(), values.cols());
+  EXPECT_EQ(0, stamps.size());
+  EXPECT_EQ(0, values.cols());
+  std::tie(stamps, values) = buffer.getBetweenValuesInterpolated(
+        secToNanosec(5), secToNanosec(15));
+  EXPECT_EQ(stamps.size(), values.cols());
+  EXPECT_EQ(0, stamps.size());
+  EXPECT_EQ(0, values.cols());
+  std::tie(stamps, values) = buffer.getBetweenValuesInterpolated(
+        secToNanosec(0), secToNanosec(15));
+  EXPECT_EQ(stamps.size(), values.cols());
+  EXPECT_EQ(0, stamps.size());
+  EXPECT_EQ(0, values.cols());
+}
+
+TEST(RingBufferTest, benchmarkBufferVsRingBuffer)
+{
+  if (!FLAGS_run_benchmark) {
+    return;
+  }
+
+  using namespace ze;
+
+  // generate random data
+  Eigen::MatrixXd data(3, 10000);
+  data.setRandom();
+
+  Buffer<real_t, 3> buffer(nanosecToSecTrunc(1024));
+  Ringbuffer<real_t, 3, 1024> ringbuffer;
+
+  //////
+  // Insert
+  auto insertRingbuffer = [&]()
+  {
+    for (int i = 0; i < data.cols(); ++i)
+    {
+      ringbuffer.insert(i, data.col(i));
+    }
+  };
+  auto insertBuffer = [&]()
+  {
+    for (int i = 0; i < data.cols(); ++i)
+    {
+      buffer.insert(i, data.col(i));
+    }
+  };
+
+  real_t ringbuffer_insert = runTimingBenchmark(insertRingbuffer, 10, 20,
+                     "Ringbuffer: Insert", true);
+  real_t buffer_insert = runTimingBenchmark(insertBuffer, 10, 20,
+                     "Buffer: Insert", true);
+
+  VLOG(1) << "[Insert]" << "Buffer/Ringbuffer: " <<  buffer_insert / ringbuffer_insert << "\n";
+
+  real_t oldest, newest;
+  std::tie(newest, oldest, std::ignore) = ringbuffer.getOldestAndNewestStamp();
+
+  VLOG(1) << "BufferSize: " << buffer.size() << "\n";
+  VLOG(1) << "RingbufferSize: " << ringbuffer.size() << "\n";
+
+  //////
+  // get nearest value
+  auto getNearestValueRingbuffer = [&]()
+  {
+    double stamp = ((double) rand() / (RAND_MAX)) * (oldest - newest) + newest - 1;
+    ringbuffer.getNearestValue(stamp);
+  };
+  auto getNearestValueBuffer = [&]()
+  {
+    double stamp = ((double) rand() / (RAND_MAX)) * (oldest - newest) + newest - 1;
+    buffer.getNearestValue(stamp);
+  };
+
+  real_t ringbuffer_nearest = runTimingBenchmark(getNearestValueRingbuffer, 10, 20,
+                     "Ringbuffer: Nearest Value", true);
+  real_t buffer_nearest = runTimingBenchmark(getNearestValueBuffer, 10, 20,
+                     "Buffer: Nearest Value", true);
+
+  VLOG(1) << "[NearestValue]" << "Buffer/Ringbuffer: " <<  buffer_nearest / ringbuffer_nearest << "\n";
+
+  //////
+  // interpolation
+  auto getBetweenValuesInterpolatedRingbuffer = [&]()
+  {
+    double stamp1 = ((double) rand() / (RAND_MAX)) * (oldest - newest) + newest - 1;
+    double stamp2 = ((double) rand() / (RAND_MAX)) * (oldest - stamp1) + stamp1 + 1;
+    ringbuffer.getBetweenValuesInterpolated(stamp1, stamp2);
+  };
+  auto getBetweenValuesInterpolatedBuffer = [&]()
+  {
+    double stamp1 = ((double) rand() / (RAND_MAX)) * (oldest - newest) + newest - 1;
+    double stamp2 = ((double) rand() / (RAND_MAX)) * (oldest - stamp1) + stamp1 + 1;
+    buffer.getBetweenValuesInterpolated(stamp1, stamp2);
+  };
+
+  real_t ringbuffer_interpolate = runTimingBenchmark(getBetweenValuesInterpolatedRingbuffer, 10, 20,
+                     "Ringbuffer: Interpolate", true);
+  real_t buffer_interpolate = runTimingBenchmark(getBetweenValuesInterpolatedBuffer, 10, 20,
+                     "Buffer: Interpolate", true);
+
+  VLOG(1) << "[Interpolate]" << "Buffer/Ringbuffer: " <<  buffer_interpolate / ringbuffer_interpolate << "\n";
+
+  //////
+  // iterator equal or before
+  buffer.lock();
+  ringbuffer.lock();
+  auto iteratorEqRingbuffer = [&]()
+  {
+    double stamp = ((double) rand() / (RAND_MAX)) * (oldest - newest) + newest - 1;
+    ringbuffer.iterator_equal_or_before(stamp);
+  };
+  auto iteratorEqBuffer = [&]()
+  {
+    double stamp = ((double) rand() / (RAND_MAX)) * (oldest - newest) + newest - 1;
+    buffer.iterator_equal_or_before(stamp);
+  };
+
+  real_t ringbuffer_iterator = runTimingBenchmark(iteratorEqRingbuffer, 10, 20,
+                     "Ringbuffer: Interpolate", true);
+  real_t buffer_iterator = runTimingBenchmark(iteratorEqBuffer, 10, 20,
+                     "Buffer: Interpolate", true);
+  buffer.unlock();
+  ringbuffer.unlock();
+
+  VLOG(1) << "[IteratorBf]" << "Buffer/Ringbuffer: " <<  buffer_iterator / ringbuffer_iterator << "\n";
+
+  //////
+  // iterator equal or after
+  buffer.lock();
+  ringbuffer.lock();
+  auto iteratorEqAfRingbuffer = [&]()
+  {
+    double stamp = ((double) rand() / (RAND_MAX)) * (oldest - newest) + newest - 1;
+    ringbuffer.iterator_equal_or_after(stamp);
+  };
+  auto iteratorEqAfBuffer = [&]()
+  {
+    double stamp = ((double) rand() / (RAND_MAX)) * (oldest - newest) + newest - 1;
+    buffer.iterator_equal_or_after(stamp);
+  };
+
+  real_t ringbuffer_iterator_af = runTimingBenchmark(iteratorEqAfRingbuffer, 10, 20,
+                     "Ringbuffer: Interpolate", true);
+  real_t buffer_iterator_af = runTimingBenchmark(iteratorEqAfBuffer, 10, 20,
+                     "Buffer: Interpolate", true);
+  buffer.unlock();
+  ringbuffer.unlock();
+
+  VLOG(1) << "[IteratorAf]" << "Buffer/Ringbuffer: " <<  buffer_iterator_af / ringbuffer_iterator_af << "\n";
+
+  //////
+  // removeDataBeforeTimestamp
+  auto removeDataBeforeTimestampRingbuffer = [&]()
+  {
+    double stamp = ((double) rand() / (RAND_MAX)) * (oldest - newest) + newest - 1;
+    ringbuffer.removeDataBeforeTimestamp(stamp);
+  };
+  auto removeDataBeforeTimestampBuffer = [&]()
+  {
+    double stamp = ((double) rand() / (RAND_MAX)) * (oldest - newest) + newest - 1;
+    buffer.removeDataBeforeTimestamp(stamp);
+  };
+
+  real_t ringbuffer_remove = runTimingBenchmark(removeDataBeforeTimestampRingbuffer, 10, 20,
+                     "Ringbuffer: Interpolate", true);
+  real_t buffer_remove = runTimingBenchmark(removeDataBeforeTimestampBuffer, 10, 20,
+                     "Buffer: Interpolate", true);
+
+  VLOG(1) << "[Remove]" << "Buffer/Ringbuffer: " <<  buffer_remove / ringbuffer_remove << "\n";
+}
+
+ZE_UNITTEST_ENTRYPOINT
diff --git a/RWR/src/ze_oss/ze_common/test/test_running_statistics.cpp b/RWR/src/ze_oss/ze_common/test/test_running_statistics.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..9f52cbd88866e816849ff3e04b0450d43b3f87a4
--- /dev/null
+++ b/RWR/src/ze_oss/ze_common/test/test_running_statistics.cpp
@@ -0,0 +1,65 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include <ze/common/test_entrypoint.hpp>
+#include <ze/common/running_statistics.hpp>
+#include <ze/common/running_statistics_collection.hpp>
+
+TEST(RunningStatisticsTest, testRunningStatistics)
+{
+  ze::RunningStatistics stat;
+  stat.addSample(1.1);
+  stat.addSample(2.2);
+  stat.addSample(3.3);
+  stat.addSample(2.7);
+  stat.addSample(4.5);
+  EXPECT_EQ(stat.numSamples(), 5u);
+  EXPECT_FLOATTYPE_EQ(stat.max(), 4.5);
+  EXPECT_FLOATTYPE_EQ(stat.min(), 1.1);
+  EXPECT_FLOATTYPE_EQ(stat.sum(), 13.8);
+  EXPECT_FLOATTYPE_EQ(stat.mean(), 2.76);
+  EXPECT_FLOATTYPE_EQ(stat.var(), 1.5979999999999994);
+  EXPECT_FLOATTYPE_EQ(stat.std(), 1.2641202474448383);
+  VLOG(1) << "Statistics:\n" << stat;
+}
+
+TEST(RunningStatisticsTest, testCollection)
+{
+  using namespace ze;
+
+  DECLARE_STATISTICS(Statistics, stats, foo, bar);
+  stats[Statistics::foo].addSample(1.0);
+  stats[Statistics::foo].addSample(1.0);
+  stats[Statistics::foo].addSample(1.0);
+  stats[Statistics::bar].addSample(2.0);
+  EXPECT_EQ(stats[Statistics::foo].numSamples(), 3u);
+  EXPECT_EQ(stats[Statistics::bar].numSamples(), 1u);
+  EXPECT_FLOATTYPE_EQ(stats[Statistics::foo].mean(), 1.0);
+  EXPECT_FLOATTYPE_EQ(stats[Statistics::bar].mean(), 2.0);
+  VLOG(1) << stats;
+}
+
+ZE_UNITTEST_ENTRYPOINT
+
diff --git a/RWR/src/ze_oss/ze_common/test/test_statistics.cpp b/RWR/src/ze_oss/ze_common/test/test_statistics.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..77358af79191be17a2d683d840252f3ac37675a2
--- /dev/null
+++ b/RWR/src/ze_oss/ze_common/test/test_statistics.cpp
@@ -0,0 +1,63 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include <cmath>
+#include <utility>
+#include <ze/common/types.hpp>
+#include <ze/common/test_entrypoint.hpp>
+#include <ze/common/statistics.hpp>
+#include <ze/common/random_matrix.hpp>
+
+TEST(StatisticsTest, testMedian)
+{
+  Eigen::VectorXd x(5);
+  x << 1, 2, 3, 4, 5;
+  auto m = ze::median(x);
+  EXPECT_DOUBLE_EQ(m.first, 3);
+}
+
+TEST(StatisticsTest, testMeasurementCovariance)
+{
+  using namespace ze;
+
+  // Generate a random distribution matrix of known covariance.
+  Vector3 variances;
+  variances << 2.0, 3.0, 4.0;
+  RandomVectorSampler<3>::Ptr sampler(
+        RandomVectorSampler<3>::variances(variances));
+
+  MatrixX measurements(3, 100000);
+  for (int i = 0; i < 100000; ++i)
+  {
+    measurements.col(i) = sampler->sample();
+  }
+
+  Matrix3 cov = measurementCovariance(measurements);
+  Matrix3 ref = Vector3(2.0, 3.0, 4.0).asDiagonal();
+
+  EXPECT_TRUE(EIGEN_MATRIX_NEAR(cov, ref, 1e-1));
+}
+
+ZE_UNITTEST_ENTRYPOINT
diff --git a/RWR/src/ze_oss/ze_common/test/test_stl_utils.cpp b/RWR/src/ze_oss/ze_common/test/test_stl_utils.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..58d0e241242264126da8e0972950f1ec1b748f5f
--- /dev/null
+++ b/RWR/src/ze_oss/ze_common/test/test_stl_utils.cpp
@@ -0,0 +1,61 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include <string>
+#include <vector>
+
+#include <ze/common/stl_utils.hpp>
+#include <ze/common/test_entrypoint.hpp>
+
+TEST(StlUtilsTests, testRange)
+{
+  using namespace ze;
+
+  {
+    auto vec = range(10);
+    EXPECT_EQ(vec.size(), 10u);
+    EXPECT_EQ(vec[0], 0u);
+    EXPECT_EQ(vec[9], 9u);
+  }
+
+  {
+    auto vec = range(0);
+    EXPECT_EQ(vec.size(), 0u);
+  }
+
+  {
+    auto vec = range(3, 6);
+    EXPECT_EQ(vec.size(), 3);
+    EXPECT_EQ(vec[0], 3u);
+    EXPECT_EQ(vec[2], 5u);
+  }
+
+  {
+    auto vec = range(4, 4);
+    EXPECT_EQ(vec.size(), 0u);
+  }
+}
+
+ZE_UNITTEST_ENTRYPOINT
diff --git a/RWR/src/ze_oss/ze_common/test/test_string_utils.cpp b/RWR/src/ze_oss/ze_common/test/test_string_utils.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b7ae49444627eca749a6d51e1ba9363028a48ad4
--- /dev/null
+++ b/RWR/src/ze_oss/ze_common/test/test_string_utils.cpp
@@ -0,0 +1,61 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include <string>
+#include <vector>
+
+#include <ze/common/string_utils.hpp>
+#include <ze/common/test_entrypoint.hpp>
+
+
+TEST(StringUtilsTest, testTrim)
+{
+  std::string a = " abc ";
+  a = ze::trimString(a);
+  EXPECT_EQ(a, "abc");
+}
+
+TEST(StringUtilsTest, testSplit)
+{
+  std::string a = "one,two,three";
+  std::vector<std::string> vec = ze::splitString(a, ',');
+  EXPECT_EQ(vec.size(), 3u);
+  EXPECT_EQ(vec[0], "one");
+  EXPECT_EQ(vec[1], "two");
+  EXPECT_EQ(vec[2], "three");
+}
+
+TEST(StringUtilsTest, testEnsureSlash)
+{
+  std::string a = "a";
+  a = ze::ensureLeftSlash(a);
+  EXPECT_EQ(a, "/a");
+
+  std::string b = "/b";
+  b = ze::ensureLeftSlash(b);
+  EXPECT_EQ(b, "/b");
+}
+
+ZE_UNITTEST_ENTRYPOINT
diff --git a/RWR/src/ze_oss/ze_common/test/test_test_utils.cpp b/RWR/src/ze_oss/ze_common/test/test_test_utils.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..7b9bfe7c1b9660cfb411fe4db6cf186bd2762138
--- /dev/null
+++ b/RWR/src/ze_oss/ze_common/test/test_test_utils.cpp
@@ -0,0 +1,47 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include <cmath>
+
+#include <ze/common/test_entrypoint.hpp>
+#include <ze/common/test_utils.hpp>
+
+TEST(TestUtilsTest, testTestData)
+{
+  EXPECT_NO_FATAL_FAILURE(ze::getTestDataDir("synthetic_room_pinhole"));
+}
+
+TEST(TestUtilsTest, testLoadCsvPoses)
+{
+  using namespace ze;
+
+  std::string data_path = getTestDataDir("synthetic_room_pinhole");
+  std::string filename = data_path + "/traj_gt.csv";
+  std::map<int64_t, Transformation> poses = loadIndexedPosesFromCsv(filename);
+  EXPECT_EQ(poses.size(), 50u);
+  EXPECT_FLOATTYPE_EQ(poses[1].getPosition().x(), real_t{1.499260});
+}
+
+ZE_UNITTEST_ENTRYPOINT
diff --git a/RWR/src/ze_oss/ze_common/test/test_thread_pool.cpp b/RWR/src/ze_oss/ze_common/test/test_thread_pool.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..599d5efdd83ce99fa2b91ba50e5f0e110bc303fb
--- /dev/null
+++ b/RWR/src/ze_oss/ze_common/test/test_thread_pool.cpp
@@ -0,0 +1,57 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include <iostream>
+#include <vector>
+#include <chrono>
+
+#include <ze/common/logging.hpp>
+#include <ze/common/test_entrypoint.hpp>
+#include <ze/common/thread_pool.hpp>
+
+TEST(ThreadPoolTests, testThreadPool)
+{
+  ze::ThreadPool pool(4);
+  std::vector<std::future<size_t>> results;
+
+  for (size_t i = 0; i < 8; ++i)
+  {
+    results.emplace_back(
+      pool.enqueue([i] {
+        VLOG(1) << "started job " << i;
+        std::this_thread::sleep_for(std::chrono::milliseconds(10));
+        VLOG(1) << "stopped job " << i;
+        return i * i;
+      })
+    );
+  }
+
+  for (size_t i = 0u; i < results.size(); ++i)
+  {
+    CHECK_EQ(results[i].get(), i * i);
+  }
+}
+
+ZE_UNITTEST_ENTRYPOINT
diff --git a/RWR/src/ze_oss/ze_common/test/test_thread_safe_fifo.cpp b/RWR/src/ze_oss/ze_common/test/test_thread_safe_fifo.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..28f4a0f52233832d072ce5d1106ef0e53eaebda4
--- /dev/null
+++ b/RWR/src/ze_oss/ze_common/test/test_thread_safe_fifo.cpp
@@ -0,0 +1,493 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include <memory>
+#include <atomic>
+#include <thread>
+#include <string>
+
+#include <ze/common/test_entrypoint.hpp>
+#include <ze/common/test_thread_blocking.hpp>
+#include <ze/common/thread_safe_fifo.hpp>
+
+using namespace ::ze;
+
+// unnamed namespace for internal stuff
+namespace {
+
+std::atomic<unsigned> s_num_live(0);
+std::atomic<unsigned> s_counter(0);
+
+constexpr unsigned c_num_objects_per_thread = 12000;
+
+class TestObject
+{
+public:
+  TestObject()
+  {
+    s_num_live.fetch_add(1, std::memory_order_relaxed);
+    counter_ = s_counter.fetch_add(1, std::memory_order_relaxed);
+  }
+  ~TestObject()
+  {
+    s_num_live.fetch_sub(1, std::memory_order_relaxed);
+  }
+  unsigned counter() const
+  {
+    return counter_;
+  }
+private:
+  unsigned counter_;
+}; // class TestObject
+
+typedef std::shared_ptr<TestObject> TestObjectPtr;
+typedef ThreadSafeFifo<TestObjectPtr, 8> TestObjectQueue;
+typedef ThreadSafeFifo<TestObjectPtr, 512> TestObjectQueue2;
+
+//------------------------------------------------------------------------------
+TestObjectPtr createObj()
+{
+  return std::make_shared<TestObject>();
+}
+
+//------------------------------------------------------------------------------
+class BlockingReadTest : public BlockingTest
+{
+public:
+  BlockingReadTest() : queue_() { }
+  ~BlockingReadTest() { }
+  virtual void performBlockingAction(unsigned testId);
+  virtual void performUnblockingAction(unsigned testId);
+private:
+  TestObjectQueue queue_;
+}; // class BlockingReadTest
+
+//------------------------------------------------------------------------------
+void BlockingReadTest::performBlockingAction(unsigned testId)
+{
+  TestObjectPtr obj = queue_.read();
+  EXPECT_TRUE(obj.get() != nullptr);
+}
+
+//------------------------------------------------------------------------------
+void BlockingReadTest::performUnblockingAction(unsigned testId)
+{
+  queue_.write(createObj());
+}
+
+//------------------------------------------------------------------------------
+class BlockingWriteTest : public BlockingTest {
+public:
+  BlockingWriteTest();
+  ~BlockingWriteTest() { }
+  virtual void performBlockingAction(unsigned testId);
+  virtual void performUnblockingAction(unsigned testId);
+private:
+  TestObjectQueue queue_;
+};
+
+//------------------------------------------------------------------------------
+BlockingWriteTest::BlockingWriteTest()
+  : queue_()
+{
+  for (unsigned i = 0; i < 7; ++i)
+  {
+    queue_.write(createObj());
+  }
+}
+
+//------------------------------------------------------------------------------
+void BlockingWriteTest::performBlockingAction(unsigned testId)
+{
+  queue_.write(createObj());
+}
+
+//------------------------------------------------------------------------------
+void BlockingWriteTest::performUnblockingAction(unsigned testId)
+{
+  TestObjectPtr obj = queue_.read();
+  EXPECT_TRUE(obj.get() != nullptr);
+}
+
+//------------------------------------------------------------------------------
+class WriterThread1
+{
+public:
+  WriterThread1(TestObjectQueue2& queue)
+    : queue_(queue)
+  {
+  }
+  void operator()()
+  {
+    for (unsigned i = 0; i < c_num_objects_per_thread; ++i)
+    {
+      queue_.write(createObj());
+    }
+  }
+private:
+  TestObjectQueue2& queue_;
+}; // class WriterThread1
+
+//------------------------------------------------------------------------------
+class WriterThread2
+{
+public:
+  WriterThread2(TestObjectQueue2& queue)
+  : queue_(queue)
+  {
+  }
+  void operator()()
+  {
+    TestObjectQueue::UniqueLock lock(queue_.getLock());
+    for (unsigned i = 0; i < c_num_objects_per_thread; ++i)
+    {
+      queue_.write(createObj(), lock);
+    }
+  }
+private:
+  TestObjectQueue2& queue_;
+}; // class WriterThread2
+
+//------------------------------------------------------------------------------
+class ReaderThread
+{
+public:
+  ReaderThread(TestObjectQueue2& queue, unsigned& counter)
+    : queue_(queue)
+    , counter_(counter)
+    , run_(true)
+  {
+  }
+  void operator()()
+  {
+    TestObjectPtr obj;
+    while (true) {
+      bool result = queue_.timedRead(obj, 100);
+      if (!result)
+      {
+        if (!run_)
+        {
+          break;
+        }
+        continue;
+      }
+      EXPECT_TRUE(obj.get() != nullptr);
+      counter_++;
+    }
+  }
+  void stop() { run_ = false; }
+private:
+  TestObjectQueue2& queue_;
+  unsigned& counter_;
+  volatile bool run_;
+}; // class ReaderThread
+
+//------------------------------------------------------------------------------
+void runThreadTest()
+{
+  TestObjectQueue2 queue;
+
+  unsigned c1 = 0;
+  unsigned c2 = 0;
+
+  WriterThread1 w1(queue);
+  WriterThread2 w2(queue);
+  WriterThread1 w3(queue);
+  WriterThread2 w4(queue);
+  ReaderThread r1(queue, c1);
+  ReaderThread r2(queue, c2);
+
+  std::thread tw1(&WriterThread1::operator(), &w1);
+  std::thread tw2(&WriterThread2::operator(), &w2);
+  std::thread tw3(&WriterThread1::operator(), &w3);
+  std::thread tw4(&WriterThread2::operator(), &w4);
+  std::thread tr1(&ReaderThread::operator(), &r1);
+  std::thread tr2(&ReaderThread::operator(), &r2);
+
+  tw1.join();
+  tw2.join();
+  tw3.join();
+  tw4.join();
+
+  r1.stop();
+  r2.stop();
+
+  tr1.join();
+  tr2.join();
+
+  unsigned total = c1 + c2;
+  EXPECT_EQ(4 * c_num_objects_per_thread, total);
+  EXPECT_TRUE(queue.empty());
+}
+
+} // unnamed namespace
+
+TEST(ThreadSafeFifo, Default)
+{
+  TestObjectQueue queue;
+
+  //
+  // write objects until the queue is full, then clear it again
+  //
+
+  s_counter = 0;
+  EXPECT_TRUE(queue.empty());
+  for (unsigned i = 0; i < 7; ++i)
+  {
+    EXPECT_EQ(i, queue.size());
+    queue.write(createObj());
+  }
+  EXPECT_EQ(7, queue.size());
+  EXPECT_EQ(7, s_num_live);
+  EXPECT_TRUE(queue.full());
+  EXPECT_FALSE(queue.nonBlockingWrite(createObj()));
+  EXPECT_FALSE(queue.timedWrite(createObj(), 1));
+
+  for (unsigned i = 0; i < 7; ++i)
+  {
+    TestObjectPtr obj = queue.read();
+    ASSERT_TRUE(obj.get() != nullptr);
+    EXPECT_EQ(i, obj->counter());
+  }
+
+  EXPECT_TRUE(queue.empty());
+  EXPECT_EQ(0, queue.size());
+  EXPECT_EQ(0, s_num_live);
+
+  TestObjectPtr obj;
+  EXPECT_FALSE(queue.nonBlockingRead(obj));
+  EXPECT_FALSE(queue.timedRead(obj, 1));
+
+  s_counter=0;
+
+  //
+  // write objects until the queue is full
+  // then read some objects
+  // then write some more
+  // then read all remaining
+  //
+
+  // write objects until the queue is full
+  for (unsigned i = 0; i < 7; ++i)
+  {
+    TestObjectPtr obj = createObj();
+    queue.write(obj); // use const reference type
+  }
+  EXPECT_EQ(7, queue.size());
+  EXPECT_EQ(7, s_num_live);
+
+  // read the events from the queue
+  for (unsigned i = 0; i < 4; ++i)
+  {
+    TestObjectPtr obj = queue.read();
+    ASSERT_TRUE(obj.get() != nullptr);
+    EXPECT_EQ(i, obj->counter());
+  }
+
+  EXPECT_EQ(3, queue.size());
+  EXPECT_EQ(3, s_num_live);
+
+  for (unsigned i = 0; i < 2; ++i)
+  {
+    queue.write(createObj());
+  }
+
+  EXPECT_EQ(5, queue.size());
+  EXPECT_EQ(5, s_num_live);
+
+  for (unsigned i = 0; i < 5; ++i)
+  {
+    TestObjectPtr obj = queue.read();
+    ASSERT_TRUE(obj.get() != nullptr);
+    EXPECT_EQ(i+4, obj->counter());
+  }
+
+  EXPECT_EQ(0, queue.size());
+  EXPECT_EQ(0, s_num_live);
+
+  //
+  // use non-blocking reads and writes
+  //
+
+  s_counter = 0;
+
+  for (unsigned i = 0; i < 7; ++i)
+  {
+    EXPECT_TRUE(queue.nonBlockingWrite(createObj()));
+  }
+  EXPECT_FALSE(queue.nonBlockingWrite(createObj()));
+  EXPECT_EQ(7, s_num_live);
+
+  for (unsigned i = 0; i < 4; ++i)
+  {
+    TestObjectPtr obj;
+    EXPECT_TRUE(queue.nonBlockingRead(obj));
+    ASSERT_TRUE(obj.get() != nullptr);
+    EXPECT_EQ(i, obj->counter());
+  }
+  for (unsigned i = 0; i < 2; ++i)
+  {
+    EXPECT_TRUE(queue.nonBlockingWrite(createObj()));
+  }
+  EXPECT_EQ(5, queue.size());
+  EXPECT_EQ(5, s_num_live);
+  for (unsigned i = 0; i < 3; ++i)
+  {
+    TestObjectPtr obj;
+    EXPECT_TRUE(queue.nonBlockingRead(obj));
+    ASSERT_TRUE(obj.get() != nullptr);
+    EXPECT_EQ(i+4, obj->counter());
+  }
+  for (unsigned i = 0; i < 2; ++i)
+  {
+    TestObjectPtr obj;
+    EXPECT_TRUE(queue.nonBlockingRead(obj));
+    ASSERT_TRUE(obj.get() != nullptr);
+    EXPECT_EQ(i+8, obj->counter()); // 7 previous reads, plus one failed write
+  }
+  EXPECT_TRUE(queue.empty());
+  EXPECT_EQ(0, queue.size());
+  EXPECT_EQ(0, s_num_live);
+
+  EXPECT_FALSE(queue.nonBlockingRead(obj));
+
+  //
+  // use timed reads and writes
+  //
+
+  s_counter = 0;
+
+  for (unsigned i = 0; i < 7; ++i)
+  {
+    EXPECT_TRUE(queue.timedWrite(createObj(), 1));
+  }
+  EXPECT_FALSE(queue.timedWrite(createObj(), 1));
+  EXPECT_EQ(7, s_num_live);
+
+  for (unsigned i = 0; i < 4; ++i)
+  {
+    TestObjectPtr obj;
+    EXPECT_TRUE(queue.timedRead(obj, 1));
+    ASSERT_TRUE(obj.get() != nullptr);
+    EXPECT_EQ(i, obj->counter());
+  }
+  for (unsigned i = 0; i < 2; ++i)
+  {
+    EXPECT_TRUE(queue.timedWrite(createObj(), 1));
+  }
+  EXPECT_EQ(5, queue.size());
+  EXPECT_EQ(5, s_num_live);
+  for (unsigned i = 0; i < 3; ++i)
+  {
+    TestObjectPtr obj;
+    EXPECT_TRUE(queue.timedRead(obj, 1));
+    ASSERT_TRUE(obj.get() != nullptr);
+    EXPECT_EQ(i+4, obj->counter());
+  }
+  for (unsigned i = 0; i < 2; ++i)
+  {
+    TestObjectPtr obj;
+    EXPECT_TRUE(queue.timedRead(obj, 1));
+    ASSERT_TRUE(obj.get() != nullptr);
+    EXPECT_EQ(i+8, obj->counter()); // 7 previous reads, plus one failed write
+  }
+  EXPECT_TRUE(queue.empty());
+  EXPECT_EQ(0, queue.size());
+  EXPECT_EQ(0, s_num_live);
+
+  EXPECT_FALSE(queue.timedRead(obj, 1));
+
+  //
+  // test the clear() functionality
+  //
+
+  for (unsigned i = 0; i < 7; ++i)
+  {
+    queue.write(createObj());
+  }
+  EXPECT_EQ(7, s_num_live);
+  EXPECT_FALSE(queue.empty());
+  EXPECT_TRUE(queue.full());
+  queue.clear();
+  EXPECT_EQ(0, s_num_live);
+  EXPECT_TRUE(queue.empty());
+  EXPECT_FALSE(queue.full());
+}
+
+TEST(ThreadSafeFifo, StringTest)
+{
+  ThreadSafeFifo<std::string, 16> queue;
+
+  queue.write("a");
+  queue.write("b");
+  queue.write("c");
+
+  EXPECT_EQ("a", queue.read());
+  EXPECT_EQ("b", queue.read());
+  EXPECT_EQ("c", queue.read());
+
+  for (unsigned i = 0; i < 3; i++) {
+    queue.write("x");
+    queue.write("y");
+    queue.write("z");
+  }
+
+  EXPECT_EQ(9, queue.size());
+
+  for (unsigned i = 0; i < 3; i++) {
+    EXPECT_EQ("x", queue.read());
+    EXPECT_EQ("y", queue.read());
+    EXPECT_EQ("z", queue.read());
+  }
+}
+
+TEST(ThreadSafeFifo, BlockingReadTest)
+{
+  EXPECT_EQ(0, s_num_live);
+  {
+    BlockingReadTest test;
+    test.runBlockingTest(0, 200);
+  }
+  EXPECT_EQ(0, s_num_live);
+}
+
+TEST(ThreadSafeFifo, BlockingWriteTest)
+{
+  EXPECT_EQ(0, s_num_live);
+  {
+    BlockingWriteTest test;
+    EXPECT_EQ(7, s_num_live);
+    test.runBlockingTest(0, 200);
+  }
+  EXPECT_EQ(0, s_num_live);
+}
+
+TEST(ThreadSafeFifo, ThreadTest)
+{
+  s_counter = 0;
+  runThreadTest();
+}
+
+ZE_UNITTEST_ENTRYPOINT
diff --git a/RWR/src/ze_oss/ze_common/test/test_timer.cpp b/RWR/src/ze_oss/ze_common/test/test_timer.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..97d0c227fdeeb4139a7a50b7e3650e1e01fad4ec
--- /dev/null
+++ b/RWR/src/ze_oss/ze_common/test/test_timer.cpp
@@ -0,0 +1,84 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include <chrono>
+#include <thread>
+#include <iostream>
+
+#include <ze/common/string_utils.hpp>
+#include <ze/common/test_entrypoint.hpp>
+#include <ze/common/timer.hpp>
+#include <ze/common/timer_collection.hpp>
+#include <ze/common/timer_statistics.hpp>
+
+TEST(TimerTests, testTimerStatistics)
+{
+  ze::TimerStatistics timer;
+  for(int i = 0; i < 10; ++i)
+  {
+    timer.start();
+    std::this_thread::sleep_for(ze::Timer::ms(10));
+    timer.stop();
+  }
+  EXPECT_EQ(timer.numTimings(), 10u);
+  EXPECT_NEAR(timer.mean(), 10.0, 0.5);
+  EXPECT_NEAR(timer.accumulated(), 100.0, 10.0);
+  EXPECT_GT(timer.max(), timer.min());
+}
+
+TEST(TimerTests, testTimerScope)
+{
+  ze::TimerStatistics timer;
+  for(int i = 0; i < 10; ++i)
+  {
+    auto t = timer.timeScope();
+    std::this_thread::sleep_for(ze::Timer::ms(10));
+  }
+  EXPECT_EQ(timer.numTimings(), 10u);
+  EXPECT_NEAR(timer.mean(), 10.0, 0.5);
+  EXPECT_NEAR(timer.accumulated(), 100.0, 10.0);
+  EXPECT_GT(timer.max(), timer.min());
+}
+
+TEST(TimerTests, testTimerCollection)
+{
+  DECLARE_TIMER(TestTimer, timers, foo, bar);
+
+  for(int i = 0; i < 10; ++i)
+  {
+    timers[TestTimer::foo].start();
+    std::this_thread::sleep_for(ze::Timer::ms(50));
+    timers[TestTimer::foo].stop();
+
+    timers[TestTimer::bar].start();
+    std::this_thread::sleep_for(ze::Timer::ms(10));
+    timers[TestTimer::bar].stop();
+  }
+  VLOG(1) << timers;
+  EXPECT_NO_FATAL_FAILURE(timers.saveToFile("/tmp", "test_timer.yaml"));
+  EXPECT_EQ(timers.size(), 2u);
+}
+
+ZE_UNITTEST_ENTRYPOINT
diff --git a/RWR/src/ze_oss/ze_common/test/test_transformation.cpp b/RWR/src/ze_oss/ze_common/test/test_transformation.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..fe6cdef6ee2dfee35227aea5c505c90c2cb977a6
--- /dev/null
+++ b/RWR/src/ze_oss/ze_common/test/test_transformation.cpp
@@ -0,0 +1,84 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include <cmath>
+
+#include <ze/common/test_entrypoint.hpp>
+#include <ze/common/test_manifold.hpp>
+#include <ze/common/transformation.hpp>
+
+
+
+TEST(TransformationTests, testSetRandom)
+{
+  ze::Transformation T;
+  T.setRandom();
+  ze::Matrix3 R = T.getRotation().getRotationMatrix();
+
+  // Check if orthonormal
+  EXPECT_TRUE(EIGEN_MATRIX_NEAR(R*R.transpose(), ze::I_3x3, 1e-6));
+}
+
+TEST(TransformationTests, testExpLog)
+{
+  for(int i = 0; i < 10; ++i)
+  {
+    ze::Transformation T1;
+    T1.setRandom();
+    ze::Transformation::Vector6 v = T1.log();
+    ze::Transformation T2 = ze::Transformation::exp(v);
+    ze::Matrix4 TT1 = T1.getTransformationMatrix();
+    ze::Matrix4 TT2 = T2.getTransformationMatrix();
+    for(int r = 0; r < 4; ++r)
+    {
+      for(int c = 0; c < 4; ++c)
+      {
+        EXPECT_NEAR(TT1(r,c), TT2(r,c), 1e-6) << "Failed at (" << r << "," << c << ")";
+      }
+    }
+  }
+}
+
+TEST(TransformationTests, quaternionMatrices)
+{
+  Eigen::Quaterniond q_AB, q_BC, q_AC_plus, q_AC_oplus, q_AC_quatmult;
+  q_AB.coeffs() = Eigen::Vector4d::Random().normalized();
+  q_BC.coeffs() = Eigen::Vector4d::Random().normalized();
+  q_AC_quatmult = q_AB * q_BC;
+  q_AC_plus.coeffs() = ze::quaternionPlusMatrix(q_AB) * q_BC.coeffs();
+  q_AC_oplus.coeffs() = ze::quaternionOplusMatrix(q_BC) * q_AB.coeffs();
+
+  Eigen::Vector3d v_C = Eigen::Vector3d::Random();
+  EXPECT_TRUE(EIGEN_MATRIX_EQUAL_DOUBLE(q_AC_quatmult * v_C, q_AC_plus * v_C));
+  EXPECT_TRUE(EIGEN_MATRIX_EQUAL_DOUBLE(q_AC_quatmult * v_C, q_AC_oplus * v_C));
+  EXPECT_TRUE(EIGEN_MATRIX_EQUAL_DOUBLE(
+                (q_AC_quatmult.inverse() * q_AC_plus).coeffs(),
+                Eigen::Quaterniond::Identity().coeffs()));
+  EXPECT_TRUE(EIGEN_MATRIX_EQUAL_DOUBLE(
+                (q_AC_quatmult.inverse() * q_AC_oplus).coeffs(),
+                Eigen::Quaterniond::Identity().coeffs()));
+}
+
+ZE_UNITTEST_ENTRYPOINT
diff --git a/RWR/src/ze_oss/ze_common/test/test_versioned_slot_handle.cpp b/RWR/src/ze_oss/ze_common/test/test_versioned_slot_handle.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f1f21c61ee6596bebc125e777b444d8b10f81be0
--- /dev/null
+++ b/RWR/src/ze_oss/ze_common/test/test_versioned_slot_handle.cpp
@@ -0,0 +1,59 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include <cmath>
+#include <bitset>
+
+#include <ze/common/test_entrypoint.hpp>
+#include <ze/common/versioned_slot_handle.hpp>
+
+TEST(VersionedSlotHandle, test)
+{
+  using VIdx = ze::VersionedSlotHandle<uint32_t, 8, 24>;
+  VIdx v1;
+  EXPECT_EQ(sizeof(VIdx), 4);
+
+  v1.slot = 1;
+  v1.version = 1;
+  VLOG(1) << std::bitset<32>(v1.handle);
+  EXPECT_EQ(v1.handle, 1 + (1<<8));
+
+  v1.slot = VIdx::maxSlot();
+  v1.version = VIdx::maxVersion();
+  VLOG(1) << std::bitset<32>(v1.handle);
+  EXPECT_EQ(v1.handle, std::numeric_limits<uint32_t>::max());
+
+  VIdx v2;
+  EXPECT_EQ(v2.handle, 0);
+
+  VIdx v3(10);
+  EXPECT_EQ(v3.handle, 10);
+
+  VIdx v4(10, 2);
+  EXPECT_EQ(v4.slot, 10);
+  EXPECT_EQ(v4.version, 2);
+}
+
+ZE_UNITTEST_ENTRYPOINT
diff --git a/RWR/src/ze_oss/ze_data_provider/CATKIN_IGNORE b/RWR/src/ze_oss/ze_data_provider/CATKIN_IGNORE
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/RWR/src/ze_oss/ze_data_provider/CMakeLists.txt b/RWR/src/ze_oss/ze_data_provider/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..be00f9bb613f9ec82361d1181000f19ac54651a9
--- /dev/null
+++ b/RWR/src/ze_oss/ze_data_provider/CMakeLists.txt
@@ -0,0 +1,52 @@
+project(ze_data_provider)
+cmake_minimum_required(VERSION 2.8.3)
+
+find_package(catkin_simple REQUIRED)
+catkin_simple()
+
+include(ze_setup)
+
+#############
+# LIBRARIES #
+#############
+set(HEADERS
+  include/ze/data_provider/data_provider_base.hpp
+  include/ze/data_provider/data_provider_factory.hpp
+  include/ze/data_provider/data_provider_csv.hpp
+  include/ze/data_provider/data_provider_rosbag.hpp
+  include/ze/data_provider/data_provider_rostopic.hpp
+  include/ze/data_provider/camera_imu_synchronizer_base.hpp
+  include/ze/data_provider/camera_imu_synchronizer.hpp
+  include/ze/data_provider/camera_imu_synchronizer_unsync.hpp
+  )
+
+set(SOURCES
+  src/data_provider_base.cpp
+  src/data_provider_factory.cpp
+  src/data_provider_csv.cpp
+  src/data_provider_rosbag.cpp
+  src/data_provider_rostopic.cpp
+  src/camera_imu_synchronizer.cpp
+  src/camera_imu_synchronizer_unsync.cpp
+  src/camera_imu_synchronizer_base.cpp
+  )
+
+cs_add_library(${PROJECT_NAME} ${SOURCES} ${HEADERS})
+
+##########
+# GTESTS #
+##########
+catkin_add_gtest(test_data_provider test/test_data_provider.cpp)
+target_link_libraries(test_data_provider ${PROJECT_NAME})
+
+catkin_add_gtest(test_camera_imu_synchronizer test/test_camera_imu_synchronizer.cpp)
+target_link_libraries(test_camera_imu_synchronizer ${PROJECT_NAME})
+
+catkin_add_gtest(test_camera_imu_synchronizer_unsync test/test_camera_imu_synchronizer_unsync.cpp)
+target_link_libraries(test_camera_imu_synchronizer_unsync ${PROJECT_NAME})
+
+##########
+# EXPORT #
+##########
+cs_install()
+cs_export()
diff --git a/RWR/src/ze_oss/ze_data_provider/include/ze/data_provider/camera_imu_synchronizer.hpp b/RWR/src/ze_oss/ze_data_provider/include/ze/data_provider/camera_imu_synchronizer.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..11c6b1fc98a45c445357d21b0cb7194692596240
--- /dev/null
+++ b/RWR/src/ze_oss/ze_data_provider/include/ze/data_provider/camera_imu_synchronizer.hpp
@@ -0,0 +1,73 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#pragma once
+
+#include <memory>
+#include <imp/core/image_base.hpp>
+#include <ze/common/ringbuffer.hpp>
+#include <ze/common/time_conversions.hpp>
+#include <ze/common/types.hpp>
+#include <ze/data_provider/camera_imu_synchronizer_base.hpp>
+
+namespace ze {
+
+// fwd
+class DataProviderBase;
+class ImageBase;
+
+// -----------------------------------------------------------------------------
+class CameraImuSynchronizer: public CameraImuSynchronizerBase
+{
+public:
+  // convenience typedefs
+  using ImuSyncBuffer = Ringbuffer<real_t, 6, 1000>;
+  using ImuBufferVector = std::vector<ImuSyncBuffer>;
+
+  //! Default constructor.
+  CameraImuSynchronizer(DataProviderBase& data_provider);
+
+  //! Add IMU measurement to the frame synchronizer.
+  void addImuData(
+      int64_t stamp,
+      const Vector3& acc,
+      const Vector3& gyr,
+      const uint32_t imu_idx);
+
+private:
+  //! Register callbacks in data provider to this class' addImgData and addImuData.
+  void subscribeDataProvider(DataProviderBase& data_provider);
+
+  //! IMU buffer stores all imu measurements, size of imu_count_.
+  ImuBufferVector imu_buffers_;
+
+  //! Initialize the image and imu buffers
+  void initBuffers();
+
+  //! This function checks if we have all data ready to call the callback.
+  virtual void checkImuDataAndCallback();
+};
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_data_provider/include/ze/data_provider/camera_imu_synchronizer_base.hpp b/RWR/src/ze_oss/ze_data_provider/include/ze/data_provider/camera_imu_synchronizer_base.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..a64085c119162b12e742cb77c8eec70049c3594d
--- /dev/null
+++ b/RWR/src/ze_oss/ze_data_provider/include/ze/data_provider/camera_imu_synchronizer_base.hpp
@@ -0,0 +1,121 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#pragma once
+
+#include <memory>
+#include <imp/core/image_base.hpp>
+#include <ze/common/types.hpp>
+#include <ze/common/time_conversions.hpp>
+#include <ze/imu/imu_buffer.hpp>
+
+namespace ze {
+
+class DataProviderBase;
+
+// convenience typedefs
+using ImuStampsVector = std::vector<ImuStamps>;
+using ImuAccGyrVector = std::vector<ImuAccGyrContainer>;
+
+// callback typedefs
+using SynchronizedCameraImuCallback =
+  std::function<void (const StampedImages& /*images*/,
+                      const ImuStampsVector& /*imu_timestamps*/,
+                      const ImuAccGyrVector& /*imu_measurements*/)>;
+
+// -----------------------------------------------------------------------------
+//! Container to buffer received images.
+struct ImageBufferItem
+{
+  int64_t stamp        { -1 };
+  ImageBasePtr img     { nullptr };
+  int32_t camera_idx   { -1 };
+  inline bool empty()
+  {
+    return stamp == -1;
+  }
+
+  inline void reset()
+  {
+    stamp = -1;
+    img.reset();
+    camera_idx = -1;
+  }
+};
+using ImgBuffer = std::vector<ImageBufferItem>;
+
+class CameraImuSynchronizerBase
+{
+public:
+  //! Default constructor.
+  CameraImuSynchronizerBase(DataProviderBase& data_provider);
+
+  void registerCameraImuCallback(const SynchronizedCameraImuCallback& callback);
+
+  //! Add Image to the frame synchronizer.
+  void addImgData(
+      int64_t stamp,
+      const ImageBase::Ptr& img,
+      uint32_t camera_idx);
+
+protected:
+  //! Allowed time differences of images in bundle.
+  static constexpr real_t c_camera_bundle_time_accuracy_ns = millisecToNanosec(2.0);
+
+  //! Stamp of previous synchronized image bundle.
+  int64_t last_img_bundle_min_stamp_ { -1 };
+
+  //! Num images to synchronize.
+  uint32_t num_cameras_;
+
+  //! Num IMUs to synchronize.
+  uint32_t num_imus_;
+
+  StampedImages sync_imgs_ready_to_process_;
+  int64_t sync_imgs_ready_to_process_stamp_ { -1 };
+
+  //! Image buffer is buffering images of max the last 2*rig_size images.
+  ImgBuffer img_buffer_;
+
+  //! This function checks if we have all data ready to call the callback.
+  virtual void checkImuDataAndCallback() = 0;
+
+  //! Validate the contents of the IMU buffer relative to the image buffers.
+  bool validateImuBuffers(
+      const int64_t& min_stamp,
+      const int64_t& max_stamp,
+      const std::vector<std::tuple<int64_t, int64_t, bool>>& oldest_newest_stamp_vector);
+
+  //! Max time difference of images in a bundle
+  int64_t img_bundle_max_dt_nsec_ = millisecToNanosec(2.0);
+
+  //! Count number of synchronized frames.
+  int sync_frame_count_  { 0 };
+
+  //! Registered callback for synchronized measurements.
+  SynchronizedCameraImuCallback cam_imu_callback_;
+};
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_data_provider/include/ze/data_provider/camera_imu_synchronizer_unsync.hpp b/RWR/src/ze_oss/ze_data_provider/include/ze/data_provider/camera_imu_synchronizer_unsync.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..1d357fccb4706d05194328bd26b9d79fcb79abac
--- /dev/null
+++ b/RWR/src/ze_oss/ze_data_provider/include/ze/data_provider/camera_imu_synchronizer_unsync.hpp
@@ -0,0 +1,82 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#pragma once
+
+#include <memory>
+#include <imp/core/image_base.hpp>
+#include <ze/common/time_conversions.hpp>
+#include <ze/common/types.hpp>
+#include <ze/data_provider/camera_imu_synchronizer_base.hpp>
+#include <ze/imu/imu_buffer.hpp>
+
+namespace ze {
+
+// fwd
+class DataProviderBase;
+class ImageBase;
+
+// -----------------------------------------------------------------------------
+//! Uses an IMU Buffer that can handle model-based measurement corrections and
+//! synchronization of gyroscope and accelerometer.
+class CameraImuSynchronizerUnsync: public CameraImuSynchronizerBase
+{
+public:
+  // convenience typedefs
+  using ImuSyncBuffer = ImuBufferLinear2000;
+  using ImuBufferVector = std::vector<ImuSyncBuffer::Ptr>;
+
+  //! Default constructor.
+  CameraImuSynchronizerUnsync(DataProviderBase& data_provider,
+                              const std::vector<ImuModel::Ptr>& imu_models);
+
+  //! Add a Gyroscope measurement to the frame synchronizer.
+  void addGyroData(
+      int64_t stamp,
+      const Vector3& gyr,
+      const uint32_t imu_idx);
+
+  //! Add an Accelerometer measurement to the frame synchronizer.
+  void addAccelData(
+      int64_t stamp,
+      const Vector3& acc,
+      const uint32_t imu_idx);
+
+private:
+  //! IMU buffer stores all imu measurements, size of imu_count_.
+  ImuBufferVector imu_buffers_;
+
+  //! Register callbacks in data provider to this class' addImgData, addGyroData
+  //! and addAccelData.
+  void subscribeDataProvider(DataProviderBase& data_provider);
+
+  //! Initialize the image and gyro/accel buffers
+  void initBuffers(const std::vector<ImuModel::Ptr>& imu_models);
+
+  //! This function checks if we have all data ready to call the callback.
+  virtual void checkImuDataAndCallback();
+};
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_data_provider/include/ze/data_provider/data_provider_base.hpp b/RWR/src/ze_oss/ze_data_provider/include/ze/data_provider/data_provider_base.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..ff42aafc7147e75e6fc221e37b3d6c0dbd39be85
--- /dev/null
+++ b/RWR/src/ze_oss/ze_data_provider/include/ze/data_provider/data_provider_base.hpp
@@ -0,0 +1,129 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#pragma once
+
+#include <atomic>
+#include <functional>
+#include <memory>
+
+#include <ze/common/macros.hpp>
+#include <ze/common/noncopyable.hpp>
+#include <ze/common/signal_handler.hpp>
+#include <ze/common/types.hpp>
+
+// fwd
+namespace cv {
+class Mat;
+}
+
+namespace ze {
+
+// fwd
+class ImageBase;
+
+using ImuCallback =
+  std::function<void (int64_t /*timestamp*/,
+                      const Vector3& /*acc*/,
+                      const Vector3& /*gyr*/,
+                      uint32_t /*imu-idx*/)>;
+
+using GyroCallback =
+  std::function<void (int64_t /*timestamp*/,
+                      const Vector3& /*gyr*/,
+                      uint32_t /*imu-idx*/)>;
+using AccelCallback =
+  std::function<void (int64_t /*timestamp*/,
+                      const Vector3& /*acc*/,
+                      uint32_t /*imu-idx*/)>;
+
+using CameraCallback =
+  std::function<void (int64_t /*timestamp*/,
+                      const std::shared_ptr<ImageBase>& /*img*/,
+                      uint32_t /*camera-idx*/)>;
+
+enum class DataProviderType {
+  Csv,
+  Rosbag,
+  Rostopic
+};
+
+//! A data provider registers to a data source and triggers callbacks when
+//! new data is available.
+class DataProviderBase : Noncopyable
+{
+public:
+  ZE_POINTER_TYPEDEFS(DataProviderBase);
+
+  DataProviderBase() = delete;
+  DataProviderBase(DataProviderType type);
+  virtual ~DataProviderBase() = default;
+
+  //! Process all callbacks. Waits until callback is processed.
+  void spin();
+
+  //! Read next data field and process callback. Returns false when datatset finished.
+  virtual bool spinOnce() = 0;
+
+  //! False if there is no more data to process or there was a shutdown signal.
+  virtual bool ok() const = 0;
+
+  //! Pause data provider.
+  virtual void pause();
+
+  //! Stop data provider.
+  virtual void shutdown();
+
+  //! Number of imus to process.
+  virtual size_t imuCount() const = 0;
+
+  //! Number of cameras to process.
+  virtual size_t cameraCount() const = 0;
+
+  //! Register callback function to call when new IMU message is available.
+  void registerImuCallback(const ImuCallback& imu_callback);
+
+  //! Register callback function to call when new camera message is available.
+  void registerCameraCallback(const CameraCallback& camera_callback);
+
+  //! Register callback function to call when new Gyroscope message is available.
+  void registerGyroCallback(const GyroCallback& gyro_callback);
+
+  //! Register callback function to call when new Accelerometer message is available.
+  void registerAccelCallback(const AccelCallback& accel_callback);
+
+protected:
+  DataProviderType type_;
+  ImuCallback imu_callback_;
+  CameraCallback camera_callback_;
+  GyroCallback gyro_callback_;
+  AccelCallback accel_callback_;
+  volatile bool running_ = true;
+
+private:
+  SimpleSigtermHandler signal_handler_; //!< Sets running_ to false when Ctrl-C is pressed.
+};
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_data_provider/include/ze/data_provider/data_provider_csv.hpp b/RWR/src/ze_oss/ze_data_provider/include/ze/data_provider/data_provider_csv.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..295518f3092403ec3ad327ef4140dad2ca065530
--- /dev/null
+++ b/RWR/src/ze_oss/ze_data_provider/include/ze/data_provider/data_provider_csv.hpp
@@ -0,0 +1,96 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#pragma once
+
+#include <map>
+#include <memory>
+#include <string>
+#include <vector>
+
+#include <ze/common/macros.hpp>
+#include <ze/common/types.hpp>
+#include <ze/data_provider/data_provider_base.hpp>
+
+
+namespace ze {
+
+//fwd
+namespace internal {
+struct MeasurementBase;
+}
+
+class DataProviderCsv : public DataProviderBase
+{
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  using StampMeasurementPair = std::pair<int64_t, std::shared_ptr<internal::MeasurementBase>> ;
+  using DataBuffer = std::multimap<int64_t, std::shared_ptr<internal::MeasurementBase>> ;
+
+  DataProviderCsv(
+      const std::string& csv_directory,
+      const std::map<std::string, size_t>& imu_topics,
+      const std::map<std::string, size_t>& camera_topics);
+
+  virtual ~DataProviderCsv() = default;
+
+  virtual bool spinOnce() override;
+
+  virtual bool ok() const override;
+
+  virtual size_t imuCount() const;
+
+  virtual size_t cameraCount() const;
+
+  inline size_t size() const
+  {
+    return buffer_.size();
+  }
+
+private:
+  void loadImuData(
+      const std::string data_dir,
+      const size_t imu_index,
+      const int64_t playback_delay);
+
+  void loadCameraData(
+      const std::string& data_dir,
+      const size_t camera_index,
+      int64_t playback_delay);
+
+  //! Buffer to chronologically sort the data.
+  DataBuffer buffer_;
+
+  //! Points to the next published buffer value. Buffer can't change once loaded!
+  DataBuffer::const_iterator buffer_it_;
+
+  std::map<std::string, size_t> imu_topics_;
+  std::map<std::string, size_t> camera_topics_;
+
+  size_t imu_count_ = 0u;
+};
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_data_provider/include/ze/data_provider/data_provider_factory.hpp b/RWR/src/ze_oss/ze_data_provider/include/ze/data_provider/data_provider_factory.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..c2539e2b11a7d7de98130bf0e336b93394599928
--- /dev/null
+++ b/RWR/src/ze_oss/ze_data_provider/include/ze/data_provider/data_provider_factory.hpp
@@ -0,0 +1,37 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#pragma once
+
+#include <gflags/gflags.h>
+#include <ze/data_provider/data_provider_base.hpp>
+
+DECLARE_uint64(num_imus);
+
+namespace ze {
+
+DataProviderBase::Ptr loadDataProviderFromGflags(const std::uint32_t num_cams);
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_data_provider/include/ze/data_provider/data_provider_rosbag.hpp b/RWR/src/ze_oss/ze_data_provider/include/ze/data_provider/data_provider_rosbag.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..78b759f2864bbbe8447ac5f227a38a383239ed40
--- /dev/null
+++ b/RWR/src/ze_oss/ze_data_provider/include/ze/data_provider/data_provider_rosbag.hpp
@@ -0,0 +1,106 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#pragma once
+
+#include <map>
+#include <string>
+#include <memory>
+#include <vector>
+
+#include <rosbag/bag.h>
+#include <rosbag/view.h>
+
+#include <sensor_msgs/Image.h>
+#include <sensor_msgs/Imu.h>
+#include <ze_ros_msg/bmx055_acc.h>
+#include <ze_ros_msg/bmx055_gyr.h>
+
+#include <ze/data_provider/data_provider_base.hpp>
+
+namespace ze {
+
+class DataProviderRosbag : public DataProviderBase
+{
+public:
+  // optional: imu_topics, required: camera_topics
+  DataProviderRosbag(
+      const std::string& bag_filename,
+      const std::map<std::string, size_t>& imu_topics,
+      const std::map<std::string, size_t>& camera_topics);
+
+  // optional: accel_topics, gyro_topics
+  DataProviderRosbag(
+      const std::string& bag_filename,
+      const std::map<std::string, size_t>& gyro_topics,
+      const std::map<std::string, size_t>& accel_topics,
+      const std::map<std::string, size_t>& camera_topics);
+
+  virtual ~DataProviderRosbag() = default;
+
+  virtual bool spinOnce() override;
+
+  virtual bool ok() const override;
+
+  virtual size_t imuCount() const;
+
+  virtual size_t cameraCount() const;
+
+  size_t size() const;
+
+private:
+  void loadRosbag(const std::string& bag_filename);
+  void initBagView(const std::vector<std::string>& topics);
+
+  inline bool cameraSpin(const sensor_msgs::ImageConstPtr m_img,
+                         const rosbag::MessageInstance& m);
+  inline bool imuSpin(const sensor_msgs::ImuConstPtr m_imu,
+                      const rosbag::MessageInstance& m);
+  inline bool accelSpin(const ze_ros_msg::bmx055_accConstPtr m_acc,
+                        const rosbag::MessageInstance& m);
+  inline bool gyroSpin(const ze_ros_msg::bmx055_gyrConstPtr m_gyr,
+                       const rosbag::MessageInstance& m);
+
+  std::unique_ptr<rosbag::Bag> bag_;
+  std::unique_ptr<rosbag::View> bag_view_;
+  rosbag::View::iterator bag_view_it_;
+  int n_processed_images_ = 0;
+
+  // subscribed topics:
+  std::map<std::string, size_t> img_topic_camidx_map_; // camera_topic --> camera_id
+  std::map<std::string, size_t> imu_topic_imuidx_map_; // imu_topic --> imu_id
+
+  std::map<std::string, size_t> accel_topic_imuidx_map_; // accel_topic --> imu_id
+  std::map<std::string, size_t> gyro_topic_imuidx_map_; // gyro_topic --> imu_id
+
+  //! Do we operate on split ros messages or the combined imu messages?
+  bool uses_split_messages_;
+
+  int64_t last_imu_stamp_ = -1;
+  int64_t last_acc_stamp_ = -1;
+  int64_t last_gyr_stamp_ = -1;
+};
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_data_provider/include/ze/data_provider/data_provider_rostopic.hpp b/RWR/src/ze_oss/ze_data_provider/include/ze/data_provider/data_provider_rostopic.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..4b813286f1a55a1c19863f3df200ab367699b113
--- /dev/null
+++ b/RWR/src/ze_oss/ze_data_provider/include/ze/data_provider/data_provider_rostopic.hpp
@@ -0,0 +1,103 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#pragma once
+
+#include <map>
+#include <string>
+#include <memory>
+#include <vector>
+
+#include <ros/callback_queue.h>
+#include <ros/ros.h>
+#include <sensor_msgs/Image.h>
+#include <sensor_msgs/Imu.h>
+#include <ze_ros_msg/bmx055_acc.h>
+#include <ze_ros_msg/bmx055_gyr.h>
+#include <image_transport/image_transport.h>
+
+#include <ze/data_provider/data_provider_base.hpp>
+
+namespace ze {
+
+class DataProviderRostopic : public DataProviderBase
+{
+public:
+  // optional: imu_topics, required: camera_topics
+  DataProviderRostopic(
+      const std::map<std::string, size_t>& imu_topics,
+      const std::map<std::string, size_t>& camera_topics,
+      uint32_t polling_rate = 1000u,
+      uint32_t img_queue_size = 100u,
+      uint32_t imu_queue_size = 1000u);
+
+  // optional: accel_topics, gyro_topics, required: camera_topics
+  DataProviderRostopic(
+      const std::map<std::string, size_t>& accel_topics,
+      const std::map<std::string, size_t>& gyro_topics,
+      const std::map<std::string, size_t>& camera_topics,
+      uint32_t polling_rate = 1000u,
+      uint32_t img_queue_size = 100u,
+      uint32_t imu_queue_size = 1000u);
+
+
+  virtual ~DataProviderRostopic() = default;
+
+  virtual bool spinOnce() override;
+
+  virtual bool ok() const override;
+
+  virtual size_t imuCount() const;
+
+  virtual size_t cameraCount() const;
+
+  void imgCallback(
+      const sensor_msgs::ImageConstPtr& m_img,
+      uint32_t cam_idx);
+
+  void imuCallback(
+      const sensor_msgs::ImuConstPtr& m_imu,
+      uint32_t imu_idx);
+
+  void accelCallback(const ze_ros_msg::bmx055_accConstPtr& m_acc,
+                     uint32_t imu_idx);
+  void gyroCallback(const ze_ros_msg::bmx055_gyrConstPtr& m_gyr,
+                    uint32_t imu_idx);
+
+private:
+  ros::CallbackQueue queue_;
+  ros::NodeHandle nh_;
+  image_transport::ImageTransport img_transport_;
+  std::vector<image_transport::Subscriber> sub_cams_;
+  std::vector<ros::Subscriber> sub_imus_;
+  std::vector<ros::Subscriber> sub_accels_;
+  std::vector<ros::Subscriber> sub_gyros_;
+  uint32_t polling_rate_;
+
+  //! Do we operate on split ros messages or the combined imu messages?
+  bool uses_split_messages_;
+};
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_data_provider/package.xml b/RWR/src/ze_oss/ze_data_provider/package.xml
new file mode 100644
index 0000000000000000000000000000000000000000..efac5ffc83f2827ab39956b1932fb3e05f428273
--- /dev/null
+++ b/RWR/src/ze_oss/ze_data_provider/package.xml
@@ -0,0 +1,29 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>ze_data_provider</name>
+  <version>0.1.4</version>
+  <description>
+    Read data from csv, bagfiles etc.
+  </description>
+  <maintainer email="christian.forster@WyssZurich.ch">Christian Forster</maintainer>
+  <license>ZE</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <buildtool_depend>catkin_simple</buildtool_depend>
+
+  <depend>eigen_catkin</depend>
+  <depend>glog_catkin</depend>
+  <depend>gflags_catkin</depend>
+  <depend>imp_core</depend>
+  <depend>imp_bridge_opencv</depend>
+  <depend>imp_bridge_ros</depend>
+  <depend>minkindr</depend>
+  <depend>rosbag</depend>
+  <depend>sensor_msgs</depend>
+  <depend>ze_cmake</depend>
+  <depend>ze_common</depend>
+  <depend>ze_imu</depend>
+  <depend>ze_ros_msg</depend>
+
+  <test_depend>gtest</test_depend>
+</package>
diff --git a/RWR/src/ze_oss/ze_data_provider/src/camera_imu_synchronizer.cpp b/RWR/src/ze_oss/ze_data_provider/src/camera_imu_synchronizer.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d5735391b46914db4402153c43fbdf488b0d9f0f
--- /dev/null
+++ b/RWR/src/ze_oss/ze_data_provider/src/camera_imu_synchronizer.cpp
@@ -0,0 +1,149 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include <ze/data_provider/camera_imu_synchronizer.hpp>
+
+#include <functional>
+#include <gflags/gflags.h>
+
+#include <ze/common/logging.hpp>
+#include <ze/data_provider/data_provider_base.hpp>
+
+namespace ze {
+
+CameraImuSynchronizer::CameraImuSynchronizer(DataProviderBase& data_provider)
+  : CameraImuSynchronizerBase(data_provider)
+{
+  subscribeDataProvider(data_provider);
+  initBuffers();
+}
+
+void CameraImuSynchronizer::subscribeDataProvider(DataProviderBase& data_provider)
+{
+  using namespace std::placeholders;
+  if (num_cameras_ == 0u)
+  {
+    LOG(ERROR) << "DataProvider must at least expose a single camera topic.";
+  }
+  data_provider.registerCameraCallback(
+        std::bind(&CameraImuSynchronizer::addImgData, this, _1, _2, _3));
+
+  if (num_imus_ > 0u)
+  {
+    data_provider.registerImuCallback(
+          std::bind(&CameraImuSynchronizer::addImuData, this, _1, _2, _3, _4));
+  }
+}
+
+void CameraImuSynchronizer::initBuffers()
+{
+  img_buffer_.resize(2 * num_cameras_);
+  imu_buffers_ = ImuBufferVector(num_imus_);
+}
+
+void CameraImuSynchronizer::addImuData(
+    int64_t stamp, const Vector3& acc, const Vector3& gyr, const uint32_t imu_idx)
+{
+  Vector6 acc_gyr;
+  acc_gyr.head<3>() = acc;
+  acc_gyr.tail<3>() = gyr;
+  imu_buffers_[imu_idx].insert(stamp, acc_gyr);
+  checkImuDataAndCallback();
+}
+
+void CameraImuSynchronizer::checkImuDataAndCallback()
+{
+  if (sync_imgs_ready_to_process_stamp_ < 0)
+  {
+    return; // Images are not synced yet.
+  }
+
+  // always provide imu structures in the callback (empty if no imu present)
+  ImuStampsVector imu_timestamps(num_imus_);
+  ImuAccGyrVector imu_measurements(num_imus_);
+
+  if (num_imus_ != 0)
+  {
+    // get oldest / newest stamp for all imu buffers
+    std::vector<std::tuple<int64_t, int64_t, bool>> oldest_newest_stamp_vector(num_imus_);
+    std::transform(
+          imu_buffers_.begin(),
+          imu_buffers_.end(),
+          oldest_newest_stamp_vector.begin(),
+          [](const ImuSyncBuffer& imu_buffer) {
+            return imu_buffer.getOldestAndNewestStamp();
+          });
+
+    // imu buffers are not consistent with the image buffers
+    if (!validateImuBuffers(
+          sync_imgs_ready_to_process_stamp_,
+          sync_imgs_ready_to_process_stamp_,
+          oldest_newest_stamp_vector))
+    {
+      return;
+    }
+
+    // If this is the very first image bundle, we send all IMU messages that we have
+    // received so far. For every later image bundle, we just send the IMU messages
+    // that we have received in between.
+    for (size_t i = 0; i < num_imus_; ++i)
+    {
+      if(last_img_bundle_min_stamp_ < 0)
+      {
+        int64_t oldest_stamp = std::get<0>(oldest_newest_stamp_vector[i]);
+        std::tie(imu_timestamps[i], imu_measurements[i]) =
+            imu_buffers_[i].getBetweenValuesInterpolated(
+              oldest_stamp,
+              sync_imgs_ready_to_process_stamp_);
+      }
+      else
+      {
+        std::tie(imu_timestamps[i], imu_measurements[i]) =
+            imu_buffers_[i].getBetweenValuesInterpolated(
+              last_img_bundle_min_stamp_,
+              sync_imgs_ready_to_process_stamp_);
+      }
+    }
+  }
+
+  // Let's process the callback.
+  cam_imu_callback_(sync_imgs_ready_to_process_, imu_timestamps, imu_measurements);
+
+  // Reset Buffer:
+  for (size_t i = 0; i <= img_buffer_.size(); ++i)
+  {
+    ImageBufferItem& item = img_buffer_[i];
+    if (std::abs(sync_imgs_ready_to_process_stamp_ - item.stamp)
+        < c_camera_bundle_time_accuracy_ns)
+    {
+      item.reset();
+    }
+  }
+  last_img_bundle_min_stamp_ = sync_imgs_ready_to_process_stamp_;
+  sync_imgs_ready_to_process_stamp_ = -1;
+  sync_imgs_ready_to_process_.clear();
+}
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_data_provider/src/camera_imu_synchronizer_base.cpp b/RWR/src/ze_oss/ze_data_provider/src/camera_imu_synchronizer_base.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..8385416fb3190aee2f9dcb84337825022c091987
--- /dev/null
+++ b/RWR/src/ze_oss/ze_data_provider/src/camera_imu_synchronizer_base.cpp
@@ -0,0 +1,198 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include <ze/data_provider/camera_imu_synchronizer_base.hpp>
+
+#include <ze/data_provider/data_provider_base.hpp>
+
+namespace ze {
+
+DEFINE_int32(data_sync_init_skip_n_frames, 0,
+             "How many frames should be skipped at the beginning.");
+
+DEFINE_int32(data_sync_stop_after_n_frames, -1,
+             "How many frames should be processed?");
+
+CameraImuSynchronizerBase::CameraImuSynchronizerBase(
+    DataProviderBase& data_provider)
+  : num_cameras_(data_provider.cameraCount())
+  , num_imus_(data_provider.imuCount())
+{
+}
+
+void CameraImuSynchronizerBase::registerCameraImuCallback(
+    const SynchronizedCameraImuCallback& callback)
+{
+  cam_imu_callback_ = callback;
+}
+
+void CameraImuSynchronizerBase::addImgData(
+    int64_t stamp, const ImageBase::Ptr& img, uint32_t camera_idx)
+{
+  CHECK_LT(camera_idx, num_cameras_);
+
+  // Skip frame processing for first N frames.
+  if (sync_frame_count_ < FLAGS_data_sync_init_skip_n_frames)
+  {
+    if (camera_idx == 0)
+    {
+      ++sync_frame_count_;
+    }
+    return;
+  }
+
+  // Add image to first available slot in our buffer:
+  int slot = -1;
+  for (size_t i = 0u; i < img_buffer_.size(); ++i)
+  {
+    if (img_buffer_[i].empty())
+    {
+      slot = i;
+      break;
+    }
+  }
+
+  if (slot == -1)
+  {
+    // No space in buffer to process frame. Delete oldest one. This happens
+    // also when the processing is not fast enough such that frames are skipped.
+    int64_t min_stamp = std::numeric_limits<int64_t>::max();
+    for (size_t i = 0u; i < img_buffer_.size(); ++i)
+    {
+      if (!img_buffer_[i].empty() && img_buffer_[i].stamp < min_stamp)
+      {
+        slot = i;
+        min_stamp = img_buffer_[i].stamp;
+      }
+    }
+  }
+
+  img_buffer_[slot].stamp = stamp;
+  img_buffer_[slot].img = img;
+  img_buffer_[slot].camera_idx = camera_idx;
+
+  // Now check, if we have all images from this bundle:
+  uint32_t num_imgs = 0u;
+  for (size_t i = 0; i <= img_buffer_.size(); ++i)
+  {
+    if (std::abs(stamp - img_buffer_[i].stamp) < millisecToNanosec(2))
+    {
+      ++num_imgs;
+    }
+  }
+
+  if (num_imgs != num_cameras_)
+  {
+    return; // We don't have all frames yet.
+  }
+
+  // We have frames with very close timestamps. Put them together in a vector.
+  sync_imgs_ready_to_process_.clear();
+  sync_imgs_ready_to_process_.resize(num_imgs, {-1, nullptr});
+  for (size_t i = 0; i <= img_buffer_.size(); ++i)
+  {
+    ImageBufferItem& item = img_buffer_[i];
+    if (std::abs(stamp - item.stamp) < c_camera_bundle_time_accuracy_ns)
+    {
+      DEBUG_CHECK_GT(item.stamp, 0);
+      DEBUG_CHECK(item.img);
+      sync_imgs_ready_to_process_.at(item.camera_idx) = { item.stamp, item.img };
+    }
+  }
+
+  // Double-check that we have all images.
+  for (size_t i = 0; i < sync_imgs_ready_to_process_.size(); ++i)
+  {
+    if (sync_imgs_ready_to_process_.at(i).first == -1)
+    {
+      LOG(ERROR) << "Sync images failed!";
+      sync_imgs_ready_to_process_.clear();
+      return;
+    }
+  }
+  sync_imgs_ready_to_process_stamp_ = sync_imgs_ready_to_process_.front().first;
+
+  checkImuDataAndCallback();
+}
+
+bool CameraImuSynchronizerBase::validateImuBuffers(
+    const int64_t& min_stamp,
+    const int64_t& max_stamp,
+    const std::vector<std::tuple<int64_t, int64_t, bool> >&
+      oldest_newest_stamp_vector)
+{
+  // Check if we have received some IMU measurements for at least one of the imu's.
+  if (std::none_of(oldest_newest_stamp_vector.begin(),
+                   oldest_newest_stamp_vector.end(),
+                   [](const std::tuple<int64_t, int64_t, bool>& oldest_newest_stamp)
+                   {
+                     if (std::get<2>(oldest_newest_stamp))
+                     {
+                       return true;
+                     }
+                     return false;
+                   })
+      )
+  {
+    LOG(WARNING) << "Received all images but no imu measurements!";
+    return false;
+  }
+
+  // At least one IMU measurements before image
+  if (std::none_of(oldest_newest_stamp_vector.begin(),
+                   oldest_newest_stamp_vector.end(),
+                   [&min_stamp](const std::tuple<int64_t, int64_t, bool>& oldest_newest_stamp)
+                   {
+                     if (std::get<0>(oldest_newest_stamp) < min_stamp) {
+                       return true;
+                     }
+                     return false;
+                   })
+     )
+  {
+    LOG(WARNING) << "Oldest IMU measurement is newer than image timestamp.";
+    return false;
+  }
+
+  // At least one IMU measurements after image
+  if (std::none_of(oldest_newest_stamp_vector.begin(),
+                   oldest_newest_stamp_vector.end(),
+                   [&max_stamp](const std::tuple<int64_t, int64_t, bool>& oldest_newest_stamp) {
+                     if (std::get<1>(oldest_newest_stamp) > max_stamp)
+                     {
+                       return true;
+                     }
+                     return false;
+                   })
+     )
+  {
+    VLOG(100) << "Waiting for IMU measurements.";
+    return false;
+  }
+
+  return true;
+}
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_data_provider/src/camera_imu_synchronizer_unsync.cpp b/RWR/src/ze_oss/ze_data_provider/src/camera_imu_synchronizer_unsync.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..41a516782fa53bf168fe926c9abbfb3a1339bb93
--- /dev/null
+++ b/RWR/src/ze_oss/ze_data_provider/src/camera_imu_synchronizer_unsync.cpp
@@ -0,0 +1,162 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include <ze/data_provider/camera_imu_synchronizer_unsync.hpp>
+
+#include <functional>
+#include <gflags/gflags.h>
+
+#include <ze/common/logging.hpp>
+#include <ze/data_provider/data_provider_base.hpp>
+
+namespace ze {
+
+CameraImuSynchronizerUnsync::CameraImuSynchronizerUnsync(
+    DataProviderBase& data_provider,
+    const std::vector<ImuModel::Ptr>& imu_models)
+  : CameraImuSynchronizerBase(data_provider)
+{
+  subscribeDataProvider(data_provider);
+  initBuffers(imu_models);
+}
+
+void CameraImuSynchronizerUnsync::subscribeDataProvider(
+    DataProviderBase& data_provider)
+{
+  using namespace std::placeholders;
+  if (num_cameras_ == 0u)
+  {
+    LOG(ERROR) << "DataProvider must at least expose a single camera topic.";
+  }
+  data_provider.registerCameraCallback(
+        std::bind(&CameraImuSynchronizerUnsync::addImgData, this, _1, _2, _3));
+
+  if (num_imus_ > 0u)
+  {
+    data_provider.registerAccelCallback(
+          std::bind(&CameraImuSynchronizerUnsync::addAccelData, this, _1, _2, _3));
+    data_provider.registerGyroCallback(
+          std::bind(&CameraImuSynchronizerUnsync::addGyroData, this, _1, _2, _3));
+  }
+}
+
+void CameraImuSynchronizerUnsync::initBuffers(
+    const std::vector<ImuModel::Ptr>& imu_models)
+{
+  img_buffer_.resize(2 * num_cameras_);
+  for (ImuModel::Ptr imu_model: imu_models)
+  {
+    imu_buffers_.push_back(std::make_shared<ImuSyncBuffer>(imu_model));
+  }
+}
+
+void CameraImuSynchronizerUnsync::addAccelData(
+    int64_t stamp, const Vector3& acc, const uint32_t imu_idx)
+{
+  imu_buffers_[imu_idx]->insertAccelerometerMeasurement(stamp, acc);
+  checkImuDataAndCallback();
+}
+
+void CameraImuSynchronizerUnsync::addGyroData(
+    int64_t stamp, const Vector3& gyr, const uint32_t imu_idx)
+{
+  imu_buffers_[imu_idx]->insertGyroscopeMeasurement(stamp, gyr);
+  checkImuDataAndCallback();
+}
+
+void CameraImuSynchronizerUnsync::checkImuDataAndCallback()
+{
+  if (sync_imgs_ready_to_process_stamp_ < 0)
+  {
+    return; // Images are not synced yet.
+  }
+
+  // always provide imu structures in the callback (empty if no imu present)
+  ImuStampsVector imu_timestamps(num_imus_);
+  ImuAccGyrVector imu_measurements(num_imus_);
+
+  if (num_imus_ != 0)
+  {
+    // get oldest / newest stamp for all imu buffers
+    std::vector<std::tuple<int64_t, int64_t, bool>> oldest_newest_stamp_vector(num_imus_);
+    std::transform(
+          imu_buffers_.begin(),
+          imu_buffers_.end(),
+          oldest_newest_stamp_vector.begin(),
+          [](const ImuSyncBuffer::Ptr imu_buffer) {
+            return imu_buffer->getOldestAndNewestStamp();
+          });
+
+    // imu buffers are not consistent with the image buffers
+    if (!validateImuBuffers(
+          sync_imgs_ready_to_process_stamp_,
+          sync_imgs_ready_to_process_stamp_,
+          oldest_newest_stamp_vector))
+    {
+      return;
+    }
+
+    // If this is the very first image bundle, we send all IMU messages that we have
+    // received so far. For every later image bundle, we just send the IMU messages
+    // that we have received in between.
+    for (size_t i = 0; i < num_imus_; ++i)
+    {
+      if(last_img_bundle_min_stamp_ < 0)
+      {
+        int64_t oldest_stamp = std::get<0>(oldest_newest_stamp_vector[i]);
+        std::tie(imu_timestamps[i], imu_measurements[i]) =
+            imu_buffers_[i]->getBetweenValuesInterpolated(
+              oldest_stamp,
+              sync_imgs_ready_to_process_stamp_);
+      }
+      else
+      {
+        std::tie(imu_timestamps[i], imu_measurements[i]) =
+            imu_buffers_[i]->getBetweenValuesInterpolated(
+              last_img_bundle_min_stamp_,
+              sync_imgs_ready_to_process_stamp_);
+      }
+    }
+  }
+
+  // Let's process the callback.
+  cam_imu_callback_(sync_imgs_ready_to_process_, imu_timestamps, imu_measurements);
+
+  // Reset Buffer:
+  for (size_t i = 0; i <= img_buffer_.size(); ++i)
+  {
+    ImageBufferItem& item = img_buffer_[i];
+    if (std::abs(sync_imgs_ready_to_process_stamp_ - item.stamp)
+        < c_camera_bundle_time_accuracy_ns)
+    {
+      item.reset();
+    }
+  }
+  last_img_bundle_min_stamp_ = sync_imgs_ready_to_process_stamp_;
+  sync_imgs_ready_to_process_stamp_ = -1;
+  sync_imgs_ready_to_process_.clear();
+}
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_data_provider/src/data_provider_base.cpp b/RWR/src/ze_oss/ze_data_provider/src/data_provider_base.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..1eacd85258a05784aa45e68dfbe0e4a1dd85b4c8
--- /dev/null
+++ b/RWR/src/ze_oss/ze_data_provider/src/data_provider_base.cpp
@@ -0,0 +1,73 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include <ze/data_provider/data_provider_base.hpp>
+
+namespace ze {
+
+DataProviderBase::DataProviderBase(DataProviderType type)
+  : type_(type)
+  , signal_handler_(running_)
+{}
+
+void DataProviderBase::spin()
+{
+  while (ok())
+  {
+    spinOnce();
+  }
+}
+
+void DataProviderBase::pause()
+{
+  running_ = false;
+}
+
+void DataProviderBase::shutdown()
+{
+  running_ = false;
+}
+
+void DataProviderBase::registerImuCallback(const ImuCallback& imu_callback)
+{
+  imu_callback_ = imu_callback;
+}
+
+void DataProviderBase::registerGyroCallback(const GyroCallback& gyro_callback)
+{
+  gyro_callback_ = gyro_callback;
+}
+
+void DataProviderBase::registerAccelCallback(const AccelCallback& accel_callback)
+{
+  accel_callback_ = accel_callback;
+}
+
+void DataProviderBase::registerCameraCallback(const CameraCallback& camera_callback)
+{
+  camera_callback_ = camera_callback;
+}
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_data_provider/src/data_provider_csv.cpp b/RWR/src/ze_oss/ze_data_provider/src/data_provider_csv.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..7fb5ca0b5b05d004b7ff05271328ddf6069ff853
--- /dev/null
+++ b/RWR/src/ze_oss/ze_data_provider/src/data_provider_csv.cpp
@@ -0,0 +1,301 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include <ze/data_provider/data_provider_csv.hpp>
+
+#include <fstream>
+#include <iostream>
+#include <ze/common/logging.hpp>
+
+#include <imp/bridge/opencv/cv_bridge.hpp>
+#include <imp/core/image_base.hpp>
+#include <ze/common/time_conversions.hpp>
+#include <ze/common/string_utils.hpp>
+#include <ze/common/file_utils.hpp>
+
+namespace ze {
+namespace internal {
+
+enum class MeasurementType
+{
+  Imu,
+  Camera,
+  FeatureTrack,
+};
+
+struct MeasurementBase
+{
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  ZE_POINTER_TYPEDEFS(MeasurementBase);
+
+public:
+  MeasurementBase() = delete;
+  virtual ~MeasurementBase() = default;
+
+protected:
+  MeasurementBase(int64_t stamp_ns, MeasurementType type)
+    : stamp_ns(stamp_ns)
+    , type(type)
+  {}
+
+public:
+  const int64_t stamp_ns;
+  const MeasurementType type;
+};
+
+struct ImuMeasurement : public MeasurementBase
+{
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  ZE_POINTER_TYPEDEFS(ImuMeasurement);
+
+  ImuMeasurement() = delete;
+  ImuMeasurement(int64_t stamp_ns, const size_t imu_idx,
+                 const Vector3& acc, const Vector3& gyr)
+    : MeasurementBase(stamp_ns, MeasurementType::Imu)
+    , acc(acc)
+    , gyr(gyr)
+    , imu_index(imu_idx)
+  {}
+  virtual ~ImuMeasurement() = default;
+
+  const Vector3 acc;
+  const Vector3 gyr;
+  const size_t imu_index;
+};
+
+struct CameraMeasurement : public MeasurementBase
+{
+  ZE_POINTER_TYPEDEFS(CameraMeasurement);
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  CameraMeasurement() = delete;
+  CameraMeasurement(int64_t stamp_ns, size_t cam_idx, const std::string& img_path)
+    : MeasurementBase(stamp_ns, MeasurementType::Camera)
+    , camera_index(cam_idx)
+    , image_filename(img_path)
+  {}
+  virtual ~CameraMeasurement() = default;
+
+  inline ImageBase::Ptr loadImage() const
+  {
+    //! @todo: Make an option which pixel-type to load.
+    ImageCv8uC1::Ptr img;
+    cvBridgeLoad<Pixel8uC1>(img, image_filename, PixelOrder::gray);
+    CHECK_NOTNULL(img.get());
+    CHECK(img->numel() > 0);
+    return img;
+  }
+
+  const size_t camera_index;
+  const std::string image_filename;
+};
+
+struct FeatureTrackMeasurement : public MeasurementBase
+{
+  ZE_POINTER_TYPEDEFS(FeatureTrackMeasurement);
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  FeatureTrackMeasurement() = delete;
+  FeatureTrackMeasurement(int64_t stamp_ns, size_t cam_idx, int track_id,
+                          const Eigen::Vector2d& keypoint_measurement,
+                          double keypoint_std_dev)
+    : MeasurementBase(stamp_ns, MeasurementType::FeatureTrack)
+    , camera_index(cam_idx)
+    , track_id(track_id)
+    , keypoint_measurement(keypoint_measurement)
+    , keypoint_std_dev(keypoint_std_dev)
+  {}
+  virtual ~FeatureTrackMeasurement() = default;
+
+  const size_t camera_index;
+  const int track_id;
+  const Eigen::Vector2d keypoint_measurement;
+  const double keypoint_std_dev;
+};
+
+} // namespace internal
+
+DataProviderCsv::DataProviderCsv(
+    const std::string& csv_directory,
+    const std::map<std::string, size_t>& imu_topics,
+    const std::map<std::string, size_t>& camera_topics)
+  : DataProviderBase(DataProviderType::Csv)
+  , imu_topics_(imu_topics)
+  , camera_topics_(camera_topics)
+{
+  VLOG(1) << "Loading .csv dataset from directory \"" << csv_directory << "\".";
+
+  for (auto it : imu_topics)
+  {
+    std::string dir = joinPath(csv_directory, it.first);
+    loadImuData(dir, it.second, 0u);
+  }
+
+  for (auto it : camera_topics)
+  {
+    std::string dir = joinPath(csv_directory, it.first);
+    loadCameraData(dir, it.second, millisecToNanosec(100));
+  }
+
+  buffer_it_ = buffer_.cbegin();
+  VLOG(1) << "done.";
+}
+
+bool DataProviderCsv::spinOnce()
+{
+  if (buffer_it_ != buffer_.cend())
+  {
+    const internal::MeasurementBase::Ptr& data = buffer_it_->second;
+    switch (data->type)
+    {
+    case internal::MeasurementType::Camera:
+    {
+      if (camera_callback_)
+      {
+        internal::CameraMeasurement::ConstPtr cam_data =
+            std::dynamic_pointer_cast<const internal::CameraMeasurement>(data);
+        camera_callback_(cam_data->stamp_ns,
+                         cam_data->loadImage(),
+                         cam_data->camera_index);
+      }
+      else
+      {
+        LOG_FIRST_N(WARNING, 1) << "No camera callback registered but measurements available.";
+      }
+      break;
+    }
+    case internal::MeasurementType::Imu:
+    {
+      if (imu_callback_)
+      {
+        internal::ImuMeasurement::ConstPtr imu_data =
+            std::dynamic_pointer_cast<const internal::ImuMeasurement>(data);
+        imu_callback_(imu_data->stamp_ns,
+                      imu_data->acc,
+                      imu_data->gyr,
+                      imu_data->imu_index);
+      }
+      else
+      {
+        LOG_FIRST_N(WARNING, 1) << "No IMU callback registered but measurements available";
+      }
+      break;
+    }
+    default:
+    {
+      LOG(FATAL) << "Unhandled message type: " << static_cast<int>(data->type);
+      break;
+    }
+    }
+    ++buffer_it_;
+    return true;
+  }
+  return false;
+}
+
+bool DataProviderCsv::ok() const
+{
+  if (!running_)
+  {
+    VLOG(1) << "Data Provider was paused/terminated.";
+    return false;
+  }
+  if (buffer_it_ == buffer_.cend())
+  {
+    VLOG(1) << "All data processed.";
+    return false;
+  }
+  return true;
+}
+
+size_t DataProviderCsv::cameraCount() const
+{
+  return camera_topics_.size();
+}
+
+size_t DataProviderCsv::imuCount() const
+{
+  return imu_topics_.size();
+}
+
+void DataProviderCsv::loadImuData(
+    const std::string data_dir,
+    const size_t imu_index,
+    const int64_t playback_delay)
+{
+  const std::string kHeader = "#timestamp [ns],w_RS_S_x [rad s^-1],w_RS_S_y [rad s^-1],w_RS_S_z [rad s^-1],a_RS_S_x [m s^-2],a_RS_S_y [m s^-2],a_RS_S_z [m s^-2]";
+  std::ifstream fs;
+  openFileStreamAndCheckHeader(data_dir+"/data.csv", kHeader, &fs);
+  std::string line;
+  size_t i = 0;
+  while (std::getline(fs, line))
+  {
+    std::vector<std::string> items = splitString(line, ',');
+    CHECK_EQ(items.size(), 7u);
+    Eigen::Vector3d acc, gyr;
+    acc << std::stod(items[4]), std::stod(items[5]), std::stod(items[6]);
+    gyr << std::stod(items[1]), std::stod(items[2]), std::stod(items[3]);
+    auto imu_measurement =
+        std::make_shared<internal::ImuMeasurement>(
+          std::stoll(items[0]), imu_index,
+          acc.cast<real_t>(), gyr.cast<real_t>());
+
+    buffer_.insert(std::make_pair(
+                     imu_measurement->stamp_ns + playback_delay,
+                     imu_measurement));
+    ++i;
+  }
+  VLOG(2) << "Loaded " << i << " IMU measurements.";
+  fs.close();
+}
+
+void DataProviderCsv::loadCameraData(
+    const std::string& data_dir,
+    const size_t camera_index,
+    int64_t playback_delay)
+{
+  const std::string kHeader = "#timestamp [ns],filename";
+  std::ifstream fs;
+  openFileStreamAndCheckHeader(data_dir+"/data.csv", kHeader, &fs);
+  std::string line;
+  size_t i = 0;
+  while (std::getline(fs, line))
+  {
+    std::vector<std::string> items = splitString(line, ',');
+    CHECK_EQ(items.size(), 2u);
+    auto camera_measurement =
+        std::make_shared<internal::CameraMeasurement>(
+          std::stoll(items[0]), camera_index, data_dir + "/data/" + items[1]);
+
+    buffer_.insert(std::make_pair(
+                     camera_measurement->stamp_ns + playback_delay,
+                     camera_measurement));
+    ++i;
+  }
+  VLOG(2) << "Loaded " << i << " camera measurements.";
+  fs.close();
+}
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_data_provider/src/data_provider_factory.cpp b/RWR/src/ze_oss/ze_data_provider/src/data_provider_factory.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..eb401fb00fcda9eb29afc9116672ab25092d3b57
--- /dev/null
+++ b/RWR/src/ze_oss/ze_data_provider/src/data_provider_factory.cpp
@@ -0,0 +1,152 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include <ze/common/logging.hpp>
+#include <ze/data_provider/data_provider_factory.hpp>
+#include <ze/data_provider/data_provider_base.hpp>
+#include <ze/data_provider/data_provider_csv.hpp>
+#include <ze/data_provider/data_provider_rosbag.hpp>
+#include <ze/data_provider/data_provider_rostopic.hpp>
+
+DEFINE_string(bag_filename, "dataset.bag", "Name of bagfile in data_dir.");
+
+DEFINE_string(topic_cam0, "/cam0/image_raw", "");
+DEFINE_string(topic_cam1, "/cam1/image_raw", "");
+DEFINE_string(topic_cam2, "/cam2/image_raw", "");
+DEFINE_string(topic_cam3, "/cam2/image_raw", "");
+
+DEFINE_string(topic_imu0, "/imu0", "");
+DEFINE_string(topic_imu1, "/imu1", "");
+DEFINE_string(topic_imu2, "/imu2", "");
+DEFINE_string(topic_imu3, "/imu3", "");
+
+DEFINE_string(topic_acc0, "/acc0", "");
+DEFINE_string(topic_acc1, "/acc1", "");
+DEFINE_string(topic_acc2, "/acc2", "");
+DEFINE_string(topic_acc3, "/acc3", "");
+
+DEFINE_string(topic_gyr0, "/gyr0", "");
+DEFINE_string(topic_gyr1, "/gyr1", "");
+DEFINE_string(topic_gyr2, "/gyr2", "");
+DEFINE_string(topic_gyr3, "/gyr3", "");
+
+DEFINE_int32(data_source, 1, " 0: CSV, 1: Rosbag, 2: Rostopic");
+DEFINE_string(data_dir, "", "Directory for csv dataset.");
+DEFINE_uint64(num_imus, 1, "Number of IMUs used in the pipeline.");
+DEFINE_uint64(num_accels, 0, "Number of Accelerometers used in the pipeline.");
+DEFINE_uint64(num_gyros, 0, "Number of Gyroscopes used in the pipeline.");
+
+namespace ze {
+
+DataProviderBase::Ptr loadDataProviderFromGflags(const uint32_t num_cams)
+{
+  CHECK_GT(num_cams, 0u);
+  CHECK_LE(num_cams, 4u);
+  CHECK_LE(FLAGS_num_imus, 4u);
+
+  // Fill camera topics.
+  std::map<std::string, size_t> cam_topics;
+  if (num_cams >= 1) cam_topics[FLAGS_topic_cam0] = 0;
+  if (num_cams >= 2) cam_topics[FLAGS_topic_cam1] = 1;
+  if (num_cams >= 3) cam_topics[FLAGS_topic_cam2] = 2;
+  if (num_cams >= 4) cam_topics[FLAGS_topic_cam3] = 3;
+
+  // Fill imu topics.
+  std::map<std::string, size_t> imu_topics;
+  if (FLAGS_num_imus >= 1) imu_topics[FLAGS_topic_imu0] = 0;
+  if (FLAGS_num_imus >= 2) imu_topics[FLAGS_topic_imu1] = 1;
+  if (FLAGS_num_imus >= 3) imu_topics[FLAGS_topic_imu2] = 2;
+  if (FLAGS_num_imus >= 4) imu_topics[FLAGS_topic_imu3] = 3;
+
+  // Fill accelerometer topics.
+  std::map<std::string, size_t> acc_topics;
+  if (FLAGS_num_accels >= 1) acc_topics[FLAGS_topic_acc0] = 0;
+  if (FLAGS_num_accels >= 2) acc_topics[FLAGS_topic_acc1] = 1;
+  if (FLAGS_num_accels >= 3) acc_topics[FLAGS_topic_acc2] = 2;
+  if (FLAGS_num_accels >= 4) acc_topics[FLAGS_topic_acc3] = 3;
+
+  // Fill gyroscope topics.
+  std::map<std::string, size_t> gyr_topics;
+  if (FLAGS_num_gyros >= 1) gyr_topics[FLAGS_topic_gyr0] = 0;
+  if (FLAGS_num_gyros >= 2) gyr_topics[FLAGS_topic_gyr1] = 1;
+  if (FLAGS_num_gyros >= 3) gyr_topics[FLAGS_topic_gyr2] = 2;
+  if (FLAGS_num_gyros >= 4) gyr_topics[FLAGS_topic_gyr3] = 3;
+
+  // Create data provider.
+  ze::DataProviderBase::Ptr data_provider;
+  switch (FLAGS_data_source)
+  {
+    case 0: // CSV
+    {
+      data_provider.reset(
+            new DataProviderCsv(FLAGS_data_dir, imu_topics, cam_topics));
+      break;
+    }
+    case 1: // Rosbag
+    {
+      // Use the split imu dataprovider
+      if (FLAGS_num_accels != 0 && FLAGS_num_gyros != 0)
+      {
+        data_provider.reset(new DataProviderRosbag(FLAGS_bag_filename,
+                                                   acc_topics,
+                                                   gyr_topics,
+                                                   cam_topics));
+      }
+      else
+      {
+        data_provider.reset(new DataProviderRosbag(FLAGS_bag_filename,
+                                                   imu_topics,
+                                                   cam_topics));
+      }
+      break;
+    }
+    case 2: // Rostopic
+    {
+      // Use the split imu dataprovider
+      if (FLAGS_num_accels != 0 && FLAGS_num_gyros != 0)
+      {
+        data_provider.reset(new DataProviderRostopic(acc_topics,
+                                                     gyr_topics,
+                                                     cam_topics));
+      }
+      else
+      {
+        data_provider.reset(new DataProviderRostopic(imu_topics,
+                                                     cam_topics));
+      }
+
+      break;
+    }
+    default:
+    {
+      LOG(FATAL) << "Data source not known.";
+      break;
+    }
+  }
+
+  return data_provider;
+}
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_data_provider/src/data_provider_rosbag.cpp b/RWR/src/ze_oss/ze_data_provider/src/data_provider_rosbag.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..855f01029121e06ad604e93190a58f397b4046fe
--- /dev/null
+++ b/RWR/src/ze_oss/ze_data_provider/src/data_provider_rosbag.cpp
@@ -0,0 +1,440 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include <ze/data_provider/data_provider_rosbag.hpp>
+
+#include <ze/common/logging.hpp>
+#include <rosbag/query.h>
+
+#include <ze/common/time_conversions.hpp>
+#include <ze/common/string_utils.hpp>
+#include <ze/common/path_utils.hpp>
+
+#include <imp/bridge/ros/ros_bridge.hpp>
+
+DEFINE_int32(data_source_stop_after_n_frames, -1,
+             "How many frames should be processed?");
+DEFINE_double(data_source_start_time_s, 0.0,
+              "Start time in seconds");
+DEFINE_double(data_source_stop_time_s, 0.0,
+              "Stop time in seconds");
+
+namespace ze {
+
+DataProviderRosbag::DataProviderRosbag(
+    const std::string& bag_filename,
+    const std::map<std::string, size_t>& imu_topic_imuidx_map,
+    const std::map<std::string, size_t>& img_topic_camidx_map)
+  : DataProviderBase(DataProviderType::Rosbag)
+  , img_topic_camidx_map_(img_topic_camidx_map)
+  , imu_topic_imuidx_map_(imu_topic_imuidx_map)
+  , uses_split_messages_(false)
+{
+  VLOG(1) << "Create Dataprovider for synchronized Gyro/Accel";
+  //! @todo: Display number of messages per topic in the beginning.
+
+  loadRosbag(bag_filename);
+
+  // empty bracket initialzer calls default constructor and
+  // adds an empty element to the map
+  if (imu_topic_imuidx_map_.size() == 1 &&
+      imu_topic_imuidx_map_.begin()->first.empty())
+  {
+    imu_topic_imuidx_map_.clear();
+  }
+
+  std::vector<std::string> topics;
+  for (auto it : img_topic_camidx_map_)
+  {
+    VLOG(1) << "Subscribing to: " << it.first;
+    topics.push_back(it.first);
+  }
+  for (auto it : imu_topic_imuidx_map_)
+  {
+    VLOG(1) << "Subscribing to: " << it.first;
+    topics.push_back(it.first);
+  }
+
+  initBagView(topics);
+}
+
+DataProviderRosbag::DataProviderRosbag(
+    const std::string& bag_filename,
+    const std::map<std::string, size_t>& accel_topic_imuidx_map,
+    const std::map<std::string, size_t>& gyro_topic_imuidx_map,
+    const std::map<std::string, size_t>& img_topic_camidx_map)
+  : DataProviderBase(DataProviderType::Rosbag)
+  , img_topic_camidx_map_(img_topic_camidx_map)
+  , accel_topic_imuidx_map_(accel_topic_imuidx_map)
+  , gyro_topic_imuidx_map_(gyro_topic_imuidx_map)
+  , uses_split_messages_(true)
+{
+  VLOG(1) << "Create Dataprovider for UN-synchronized Gyro/Accel";
+  //! @todo: Check if topics exists.
+  //! @todo: Display number of messages per topic in the beginning.
+
+  CHECK_EQ(accel_topic_imuidx_map_.size(), gyro_topic_imuidx_map_.size());
+
+  loadRosbag(bag_filename);
+
+  // empty bracket initialzer calls default constructor and
+  // adds an empty element to the map
+  if (accel_topic_imuidx_map_.size() == 1 &&
+      accel_topic_imuidx_map_.begin()->first.empty())
+  {
+    accel_topic_imuidx_map_.clear();
+    gyro_topic_imuidx_map_.clear();
+  }
+
+  std::vector<std::string> topics;
+  for (auto it : img_topic_camidx_map_)
+  {
+    VLOG(1) << "Subscribing to: " << it.first;
+    topics.push_back(it.first);
+  }
+  for (auto it : accel_topic_imuidx_map_)
+  {
+    VLOG(1) << "Subscribing to: " << it.first;
+    topics.push_back(it.first);
+  }
+  for (auto it : gyro_topic_imuidx_map_)
+  {
+    VLOG(1) << "Subscribing to: " << it.first;
+    topics.push_back(it.first);
+  }
+
+  initBagView(topics);
+}
+
+void DataProviderRosbag::loadRosbag(const std::string& bag_filename)
+{
+  CHECK(fileExists(bag_filename)) << "File does not exist: " << bag_filename;
+  VLOG(1) << "Opening rosbag: " << bag_filename << " ...";
+  bag_.reset(new rosbag::Bag);
+  try
+  {
+    bag_->open(bag_filename, rosbag::bagmode::Read);
+  }
+  catch (const std::exception e)
+  {
+    LOG(FATAL) << "Could not open rosbag " << bag_filename << ": " << e.what();
+  }
+}
+
+void DataProviderRosbag::initBagView(const std::vector<std::string>& topics)
+{
+  bag_view_.reset(new rosbag::View(*bag_, rosbag::TopicQuery(topics)));
+  if (FLAGS_data_source_start_time_s != 0.0 ||
+      FLAGS_data_source_stop_time_s != 0.0)
+  {
+    CHECK_GE(FLAGS_data_source_start_time_s, 0);
+    CHECK_GE(FLAGS_data_source_stop_time_s, 0);
+
+    // Retrieve begin and end times from the bag file (given the topic query).
+    const ros::Time absolute_time_offset = bag_view_->getBeginTime();
+    VLOG(2) << "Bag begin time: " << absolute_time_offset;
+    const ros::Time absolute_end_time = bag_view_->getEndTime();
+    VLOG(2) << "Bag end time: " << absolute_end_time;
+    if (absolute_end_time < absolute_time_offset)
+    {
+      LOG(FATAL) << "Invalid bag end time: "
+                 << absolute_end_time
+                 << ". Check that the bag file is properly indexed"
+                 << " by running 'rosbag reindex file.bag'.";
+    }
+
+    // Compute start and stop time.
+    const ros::Duration data_source_start_time(FLAGS_data_source_start_time_s);
+    const ros::Time absolute_start_time =
+        data_source_start_time.isZero() ?
+          absolute_time_offset : absolute_time_offset + data_source_start_time;
+    const ros::Duration data_source_stop_time(FLAGS_data_source_stop_time_s);
+    const ros::Time absolute_stop_time =
+        data_source_stop_time.isZero() ?
+          absolute_end_time : absolute_time_offset + data_source_stop_time;
+
+    // Ensure that the provided stop time is valid.
+    // When a bag file is corrupted / invalid the bag end time
+    // cannot be retrieved. Run rosbag info to check if the bag file
+    // is properly indexed.
+    if (absolute_stop_time < absolute_start_time)
+    {
+      LOG(ERROR) << "Provided stop time is less than bag begin time. "
+                 << "Please make sure to provide a valid stop time and "
+                 << "check that the bag file is properly indexed "
+                 << "by running 'rosbag reindex file.bag'.";
+    }
+    else if (absolute_stop_time > absolute_end_time)
+    {
+      LOG(ERROR) << "Provided stop time is greater than bag end time. "
+                 << "Please make sure to provide a valid stop time and "
+                 << "check that the bag file is properly indexed "
+                 << "by running 'rosbag reindex file.bag'.";
+    }
+    else
+    {
+      VLOG(1) << "Absolute start time set to " << absolute_start_time;
+      VLOG(1) << "Absolute stop time set to " << absolute_stop_time;
+    }
+
+    // Reset the bag View
+    CHECK_GT(absolute_stop_time, absolute_start_time);
+    CHECK_LE(absolute_stop_time, absolute_end_time);
+    bag_view_.reset(new rosbag::View(*bag_, rosbag::TopicQuery(topics),
+                                     absolute_start_time, absolute_stop_time));
+  }
+  bag_view_it_ = bag_view_->begin();
+
+  // Ensure that topics exist
+  // The connection info only contains topics that are available in the bag
+  // If a topic is requested that is not avaiable, it does not show up in the info.
+  std::vector<const rosbag::ConnectionInfo*> connection_infos =
+      bag_view_->getConnections();
+  if (topics.size() != connection_infos.size())
+  {
+    LOG(ERROR) << "Successfully connected to " << connection_infos.size() << " topics:";
+    for (const rosbag::ConnectionInfo* info : connection_infos)
+    {
+      LOG(ERROR) << "*) " << info->topic;
+    }
+    LOG(ERROR) << "Requested " << topics.size() << " topics:";
+    for (const std::string topic : topics)
+    {
+      LOG(ERROR) << "*) " << topic;
+    }
+    LOG(FATAL) << "Not all requested topics founds in bagfile. "
+               << "Is topic_cam0, topic_imu0, etc. set correctly? "
+               << "Maybe removing/adding a slash as prefix solves the problem.";
+  }
+}
+
+size_t DataProviderRosbag::cameraCount() const
+{
+  return img_topic_camidx_map_.size();
+}
+
+size_t DataProviderRosbag::imuCount() const
+{
+  if (uses_split_messages_)
+  {
+    return accel_topic_imuidx_map_.size();
+  }
+
+  return imu_topic_imuidx_map_.size();
+}
+
+bool DataProviderRosbag::spinOnce()
+{
+  if (bag_view_it_ != bag_view_->end())
+  {
+    const rosbag::MessageInstance m = *bag_view_it_;
+
+    // Camera Messages:
+    const sensor_msgs::ImageConstPtr m_img = m.instantiate<sensor_msgs::Image>();
+    if (m_img && camera_callback_)
+    {
+      if (!cameraSpin(m_img, m))
+      {
+        return false;
+      }
+    }
+    else if (m_img && !camera_callback_)
+    {
+      LOG_FIRST_N(WARNING, 1) << "No camera callback registered but measurements available";
+    }
+
+    // Imu Messages:
+    const sensor_msgs::ImuConstPtr m_imu = m.instantiate<sensor_msgs::Imu>();
+    if (m_imu && imu_callback_)
+    {
+      if (!imuSpin(m_imu, m))
+      {
+        return false;
+      }
+    }
+    else if (m_imu && !imu_callback_)
+    {
+      LOG_FIRST_N(WARNING, 1) << "No IMU callback registered but measurements available";
+    }
+
+    // Accelerometer Messages:
+    const ze_ros_msg::bmx055_accConstPtr m_acc =
+        m.instantiate<ze_ros_msg::bmx055_acc>();
+    if (m_acc && accel_callback_)
+    {
+      if (!accelSpin(m_acc, m))
+      {
+        return false;
+      }
+    }
+    else if (m_acc && !accel_callback_)
+    {
+      LOG_FIRST_N(WARNING, 1) << "No Accelerometer callback registered but"
+                              << "measurements available";
+    }
+
+    // Gyroscope Messages:
+    const ze_ros_msg::bmx055_gyrConstPtr m_gyr =
+        m.instantiate<ze_ros_msg::bmx055_gyr>();
+    if (m_gyr && gyro_callback_)
+    {
+      if(!gyroSpin(m_gyr, m))
+      {
+        return false;
+      }
+    }
+    else if (m_gyr && !gyro_callback_)
+    {
+      LOG_FIRST_N(WARNING, 1) << "No Gyroscope callback registered but"
+                              << "measurements available";
+    }
+
+    ++bag_view_it_;
+    return true;
+  }
+  return false;
+}
+
+bool DataProviderRosbag::cameraSpin(sensor_msgs::ImageConstPtr m_img,
+                                    const rosbag::MessageInstance& m)
+{
+  auto it = img_topic_camidx_map_.find(m.getTopic());
+  if (it != img_topic_camidx_map_.end())
+  {
+    ++n_processed_images_;
+    if (FLAGS_data_source_stop_after_n_frames > 0
+        && n_processed_images_ > FLAGS_data_source_stop_after_n_frames)
+    {
+      LOG(WARNING) << "Data source has reached max number of desired frames.";
+      running_ = false;
+      return false;
+    }
+
+    ze::ImageBase::Ptr img = toImageCpu(*m_img);
+    camera_callback_(m_img->header.stamp.toNSec(), img, it->second);
+  }
+  else
+  {
+    LOG_FIRST_N(WARNING, 1) << "Topic in bag that is not subscribed: " << m.getTopic();
+  }
+
+  return true;
+}
+
+bool DataProviderRosbag::imuSpin(sensor_msgs::ImuConstPtr m_imu,
+                                 const rosbag::MessageInstance& m)
+{
+  auto it = imu_topic_imuidx_map_.find(m.getTopic());
+  if (it != imu_topic_imuidx_map_.end())
+  {
+    const Vector3 gyr(
+          m_imu->angular_velocity.x,
+          m_imu->angular_velocity.y,
+          m_imu->angular_velocity.z);
+    const Vector3 acc(
+          m_imu->linear_acceleration.x,
+          m_imu->linear_acceleration.y,
+          m_imu->linear_acceleration.z);
+    int64_t stamp = m_imu->header.stamp.toNSec();
+    CHECK_GT(stamp, last_imu_stamp_);
+    imu_callback_(stamp, acc, gyr, it->second);
+    last_imu_stamp_ = stamp;
+  }
+  else
+  {
+    LOG_FIRST_N(WARNING, 1) << "Topic in bag that is not subscribed: " << m.getTopic();
+  }
+
+  return true;
+}
+
+bool DataProviderRosbag::accelSpin(ze_ros_msg::bmx055_accConstPtr m_acc,
+                                   const rosbag::MessageInstance& m)
+{
+  auto it = accel_topic_imuidx_map_.find(m.getTopic());
+  if (it != accel_topic_imuidx_map_.end())
+  {
+    const Vector3 acc(
+          m_acc->linear_acceleration.x,
+          m_acc->linear_acceleration.y,
+          m_acc->linear_acceleration.z);
+    int64_t stamp = m_acc->header.stamp.toNSec();
+    CHECK_GT(stamp, last_acc_stamp_);
+    accel_callback_(stamp, acc, it->second);
+    last_acc_stamp_ = stamp;
+  }
+  else
+  {
+    LOG_FIRST_N(WARNING, 1) << "Topic in bag that is not subscribed: " << m.getTopic();
+  }
+
+  return true;
+}
+
+bool DataProviderRosbag::gyroSpin(ze_ros_msg::bmx055_gyrConstPtr m_gyr,
+                                  const rosbag::MessageInstance& m)
+{
+  auto it = gyro_topic_imuidx_map_.find(m.getTopic());
+  if (it != gyro_topic_imuidx_map_.end())
+  {
+    const Vector3 gyr(
+          m_gyr->angular_velocity.x,
+          m_gyr->angular_velocity.y,
+          m_gyr->angular_velocity.z);
+    int64_t stamp = m_gyr->header.stamp.toNSec();
+    CHECK_GT(stamp, last_gyr_stamp_);
+    gyro_callback_(stamp, gyr, it->second);
+    last_gyr_stamp_ = stamp;
+  }
+  else
+  {
+    LOG_FIRST_N(WARNING, 1) << "Topic in bag that is not subscribed: " << m.getTopic();
+  }
+
+  return true;
+}
+
+bool DataProviderRosbag::ok() const
+{
+  if (!running_)
+  {
+    VLOG(1) << "Data Provider was paused/terminated.";
+    return false;
+  }
+  if (bag_view_it_ == bag_view_->end())
+  {
+    VLOG(1) << "All data processed.";
+    return false;
+  }
+  return true;
+}
+
+size_t DataProviderRosbag::size() const
+{
+  CHECK(bag_view_);
+  return bag_view_->size();
+}
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_data_provider/src/data_provider_rostopic.cpp b/RWR/src/ze_oss/ze_data_provider/src/data_provider_rostopic.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..8ab0492b483129caa035027497eafd523dd0ce89
--- /dev/null
+++ b/RWR/src/ze_oss/ze_data_provider/src/data_provider_rostopic.cpp
@@ -0,0 +1,226 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include <ze/data_provider/data_provider_rostopic.hpp>
+
+#include <imp/bridge/ros/ros_bridge.hpp>
+#include <ze/common/logging.hpp>
+#include <ze/common/time_conversions.hpp>
+#include <ze/common/string_utils.hpp>
+#include <ze/common/path_utils.hpp>
+
+namespace ze {
+
+DataProviderRostopic::DataProviderRostopic(
+    const std::map<std::string, size_t>& imu_topic_imuidx_map,
+    const std::map<std::string, size_t>& img_topic_camidx_map,
+    uint32_t polling_rate,
+    uint32_t img_queue_size,
+    uint32_t imu_queue_size)
+  : DataProviderBase(DataProviderType::Rostopic)
+  , img_transport_(nh_)
+  , polling_rate_(polling_rate)
+  , uses_split_messages_(false)
+{
+  VLOG(1) << "Create Dataprovider for synchronized Gyro/Accel";
+
+  nh_.setCallbackQueue(&queue_);
+  img_transport_ = image_transport::ImageTransport(nh_);
+
+  // Subscribe to camera:
+  for (auto it : img_topic_camidx_map)
+  {
+    auto cb = std::bind(&DataProviderRostopic::imgCallback,
+                        this, std::placeholders::_1, it.second);
+    sub_cams_.emplace_back(img_transport_.subscribe(it.first, img_queue_size, cb));
+    VLOG(1) << "Subscribed to camera topic " << it.first;
+  }
+
+  for (auto it : imu_topic_imuidx_map)
+  {
+    auto cb = std::bind(&DataProviderRostopic::imuCallback,
+                          this, std::placeholders::_1, it.second);
+    sub_imus_.emplace_back(
+          nh_.subscribe<sensor_msgs::Imu>(it.first, imu_queue_size, cb));
+    VLOG(1) << "Subscribed to imu topic " << it.first;
+  }
+}
+
+DataProviderRostopic::DataProviderRostopic(
+    const std::map<std::string, size_t>& accel_topic_imuidx_map,
+    const std::map<std::string, size_t>& gyro_topic_imuidx_map,
+    const std::map<std::string, size_t>& img_topic_camidx_map,
+    uint32_t polling_rate,
+    uint32_t img_queue_size,
+    uint32_t imu_queue_size)
+  : DataProviderBase(DataProviderType::Rostopic)
+  , img_transport_(nh_)
+  , polling_rate_(polling_rate)
+  , uses_split_messages_(true)
+{
+  VLOG(1) << "Create Dataprovider for UN-synchronized Gyro/Accel";
+
+  nh_.setCallbackQueue(&queue_);
+  img_transport_ = image_transport::ImageTransport(nh_);
+
+  // Subscribe to camera:
+  for (auto it : img_topic_camidx_map)
+  {
+    auto cb = std::bind(&DataProviderRostopic::imgCallback,
+                        this, std::placeholders::_1, it.second);
+    sub_cams_.emplace_back(img_transport_.subscribe(it.first, img_queue_size, cb));
+    VLOG(1) << "Subscribed to camera topic " << it.first;
+  }
+
+  for (auto it : accel_topic_imuidx_map)
+  {
+    auto cb = std::bind(&DataProviderRostopic::accelCallback,
+                          this, std::placeholders::_1, it.second);
+    sub_accels_.emplace_back(
+          nh_.subscribe<ze_ros_msg::bmx055_acc>(it.first, imu_queue_size, cb));
+    VLOG(1) << "Subscribed to accel topic " << it.first;
+  }
+
+  for (auto it : gyro_topic_imuidx_map)
+  {
+    auto cb = std::bind(&DataProviderRostopic::gyroCallback,
+                          this, std::placeholders::_1, it.second);
+    sub_gyros_.emplace_back(
+          nh_.subscribe<ze_ros_msg::bmx055_gyr>(it.first, imu_queue_size, cb));
+    VLOG(1) << "Subscribed to gyro topic " << it.first;
+  }
+}
+
+size_t DataProviderRostopic::cameraCount() const
+{
+  return sub_cams_.size();
+}
+
+size_t DataProviderRostopic::imuCount() const
+{
+  if (uses_split_messages_)
+  {
+    return sub_accels_.size();
+  }
+
+  return sub_imus_.size();
+}
+
+bool DataProviderRostopic::spinOnce()
+{
+  queue_.callAvailable(ros::WallDuration(ros::Rate(polling_rate_)));
+  return ok();
+}
+
+bool DataProviderRostopic::ok() const
+{
+  if (!running_)
+  {
+    VLOG(1) << "Data Provider was paused/terminated.";
+    return false;
+  }
+  if (!ros::ok())
+  {
+    VLOG(1) << "ROS not OK.";
+    return false;
+  }
+  return true;
+}
+
+void DataProviderRostopic::imgCallback(
+    const sensor_msgs::ImageConstPtr& m_img,
+    uint32_t cam_idx)
+{
+  if (!camera_callback_)
+  {
+    LOG_FIRST_N(WARNING, 1) << "No Image callback registered but measurements available";
+    return;
+  }
+
+  ze::ImageBase::Ptr img = toImageCpu(*m_img);
+  camera_callback_(m_img->header.stamp.toNSec(), img, cam_idx);
+}
+
+void DataProviderRostopic::imuCallback(
+    const sensor_msgs::ImuConstPtr& m_imu,
+    uint32_t imu_idx)
+{
+  if (!imu_callback_)
+  {
+    LOG_FIRST_N(WARNING, 1) << "No IMU callback registered but measurements available";
+    return;
+  }
+
+  const Vector3 gyr(
+        m_imu->angular_velocity.x,
+        m_imu->angular_velocity.y,
+        m_imu->angular_velocity.z);
+  const Vector3 acc(
+        m_imu->linear_acceleration.x,
+        m_imu->linear_acceleration.y,
+        m_imu->linear_acceleration.z);
+  int64_t stamp = m_imu->header.stamp.toNSec();
+  imu_callback_(stamp, acc, gyr, imu_idx);
+}
+
+void DataProviderRostopic::accelCallback(
+    const ze_ros_msg::bmx055_accConstPtr& m_acc,
+    uint32_t imu_idx)
+{
+  if (!accel_callback_)
+  {
+    LOG_FIRST_N(WARNING, 1) << "No Accel callback registered but measurements available";
+    return;
+  }
+
+  const Vector3 acc(
+        m_acc->linear_acceleration.x,
+        m_acc->linear_acceleration.y,
+        m_acc->linear_acceleration.z);
+  int64_t stamp = m_acc->header.stamp.toNSec();
+  accel_callback_(stamp, acc, imu_idx);
+}
+
+void DataProviderRostopic::gyroCallback(
+    const ze_ros_msg::bmx055_gyrConstPtr& m_gyr,
+    uint32_t imu_idx)
+{
+  if (!gyro_callback_)
+  {
+    LOG_FIRST_N(WARNING, 1) << "No Gyro callback registered but measurements available";
+    return;
+  }
+
+  const Vector3 gyr(
+        m_gyr->angular_velocity.x,
+        m_gyr->angular_velocity.y,
+        m_gyr->angular_velocity.z);
+
+  int64_t stamp = m_gyr->header.stamp.toNSec();
+  gyro_callback_(stamp, gyr, imu_idx);
+}
+
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_data_provider/test/test_camera_imu_synchronizer.cpp b/RWR/src/ze_oss/ze_data_provider/test/test_camera_imu_synchronizer.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..82267c69693569a8d5748b8c8a7feb77c27b29a4
--- /dev/null
+++ b/RWR/src/ze_oss/ze_data_provider/test/test_camera_imu_synchronizer.cpp
@@ -0,0 +1,270 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include <string>
+#include <iostream>
+
+#include <imp/core/image_base.hpp>
+#include <imp/core/image_raw.hpp>
+#include <imp/bridge/opencv/image_cv.hpp>
+#include <ze/common/test_entrypoint.hpp>
+#include <ze/common/test_utils.hpp>
+#include <ze/data_provider/data_provider_csv.hpp>
+#include <ze/data_provider/data_provider_rosbag.hpp>
+#include <ze/data_provider/camera_imu_synchronizer.hpp>
+
+namespace ze {
+// a dummy data provider
+class DataProviderDummy : public DataProviderBase
+{
+public:
+  DataProviderDummy(): DataProviderBase(DataProviderType::Rosbag) {}
+  void spin() {}
+  virtual bool spinOnce() { return true; }
+  virtual bool ok() const { return true; }
+  virtual size_t imuCount() const
+  {
+    return imu_count_;
+  };
+  virtual size_t cameraCount() const
+  {
+    return camera_count_;
+  }
+  size_t imu_count_;
+  size_t camera_count_;
+};
+}
+
+TEST(CameraImuSynchronizerTest, testCsv)
+{
+  using namespace ze;
+
+  std::string data_dir = getTestDataDir("csv_dataset");
+  EXPECT_FALSE(data_dir.empty());
+
+  DataProviderCsv dp(data_dir+"/data", {{"imu0", 0}}, {{"cam0", 0}});
+  CameraImuSynchronizer sync(dp);
+  sync.registerCameraImuCallback(
+        [&](const StampedImages& images,
+            const ImuStampsVector& imu_timestamps,
+            const ImuAccGyrVector& imu_measurements)
+  {
+    static int64_t last_img_stamp = 1403636579758555392;
+    VLOG(1) << "Image stamp = " << images[0].first << ", "
+            << "IMU-min = " << imu_timestamps[0](0) << ", "
+            << "IMU-max = " << imu_timestamps[0](imu_timestamps.size()-1) << "\n";
+    EXPECT_EQ(last_img_stamp,  imu_timestamps[0](0));
+    EXPECT_EQ(images[0].first, imu_timestamps[0](imu_timestamps[0].size()-1));
+    EXPECT_EQ(static_cast<int>(imu_timestamps[0].size()), imu_measurements[0].cols());
+    last_img_stamp = images[0].first;
+  });
+
+  dp.spin();
+}
+
+TEST(CameraImuSynchronizerTest, testCameraOnlyPublishesFullFrames)
+{
+  using namespace ze;
+  // fake data provider with two cameras
+  DataProviderDummy data_provider;
+  data_provider.camera_count_ = 2;
+  data_provider.imu_count_ = 0;
+
+  CameraImuSynchronizer sync(data_provider);
+
+  size_t measurements = 0u;
+  sync.registerCameraImuCallback(
+        [&](const StampedImages& images,
+            const ImuStampsVector& imu_timestamps,
+            const ImuAccGyrVector& imu_measurements)
+        {
+          ++measurements;
+          EXPECT_EQ(2, images.size());
+          EXPECT_EQ(0, imu_timestamps.size());
+          EXPECT_EQ(0, imu_measurements.size());
+        }
+  );
+
+  // trigger callbacks twice for images
+  int64_t stamp = 1403636579763555584;
+
+  auto img1 = std::make_shared<ImageRaw8uC1>(1, 1);
+  auto img2 = std::make_shared<ImageRaw8uC1>(1, 1);
+
+  sync.addImgData(stamp, img1, 0);
+  sync.addImgData(stamp, img2, 1);
+
+  // cam imu callback only called once
+  EXPECT_EQ(1, measurements);
+}
+
+TEST(CameraImuSynchronizerTest, testCameraOnlyPublishesWithImu)
+{
+  using namespace ze;
+  // fake data provider with two cameras
+  DataProviderDummy data_provider;
+  data_provider.camera_count_ = 1;
+  data_provider.imu_count_ = 1;
+
+  CameraImuSynchronizer sync(data_provider);
+
+  size_t measurements = 0u;
+  sync.registerCameraImuCallback(
+        [&](const StampedImages& images,
+            const ImuStampsVector& imu_timestamps,
+            const ImuAccGyrVector& imu_measurements)
+        {
+          ++measurements;
+          EXPECT_EQ(1, images.size());
+          EXPECT_EQ(1, imu_timestamps.size());
+          EXPECT_EQ(1, imu_measurements.size());
+        }
+  );
+
+  int64_t stamp1 = 1403636579763555583;
+  int64_t stamp2 = 1403636579763555584;
+  int64_t stamp3 = 1403636579763555585;
+
+  auto img1 = std::make_shared<ImageRaw8uC1>(1, 1);
+
+  sync.addImuData(stamp1, Vector3(), Vector3(), 0);
+  EXPECT_EQ(0, measurements);
+
+  sync.addImgData(stamp2, img1, 0);
+  EXPECT_EQ(0, measurements);
+
+  sync.addImuData(stamp3, Vector3(), Vector3(), 0);
+  EXPECT_EQ(1, measurements);
+}
+
+TEST(CameraImuSynchronizerTest, testImagesAreDiscardedIfFramesInconsistent)
+{
+  using namespace ze;
+  // fake data provider with two cameras
+  DataProviderDummy data_provider;
+  data_provider.camera_count_ = 2;
+  data_provider.imu_count_ = 0;
+
+  CameraImuSynchronizer sync(data_provider);
+
+  size_t measurements = 0u;
+  sync.registerCameraImuCallback(
+        [&](const StampedImages& images,
+            const ImuStampsVector& imu_timestamps,
+            const ImuAccGyrVector& imu_measurements)
+        {
+          ++measurements;
+          EXPECT_EQ(2, images.size());
+          EXPECT_EQ(0, imu_timestamps.size());
+          EXPECT_EQ(0, imu_measurements.size());
+        }
+  );
+
+  // trigger callbacks twice for images
+  int64_t stamp1 = 1403636579763555584;
+  int64_t stamp2 = 1403636579863555584;
+
+  auto img1 = std::make_shared<ImageRaw8uC1>(1, 1);
+  auto img2 = std::make_shared<ImageRaw8uC1>(1, 1);
+
+  sync.addImgData(stamp1, img1, 0);
+  sync.addImgData(stamp2, img2, 1);
+
+  // cam imu callback only called once
+  EXPECT_EQ(0, measurements);
+
+  // next image of cam0 that matches timestamp will trigger callback
+  sync.addImgData(stamp2, img1, 0);
+
+  EXPECT_EQ(1, measurements);
+}
+
+TEST(CameraImuSynchronizerTest, testImagesAreDiscardedIfNoImuBefore)
+{
+  using namespace ze;
+  // fake data provider with two cameras
+  DataProviderDummy data_provider;
+  data_provider.camera_count_ = 1;
+  data_provider.imu_count_ = 1;
+
+  CameraImuSynchronizer sync(data_provider);
+
+  size_t measurements = 0u;
+  sync.registerCameraImuCallback(
+        [&](const StampedImages& images,
+            const ImuStampsVector& imu_timestamps,
+            const ImuAccGyrVector& imu_measurements)
+        {
+          ++measurements;
+        }
+  );
+
+  int64_t stamp1 = 1403636579763555584;
+  int64_t stamp2 = 1403636579763555584;  // same as stamp1
+  int64_t stamp3 = 1403636579763555585;  // after stamp1
+
+  auto img1 = std::make_shared<ImageRaw8uC1>(1, 1);
+
+  sync.addImgData(stamp1, img1, 0);
+  sync.addImuData(stamp2, Vector3(), Vector3(), 0);
+  sync.addImuData(stamp3, Vector3(), Vector3(), 0);
+
+  EXPECT_EQ(0, measurements);
+}
+
+TEST(CameraImuSynchronizerTest, testImagesAreDiscardedIfNoImuAfter)
+{
+  using namespace ze;
+  // fake data provider with two cameras
+  DataProviderDummy data_provider;
+  data_provider.camera_count_ = 1;
+  data_provider.imu_count_ = 1;
+
+  CameraImuSynchronizer sync(data_provider);
+
+  size_t measurements = 0u;
+  sync.registerCameraImuCallback(
+        [&](const StampedImages& images,
+            const ImuStampsVector& imu_timestamps,
+            const ImuAccGyrVector& imu_measurements)
+        {
+          ++measurements;
+        }
+  );
+
+  int64_t stamp1 = 1403636579763555584;
+  int64_t stamp2 = 1403636579763555584;  // same as stamp1
+  int64_t stamp3 = 1403636579763555583;  // before stamp1
+
+  auto img1 = std::make_shared<ImageRaw8uC1>(1, 1);
+
+  sync.addImuData(stamp3, Vector3(), Vector3(), 0);
+  sync.addImgData(stamp1, img1, 0);
+  sync.addImuData(stamp2, Vector3(), Vector3(), 0);
+
+  EXPECT_EQ(0, measurements);
+}
+
+ZE_UNITTEST_ENTRYPOINT
diff --git a/RWR/src/ze_oss/ze_data_provider/test/test_camera_imu_synchronizer_unsync.cpp b/RWR/src/ze_oss/ze_data_provider/test/test_camera_imu_synchronizer_unsync.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..71bdb3b532af43a5bd9dd9f486c38ede47866dce
--- /dev/null
+++ b/RWR/src/ze_oss/ze_data_provider/test/test_camera_imu_synchronizer_unsync.cpp
@@ -0,0 +1,116 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include <string>
+#include <iostream>
+
+#include <imp/core/image_base.hpp>
+#include <imp/core/image_raw.hpp>
+#include <imp/bridge/opencv/image_cv.hpp>
+#include <ze/common/test_entrypoint.hpp>
+#include <ze/common/test_utils.hpp>
+#include <ze/data_provider/data_provider_csv.hpp>
+#include <ze/data_provider/data_provider_rosbag.hpp>
+#include <ze/data_provider/camera_imu_synchronizer_unsync.hpp>
+#include <ze/imu/imu_model.hpp>
+
+namespace ze {
+// a dummy data provider
+class DataProviderDummy : public DataProviderBase
+{
+public:
+  DataProviderDummy(): DataProviderBase(DataProviderType::Rosbag) {}
+  void spin() {}
+  virtual bool spinOnce() { return true; }
+  virtual bool ok() const { return true; }
+  virtual size_t imuCount() const
+  {
+    return imu_count_;
+  };
+  virtual size_t cameraCount() const
+  {
+    return camera_count_;
+  }
+  size_t imu_count_;
+  size_t camera_count_;
+};
+}
+
+TEST(CameraImuSynchronizerUnsyncTest, testFunctionality)
+{
+  using namespace ze;
+  // fake data provider with two cameras
+  DataProviderDummy data_provider;
+  data_provider.camera_count_ = 1;
+  data_provider.imu_count_ = 1;
+
+  // get an imu model
+  std::shared_ptr<ImuIntrinsicModelCalibrated> intrinsics =
+      std::make_shared<ImuIntrinsicModelCalibrated>();
+  std::shared_ptr<ImuNoiseNone> noise = std::make_shared<ImuNoiseNone>();
+
+  AccelerometerModel::Ptr a_model =
+      std::make_shared<AccelerometerModel>(intrinsics, noise);
+  GyroscopeModel::Ptr g_model =
+      std::make_shared<GyroscopeModel>(intrinsics, noise);
+
+  ImuModel model(a_model, g_model);
+
+  CameraImuSynchronizerUnsync sync(data_provider,
+                                  { std::make_shared<ImuModel>(a_model, g_model) });
+
+  size_t measurements = 0u;
+  sync.registerCameraImuCallback(
+        [&](const StampedImages& images,
+            const ImuStampsVector& imu_timestamps,
+            const ImuAccGyrVector& imu_measurements)
+        {
+          ++measurements;
+          EXPECT_EQ(1, images.size());
+          EXPECT_EQ(1, imu_timestamps.size());
+          EXPECT_EQ(1, imu_measurements.size());
+        }
+  );
+
+  int64_t stamp1 = 1403636579763555580;
+  int64_t stamp2 = 1403636579763555590;
+  int64_t stamp3 = 1403636579763555600;
+
+  auto img1 = std::make_shared<ImageRaw8uC1>(1, 1);
+
+  sync.addGyroData(stamp1, Vector3(), 0);
+  sync.addAccelData(stamp1 + 1, Vector3(), 0);
+  EXPECT_EQ(0, measurements);
+
+  sync.addImgData(stamp2, img1, 0);
+  EXPECT_EQ(0, measurements);
+
+  sync.addGyroData(stamp3, Vector3(), 0);
+  EXPECT_EQ(0, measurements);
+  sync.addAccelData(stamp3 + 1, Vector3(), 0);
+  EXPECT_EQ(1, measurements);
+}
+
+ZE_UNITTEST_ENTRYPOINT
diff --git a/RWR/src/ze_oss/ze_data_provider/test/test_data_provider.cpp b/RWR/src/ze_oss/ze_data_provider/test/test_data_provider.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..58004c1c595f642674b813f2eb2e421fc9fcaee6
--- /dev/null
+++ b/RWR/src/ze_oss/ze_data_provider/test/test_data_provider.cpp
@@ -0,0 +1,159 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include <string>
+#include <iostream>
+
+#include <ze/common/test_entrypoint.hpp>
+#include <ze/common/test_utils.hpp>
+#include <ze/common/path_utils.hpp>
+#include <ze/data_provider/data_provider_csv.hpp>
+#include <ze/data_provider/data_provider_rosbag.hpp>
+#include <imp/core/image_base.hpp>
+
+TEST(DataProviderTests, testCsv)
+{
+  using namespace ze;
+
+  std::string data_dir = getTestDataDir("csv_dataset");
+  EXPECT_FALSE(data_dir.empty());
+
+  DataProviderCsv dp(joinPath(data_dir, "data"), {{"imu0", 0}}, {{"cam0", 0}});
+
+  size_t num_imu_measurements = 0u;
+  int64_t expect_imu_stamp = 1403636579758555392;
+  dp.registerImuCallback(
+        [&](int64_t stamp, const Vector3& /*acc*/, const Vector3& /*gyr*/, const uint32_t /*imu_idx*/)
+  {
+    ++num_imu_measurements;
+    if(stamp == expect_imu_stamp)
+    {
+      expect_imu_stamp = 0;
+    }
+  });
+
+  size_t num_cam_measurements = 0u;
+  int64_t expect_cam_stamp = 1403636579763555584;
+  dp.registerCameraCallback(
+        [&](int64_t stamp, const ImageBase::Ptr& /*img*/, uint32_t /*cam_idx*/)
+  {
+    ++num_cam_measurements;
+    if(stamp == expect_cam_stamp)
+    {
+      expect_cam_stamp = 0;
+    }
+  });
+
+  dp.spin();
+
+  EXPECT_EQ(expect_imu_stamp, 0);
+  EXPECT_EQ(expect_cam_stamp, 0);
+  EXPECT_EQ(num_cam_measurements, 5u);
+  EXPECT_EQ(num_imu_measurements, 69u);
+}
+
+TEST(DataProviderTests, testRosbag)
+{
+  using namespace ze;
+
+  std::string data_dir = getTestDataDir("rosbag_euroc_snippet");
+  std::string bag_filename = joinPath(data_dir, "dataset.bag");
+  ASSERT_TRUE(fileExists(bag_filename));
+
+  DataProviderRosbag dp(bag_filename, {{"/imu0", 0}}, { {"/cam0/image_raw", 0},
+                                                        {"/cam1/image_raw", 1} });
+
+  size_t num_imu_measurements = 0u;
+  dp.registerImuCallback(
+        [&](int64_t /*stamp*/, const Vector3& /*acc*/, const Vector3& /*gyr*/, const uint32_t imu_idx)
+  {
+    if(imu_idx == 0)
+    {
+      ++num_imu_measurements;
+    }
+  });
+
+  size_t num_cam0_measurements = 0u, num_cam1_measurements = 0u;
+  dp.registerCameraCallback(
+        [&](int64_t /*stamp*/, const ImageBase::Ptr& /*img*/, uint32_t cam_idx)
+  {
+    if(cam_idx == 0)
+    {
+      ++num_cam0_measurements;
+    }
+    if(cam_idx == 1)
+    {
+      ++num_cam1_measurements;
+    }
+  });
+
+  dp.spin();
+
+  EXPECT_EQ(num_cam0_measurements, 21u);
+  EXPECT_EQ(num_cam1_measurements, 20u);
+  EXPECT_EQ(num_imu_measurements, 210u);
+}
+
+TEST(DataProviderTests, testRosbagCamOnly)
+{
+  using namespace ze;
+
+  std::string data_dir = getTestDataDir("rosbag_euroc_snippet");
+  std::string bag_filename = joinPath(data_dir, "dataset.bag");
+  ASSERT_TRUE(fileExists(bag_filename));
+
+  DataProviderRosbag dp(bag_filename, {}, { {"/cam0/image_raw", 0},
+                                            {"/cam1/image_raw", 1} });
+
+  size_t num_imu_measurements = 0u;
+  dp.registerImuCallback(
+        [&](int64_t /*stamp*/, const Vector3& /*acc*/, const Vector3& /*gyr*/, const uint32_t /*imu_idx*/)
+  {
+    ++num_imu_measurements;
+  });
+
+  size_t num_cam0_measurements = 0u, num_cam1_measurements = 0u;
+  dp.registerCameraCallback(
+        [&](int64_t /*stamp*/, const ImageBase::Ptr& /*img*/, uint32_t cam_idx)
+  {
+    if(cam_idx == 0)
+    {
+      ++num_cam0_measurements;
+    }
+    if(cam_idx == 1)
+    {
+      ++num_cam1_measurements;
+    }
+  });
+
+  dp.spin();
+
+  EXPECT_EQ(num_cam0_measurements, 21u);
+  EXPECT_EQ(num_cam1_measurements, 20u);
+  EXPECT_EQ(num_imu_measurements, 0u);
+}
+
+
+ZE_UNITTEST_ENTRYPOINT
diff --git a/RWR/src/ze_oss/ze_geometry/CATKIN_IGNORE b/RWR/src/ze_oss/ze_geometry/CATKIN_IGNORE
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/RWR/src/ze_oss/ze_geometry/CMakeLists.txt b/RWR/src/ze_oss/ze_geometry/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..e034f6abdf8ec9f5ec11842010548e73bd108116
--- /dev/null
+++ b/RWR/src/ze_oss/ze_geometry/CMakeLists.txt
@@ -0,0 +1,83 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(ze_geometry)
+
+find_package(catkin_simple REQUIRED)
+catkin_simple(ALL_DEPS_REQUIRED)
+
+include(ze_setup)
+
+# quick fix to gcc bug / boost defining literals
+if("${CMAKE_CXX_COMPILER_ID}" STREQUAL "GNU")
+  #  gcc < 4.9 only
+  if (CMAKE_CXX_COMPILER_VERSION VERSION_LESS 4.9)
+    set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fext-numeric-literals")      
+  endif()
+endif()
+
+
+#############
+# LIBRARIES #
+#############
+set(HEADERS
+  include/ze/geometry/align_points.hpp
+  include/ze/geometry/align_poses.hpp
+  include/ze/geometry/clam.hpp
+  include/ze/geometry/epipolar_geometry.hpp
+  include/ze/geometry/line.hpp
+  include/ze/geometry/lsq_solver.hpp
+  include/ze/geometry/lsq_solver-inl.hpp
+  include/ze/geometry/lsq_state.hpp
+  include/ze/geometry/pose_optimizer.hpp
+  include/ze/geometry/pose_prior.hpp
+  include/ze/geometry/ransac_relative_pose.hpp
+  include/ze/geometry/robust_cost.hpp
+  include/ze/geometry/triangulation.hpp
+  )
+
+set(SOURCES
+  src/align_points.cpp
+  src/align_poses.cpp
+  src/clam.cpp
+  src/line.cpp
+  src/pose_optimizer.cpp
+  src/ransac_relative_pose.cpp
+  src/triangulation.cpp
+  )
+
+cs_add_library(${PROJECT_NAME} ${SOURCES} ${HEADERS})
+
+##########
+# GTESTS #
+##########
+catkin_add_gtest(test_align_points test/test_align_points.cpp)
+target_link_libraries(test_align_points ${PROJECT_NAME})
+
+catkin_add_gtest(test_align_poses test/test_align_poses.cpp)
+target_link_libraries(test_align_poses ${PROJECT_NAME})
+
+catkin_add_gtest(test_clam test/test_clam.cpp)
+target_link_libraries(test_clam ${PROJECT_NAME})
+
+catkin_add_gtest(test_line test/test_line.cpp)
+target_link_libraries(test_line ${PROJECT_NAME})
+
+catkin_add_gtest(test_lsq_state test/test_lsq_state.cpp)
+target_link_libraries(test_lsq_state ${PROJECT_NAME})
+
+catkin_add_gtest(test_pose_optimizer test/test_pose_optimizer.cpp)
+target_link_libraries(test_pose_optimizer ${PROJECT_NAME})
+
+catkin_add_gtest(test_ransac_relative_pose test/test_ransac_relative_pose.cpp)
+target_link_libraries(test_ransac_relative_pose ${PROJECT_NAME})
+
+catkin_add_gtest(test_robust_cost test/test_robust_cost.cpp)
+target_link_libraries(test_robust_cost ${PROJECT_NAME})
+
+catkin_add_gtest(test_triangulation test/test_triangulation.cpp)
+target_link_libraries(test_triangulation ${PROJECT_NAME})
+
+##########
+# EXPORT #
+##########
+cs_install()
+cs_export()
diff --git a/RWR/src/ze_oss/ze_geometry/doc/bibliography_lines.bib b/RWR/src/ze_oss/ze_geometry/doc/bibliography_lines.bib
new file mode 100644
index 0000000000000000000000000000000000000000..6cce9321ae85b172ad25f01dbdbe8c0db8beb51d
--- /dev/null
+++ b/RWR/src/ze_oss/ze_geometry/doc/bibliography_lines.bib
@@ -0,0 +1,31 @@
+% This file was created with JabRef 2.10b2.
+% Encoding: UTF-8
+
+
+@Article{guolarge,
+  Title                    = {Large-Scale Cooperative {3D} Visual-Inertial Mapping in a Manhattan World},
+  Author                   = {Guo, Chao X and Sartipi, Kourosh and DuToit, Ryan C and Georgiou, Georgios and Li, Ruipeng and O’Leary, John and Nerurkar, Esha D and Hesch, Joel A and Roumeliotis, Stergios I}
+}
+
+@article{holzmanndirect,
+  title={Direct Stereo Visual Odometry Based on Lines},
+  author={Holzmann, Thomas and Fraundorfer, Friedrich and Bischof, Horst}
+}
+
+@inproceedings{vacchetti2004combining,
+  title={Combining edge and texture information for real-time accurate 3d camera tracking},
+  author={Vacchetti, Luca and Lepetit, Vincent and Fua, Pascal},
+  booktitle={Mixed and Augmented Reality, 2004. ISMAR 2004. Third IEEE and ACM International Symposium on},
+  pages={48--56},
+  year={2004},
+  organization={IEEE}
+}
+
+@inproceedings{kottas2013efficient,
+  title={Efficient and consistent vision-aided inertial navigation using line observations},
+  author={Kottas, Dimitrios G and Roumeliotis, Stergios I},
+  booktitle={Robotics and Automation (ICRA), 2013 IEEE International Conference on},
+  pages={1540--1547},
+  year={2013},
+  organization={IEEE}
+}
diff --git a/RWR/src/ze_oss/ze_geometry/doc/line_parametrization.pdf b/RWR/src/ze_oss/ze_geometry/doc/line_parametrization.pdf
new file mode 100644
index 0000000000000000000000000000000000000000..bab9161faba13220aad337580e891be17659a5a5
Binary files /dev/null and b/RWR/src/ze_oss/ze_geometry/doc/line_parametrization.pdf differ
diff --git a/RWR/src/ze_oss/ze_geometry/doc/line_parametrization.tex b/RWR/src/ze_oss/ze_geometry/doc/line_parametrization.tex
new file mode 100644
index 0000000000000000000000000000000000000000..dfed8a38c9f9d150475f3997f2a6f36c4b71ee03
--- /dev/null
+++ b/RWR/src/ze_oss/ze_geometry/doc/line_parametrization.tex
@@ -0,0 +1,197 @@
+\documentclass[10pt,a4paper]{article}
+\usepackage[utf8]{inputenc}
+\usepackage{amsmath}
+\usepackage{amsfonts}
+\usepackage{amssymb}
+\usepackage{mathtools}
+\usepackage{tensor}
+\author{Andreas Forster}
+\numberwithin{equation}{section}
+\renewcommand{\vec}[1]{\ensuremath{\mathbf{#1}}}
+\newcommand{\vecs}[1]{\ensuremath{\boldsymbol{#1}}}
+\renewcommand{\deg}{\ensuremath{^\circ}}
+\newcommand{\norm}[1]{\ensuremath{\left|\left|#1\right|\right|}}
+\newcommand{\Exp}{\mathrm{Exp}}
+
+\renewcommand{\arraystretch}{1.5}
+%\setcounter{secnumdepth}{0}
+
+\begin{document}
+\section{Free Line Parametrization}
+A coordinate frame $L_i$ is defined with origin $\tensor[_W]{\vec{p}}{_L}$ on the line and with minimum distance $d_{L}$ to the frame origin. The $x$ axis is aligned with the line and the $z$ axis points away from the origin. The parametrization is as follows:
+\begin{equation}
+\vec{l} = \left[q_{WL}^T \quad d_L\right].
+\end{equation}
+The line anchor $\tensor[_W]{\vec{p}}{_L}$ can be calculated in the following way:
+\begin{equation}
+\tensor[_W]{\vec{p}}{_L} = q_{WL}\times\begin{bmatrix}
+0\\0\\d_L\end{bmatrix}
+\end{equation}
+And the line direction $\tensor[_W]{\vec{x}}{_L}$:
+\begin{equation}
+\tensor[_W]{\vec{x}}{_L} = q_{WL}\times\begin{bmatrix}1\\0\\0 \end{bmatrix}
+\end{equation}
+%
+\section{Measurement}
+Let $\vec{f}_1$ and $\vec{f}_2$ be the bearing vectors through the two endpoints of the detected line segment. $\tilde{\vec{n}} = \frac{\vec{f}_1\times\vec{f}_2}{\norm{\vec{f}_1\times\vec{f}_2}}$ is the normalized normal vector of the plane spanned by the two bearing vectors and defines our measurement.
+%
+\section{Error formulation}
+Given: Measurement $\tilde{\vec{n}}$ and line $\vec{l} = [q_{WL}^T\quad d_L]$.
+\subsection{Parallel constraint}
+The direction of the line is parallel to the plane defined with $\tilde{\vec{n}}$:
+\begin{equation}
+\varepsilon_1 = \tensor[_C]{\tilde{\vec{n}}}{^T}C_{CB}\hat{C}_{BW}\tensor[_W]{\vec{x}}{_L}.
+\end{equation}
+$\varepsilon_1$ is related to the angle $\delta\phi$ between plane and the line $\vec{l}$ in the following way:
+\begin{equation}
+\varepsilon_1 = \sin \delta\phi.
+\end{equation}
+\subsection{Distance constraint}
+The anchor point should be part of the measured plane. We define the second error term $\varepsilon_2$ as the sine of the angle between the bearing vectors through the anchor point and the projection of that point on the measured plane.  
+\begin{align}
+\varepsilon_2 &= \frac{\tensor[_C]{\tilde{\vec{n}}}{^T} C_{CB} \hat{C}_{BW} \left(\tensor[_W]{\hat{\vec{p}}}{_C} - \tensor[_W]{\vec{p}}{_L}\right)}{\norm{\tensor[_W]{\hat{\vec{p}}}{_C} - \tensor[_W]{\vec{p}}{_L}}}\\
+\tensor[_W]{\hat{\vec{p}}}{_C} &= -\left(\tensor[_W]{\hat{\vec{t}}}{_{CB}}+\tensor[_W]{\hat{\vec{t}}}{_{BW}}\right)\\ 
+&=-\left(\hat{C}_{WB} C_{BC} \tensor[_C]{\vec{t}}{_{CB}} + 
+\hat{C}_{WB} \tensor[_B]{\hat{\vec{t}}}{_{BW}}\right)\\
+&=-\hat{C}_{WB} \left(C_{BC} \tensor[_C]{\vec{t}}{_{CB}} + 
+\tensor[_B]{\hat{\vec{t}}}{_{BW}}\right)
+\end{align}
+\section{Jacobian w.r.t to $T_{BW}$}
+\begin{align}
+C_{BW} &\leftarrow C_{BW}\Exp(\vecs{\delta\phi})\\
+\tensor[_B]{\vec{t}}{_{BW}} &\leftarrow \tensor[_B]{\vec{t}}{_{BW}} + C_{BW}\vecs{\delta t}
+\end{align}
+\subsection{$\vecs{\varepsilon_1}$}
+\begin{align}
+\varepsilon_1 &= \tensor[_C]{\tilde{\vec{n}}}{^T}C_{CB} C_{BW}\Exp(\vecs{\delta\phi}) \tensor[_W]{\vec{x}}{_L}\\
+&\approx \tensor[_C]{\tilde{\vec{n}}}{^T}C_{CB} C_{BW}\tensor[_W]{\vec{x}}{_L} - \tensor[_C]{\tilde{\vec{n}}}{^T}C_{CB} C_{BW} \tensor[_W]{\vec{x}}{_L^\wedge}\vecs{\delta\phi}
+\end{align}
+\begin{align}
+\frac{\partial \varepsilon_1}{\partial\vecs{\delta\phi}} &= -\tensor[_C]{\tilde{\vec{n}}}{^T}C_{CB} C_{BW}\tensor[_W]{\vec{x}}{_L^\wedge}\\
+\frac{\partial \varepsilon_1}{\partial\vecs{\delta t}} &= \begin{bmatrix} 0&0&0 \end{bmatrix}
+\end{align}
+\subsection{$\vecs{\varepsilon_2}$}
+%
+%
+Let:
+\begin{equation}
+\vec{v} = \frac{\tensor[_W]{\hat{\vec{p}}}{_C} - \tensor[_W]{\vec{p}}{_L}}{\norm{\tensor[_W]{\hat{\vec{p}}}{_C} - \tensor[_W]{\vec{p}}{_L}}}
+\end{equation}
+Then:
+\begin{align}
+\varepsilon_2 &= \tensor[_C]{\tilde{\vec{n}}}{^T} C_{CB} C_{BW} \Exp(\vecs{\delta\phi}) \vec{v}\\
+&\approx \tensor[_C]{\tilde{\vec{n}}}{^T} C_{CB} C_{BW} \vec{v} - \tensor[_C]{\tilde{\vec{n}}}{^T} C_{CB} C_{BW} \vec{v}^\wedge \vecs{\delta\phi}
+\end{align}
+\begin{align}
+\frac{\partial\varepsilon_2}{\partial\vecs{\delta\phi}} &= \tensor[_C]{\tilde{\vec{n}}}{^T} C_{CB} C_{BW}\frac{\partial \vec{v}}{\partial\vecs{\delta\phi}} - \tensor[_C]{\tilde{\vec{n}}}{^T} C_{CB} C_{BW}\vec{v}^\wedge\\
+\frac{\partial\varepsilon_2}{\partial\vecs{\delta t}} &= \tensor[_C]{\tilde{\vec{n}}}{^T} C_{CB} C_{BW}\frac{\partial\vec{v}}{\partial \vecs{\delta t}}
+\end{align}
+\begin{align}
+\frac{\partial\vec{v}}{\partial\vecs{\delta\phi}} &= \frac{\partial \vec{v}}{\partial\tensor[_W]{\vec{p}}{_C}} \frac{\partial \tensor[_W]{\vec{p}}{_{C}}}{\partial \vecs{\delta\phi}}\\
+\frac{\partial\vec{v}}{\partial\vecs{\delta t}} &= \frac{\partial \vec{v}}{\partial\tensor[_W]{\vec{p}}{_C}} \frac{\partial \tensor[_W]{\vec{p}}{_C}}{\partial \vecs{\delta t}} 
+\end{align}
+Some derivatives:
+\begin{align}
+\frac{d\norm{\vec{w}}}{d\vec{w}} &= \frac{1}{\norm{\vec{w}}}\vec{w}^T\\
+\frac{d\frac{1}{\norm{\vec{w}}}}{d\vec{w}} &= -\frac{1}{\norm{\vec{w}}^3}\vec{w}^T\\
+\frac{d\frac{\vec{w}}{\norm{\vec{w}}}}{d\vec{w}} &= \frac{1}{\norm{\vec{w}}}I - \frac{1}{\norm{\vec{w}}^3}\vec{w}\vec{w}^T\notag\\
+&= \frac{1}{\norm{\vec{w}}}\left(I - \frac{\vec{w}}{\norm{\vec{w}}}\left(\frac{\vec{w}}{\norm{\vec{w}}}\right)^T\right)
+\end{align}
+It follows:
+\begin{align}
+\frac{\partial \vec{v}}{\partial\tensor[_W]{\vec{p}}{_C}} &= 
+%\frac{1}{\norm{\tensor[_W]{\vec{p}}{_C} - \tensor[_W]{\vec{p}}{_L}}}I
+%-\frac{1}{\norm{\tensor[_W]{\vec{p}}{_C} - \tensor[_W]{\vec{p}}{_L}}^3} (\tensor[_W]{\vec{p}}{_C} - \tensor[_W]{\vec{p}}{_L})(\tensor[_W]{\vec{p}}{_C} - \tensor[_W]{\vec{p}}{_L})^T\notag\\
+%&=\frac{1}{\norm{\tensor[_W]{\vec{p}}{_C} - \tensor[_W]{\vec{p}}{_L}}}
+%\left( 
+%I - \frac{(\tensor[_W]{\vec{p}}{_C} - \tensor[_W]{\vec{p}}{_L})(\tensor[_W]{\vec{p}}{_C} - \tensor[_W]{\vec{p}}{_L})^T}{\norm{\tensor[_W]{\vec{p}}{_C} - \tensor[_W]{\vec{p}}{_L}}^2}
+%\right)\\
+%&=
+\frac{1}{\norm{\tensor[_W]{\vec{p}}{_C} - \tensor[_W]{\vec{p}}{_L}}}
+\left(
+I - \vec{v}\vec{v}^T
+\right)
+\end{align}
+\subsubsection{Derivative of camera position w.r.t to $T_{BW}$}
+\begin{align}
+\tensor[_W]{\vec{p}}{_C} &= -\Exp(-\vecs{\delta\phi}) C_{WB} \left( C_{BC} \tensor[_C]{\vec{t}}{_{CB}} + \left(\tensor[_B]{\vec{t}}{_{BW}} + C_{BW} \vecs{\delta t}\right)\right)\\
+&\approx -\left[C_{WB} \left( C_{BC} \tensor[_C]{\vec{t}}{_{CB}} + \left(\tensor[_B]{\vec{t}}{_{BW}} + C_{BW} \vecs{\delta t}\right)\right)\right.\\
+&\left.\qquad-\vecs{\delta\phi}^\wedge C_{WB} \left( C_{BC} \tensor[_C]{\vec{t}}{_{CB}} + \left(\tensor[_B]{\vec{t}}{_{BW}} + C_{BW} \vecs{\delta t}\right)\right)\right]\\
+&\approx -C_{WB} \left( C_{BC} \tensor[_C]{\vec{t}}{_{CB}} + \left(\tensor[_B]{\vec{t}}{_{BW}} + C_{BW} \vecs{\delta t}\right)\right)\\
+&\qquad+\vecs{\delta\phi}^\wedge C_{WB} \left( C_{BC} \tensor[_C]{\vec{t}}{_{CB}} + \left(\tensor[_B]{\vec{t}}{_{BW}} + C_{BW} \vecs{\delta t}\right)\right)\\
+%
+&\approx -(\tensor[_W]{\vec{t}}{_{CB}} + \tensor[_W]{\vec{t}}{_{BW}} + \vecs{\delta t})
++ \vecs{\delta\phi}^\wedge\left(\tensor[_W]{\vec{t}}{_{CB}} + \tensor[_W]{\vec{t}}{_{BW}} + \vecs{\delta t}\right)\\
+&\approx -(\tensor[_W]{\vec{t}}{_{CB}} + \tensor[_W]{\vec{t}}{_{BW}} + \vecs{\delta t})
+- \left(\tensor[_W]{\vec{t}}{_{CB}} + \tensor[_W]{\vec{t}}{_{BW}} + \vecs{\delta t}\right)^\wedge\vecs{\delta\phi}\\
+%
+%&\approx-\left(\tensor[_W]{\vec{t}}{_{CB}}+\tensor[_W]{\vec{t}}{_{CB}^\wedge}\vecs{\delta\phi}\right) - \left(\tensor[_W]{\vec{t}}{_{BW}} + \vecs{\delta t} + \tensor[_W]{\vec{t}}{_{BW}^\wedge}\vecs{\delta\phi} +\vecs{\delta t}^\wedge\vecs{\delta\phi}\right)
+\end{align}
+\begin{align}
+\frac{\partial\tensor[_W]{\vec{p}}{_C}}{\partial\vecs{\delta\phi}} &= -(\tensor[_W]{\vec{t}}{_{CB}} + \tensor[_W]{\vec{t}}{_{BW}})^\wedge\\
+\frac{\partial\tensor[_W]{\vec{p}}{_C}}{\partial\vecs{\delta t}} &= -I
+\end{align}
+\subsubsection{Full jacobian}
+\begin{align}
+\frac{\partial\varepsilon_2}{\partial\vecs{\delta\phi}} &= \tensor[_C]{\tilde{\vec{n}}}{^T} C_{CB} C_{BW}\frac{\partial \vec{v}}{\partial\vecs{\delta\phi}} - \tensor[_C]{\tilde{\vec{n}}}{^T} C_{CB} C_{BW}\vec{v}^\wedge\\
+&= \tensor[_C]{\tilde{\vec{n}}}{^T} C_{CB} C_{BW}\frac{\partial \vec{v}}{\partial\tensor[_W]{\vec{p}}{_C}} 
+\frac{\partial \tensor[_W]{\vec{p}}{_{C}}}{\partial \vecs{\delta\phi}}
+ - \tensor[_C]{\tilde{\vec{n}}}{^T} C_{CB} C_{BW}\vec{v}^\wedge\\
+&= \tensor[_W]{\tilde{\vec{n}}}{^T}
+\frac{1}{\norm{\tensor[_W]{\vec{p}}{_C} - \tensor[_W]{\vec{p}}{_L}}}
+\left(
+I - \vec{v}\vec{v}^T
+\right)
+\frac{\partial \tensor[_W]{\vec{p}}{_{C}}}{\partial \vecs{\delta\phi}}
+ - \tensor[_W]{\tilde{\vec{n}}}{^T} \vec{v}^\wedge\\
+ &= \frac{1}{\norm{\tensor[_W]{\vec{p}}{_C} - \tensor[_W]{\vec{p}}{_L}}}\tensor[_W]{\tilde{\vec{n}}}{^T}
+\left(
+I - \vec{v}\vec{v}^T
+\right)
+\left(-(\tensor[_W]{\vec{t}}{_{CB}} + \tensor[_W]{\vec{t}}{_{BW}})^\wedge\right)
+ - \tensor[_W]{\tilde{\vec{n}}}{^T} \vec{v}^\wedge\\
+ &=\frac{-1}{\norm{\tensor[_W]{\vec{p}}{_C} - \tensor[_W]{\vec{p}}{_L}}}\tensor[_W]{\tilde{\vec{n}}}{^T}
+\left(
+I - \vec{v}\vec{v}^T
+\right)
+\left(\tensor[_W]{\vec{t}}{_{CB}} + \tensor[_W]{\vec{t}}{_{BW}}\right)^\wedge
+ - \tensor[_W]{\tilde{\vec{n}}}{^T} \vec{v}^\wedge\\
+&=-\tensor[_W]{\tilde{\vec{n}}}{^T}\left(\frac{1}{\norm{\tensor[_W]{\vec{p}}{_C} - \tensor[_W]{\vec{p}}{_L}}}
+\left(
+I - \vec{v}\vec{v}^T
+\right)
+\left(\tensor[_W]{\vec{t}}{_{CB}} + \tensor[_W]{\vec{t}}{_{BW}}\right)^\wedge + \vec{v}^\wedge\right)
+\end{align}
+\begin{align}
+\frac{\partial\varepsilon_2}{\partial\vecs{\delta t}} &= 
+\tensor[_C]{\tilde{\vec{n}}}{^T} C_{CB} C_{BW}\frac{\partial\vec{v}}{\partial \vecs{\delta t}}\\
+&= 
+\tensor[_W]{\tilde{\vec{n}}}{^T} 
+\frac{\partial \vec{v}}{\partial\tensor[_W]{\vec{p}}{_C}}
+\frac{\partial \tensor[_W]{\vec{p}}{_C}}{\partial \vecs{\delta t}}\\
+&=
+\tensor[_W]{\tilde{\vec{n}}}{^T} 
+\frac{1}{\norm{\tensor[_W]{\vec{p}}{_C} - \tensor[_W]{\vec{p}}{_L}}}
+\left(
+I - \vec{v}\vec{v}^T
+\right)
+\frac{\partial \tensor[_W]{\vec{p}}{_C}}{\partial \vecs{\delta t}}\\
+&=
+\frac{1}{\norm{\tensor[_W]{\vec{p}}{_C} - \tensor[_W]{\vec{p}}{_L}}}
+\tensor[_W]{\tilde{\vec{n}}}{^T}
+\left(
+I - \vec{v}\vec{v}^T
+\right)
+(-I)\\
+&=\frac{-1}{\norm{\tensor[_W]{\vec{p}}{_C} - \tensor[_W]{\vec{p}}{_L}}}
+\tensor[_W]{\tilde{\vec{n}}}{^T}
+\left(
+I - \vec{v}\vec{v}^T
+\right)
+\end{align}
+
+\nocite{*}
+\bibliographystyle{IEEEtran}
+\bibliography{bibliography_lines}
+
+\end{document}
\ No newline at end of file
diff --git a/RWR/src/ze_oss/ze_geometry/include/ze/geometry/align_points.hpp b/RWR/src/ze_oss/ze_geometry/include/ze/geometry/align_points.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..617b9900236e3e12bcf176d3c6cb5f3907a0f041
--- /dev/null
+++ b/RWR/src/ze_oss/ze_geometry/include/ze/geometry/align_points.hpp
@@ -0,0 +1,90 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#pragma once
+
+#include <ze/common/types.hpp>
+#include <ze/common/transformation.hpp>
+#include <ze/geometry/robust_cost.hpp>
+#include <ze/geometry/lsq_solver.hpp>
+
+namespace ze {
+
+//! Estimates relative transformation between two sets of associated points.
+//! Iterative least squares solution.
+class PointAligner : public LeastSquaresSolver<Transformation, PointAligner>
+{
+public:
+  using LeastSquaresSolver::HessianMatrix;
+  using LeastSquaresSolver::GradientVector;
+  using ScaleEstimator = UnitScaleEstimator<real_t>;
+  using WeightFunction = UnitWeightFunction<real_t>;
+
+  PointAligner(
+      const Positions& p_A,
+      const Positions& p_B);
+
+  double evaluateError(
+      const Transformation& T_A_B,
+      HessianMatrix* H,
+      GradientVector* g);
+
+private:
+  real_t measurement_sigma_;
+  const Positions& p_A_;
+  const Positions& p_B_;
+};
+
+inline Matrix36 dPointdistance_dRelpose(
+    const Transformation& T_A_B,
+    const Eigen::Ref<const Vector3>& p_A,
+    const Eigen::Ref<const Vector3>& p_B)
+{
+  Matrix3 R = T_A_B.getRotationMatrix();
+  Matrix36 J;
+  J.block<3,3>(0,0) = - R; // translation
+  J.block<3,3>(0,3) = R * skewSymmetric(p_B); // orientation
+  return J;
+}
+
+//! Compute LSQ alignment in SE3 (closed form solution by K. S. Arun et al.:
+//! Least-Squares Fitting of Two 3-D Point Sets, IEEE Trans. Pattern Anal.
+//! Mach. Intell., 9, NO. 5, SEPTEMBER 1987)
+//! @param pts_A A vector of N points in the 'A' reference system (3xN)
+//! @param pts_B A vector of N points in the 'B' reference system (3xN)
+//! @return T_B_A such that ||T_B_A * pts_A - pts_B|| is minimized
+Transformation alignSE3(
+    const Positions& pts_A, const Positions& pts_B);
+
+//! Compute LSQ alignment in Sim3 (close form solution by S. Umeyama:
+//! Least-Squares Estimation of Transformation Parameters Between Two Point
+//! Patterns, IEEE Trans. Pattern Anal. Mach. Intell., vol. 13, no. 4, 1991.)
+//! @param pts_A A vector of N points in the 'A' reference system (3xN)
+//! @param pts_B A vector of N points in the 'B' reference system (3xN)
+//! @return <s, T_B_A> such that ||s*T_B_A * pts_A - pts_B|| is minimized.
+std::pair<real_t, Transformation> alignSim3(
+    const Positions& pts_A, const Positions& pts_B);
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_geometry/include/ze/geometry/align_poses.hpp b/RWR/src/ze_oss/ze_geometry/include/ze/geometry/align_poses.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..7534ad514a1171bc90fb8bb6c51adeb09e5787e8
--- /dev/null
+++ b/RWR/src/ze_oss/ze_geometry/include/ze/geometry/align_poses.hpp
@@ -0,0 +1,78 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#pragma once
+
+#include <ze/common/types.hpp>
+#include <ze/common/transformation.hpp>
+#include <ze/geometry/robust_cost.hpp>
+#include <ze/geometry/lsq_solver.hpp>
+
+namespace ze {
+
+//! Estimates relative transformation between two sets of associated pints
+class PoseAligner :
+    public LeastSquaresSolver<Transformation, PoseAligner>
+{
+public:
+  using LeastSquaresSolver::HessianMatrix;
+  using LeastSquaresSolver::GradientVector;
+  using ScaleEstimator = UnitScaleEstimator<real_t>;
+  using WeightFunction = UnitWeightFunction<real_t>;
+
+  PoseAligner(
+      const TransformationVector& T_W_A,
+      const TransformationVector& T_W_B,
+      const real_t measurement_sigma_pos,
+      const real_t measurement_sigma_rot);
+
+  double evaluateError(
+      const Transformation& T_A_B,
+      HessianMatrix* H,
+      GradientVector *g);
+
+private:
+  const TransformationVector& T_W_A_;
+  const TransformationVector& T_W_B_;
+  real_t measurement_sigma_pos_;
+  real_t measurement_sigma_rot_;
+};
+
+inline Matrix6 dRelpose_dTransformation(
+    const Transformation& T_A0_B0,
+    const Transformation& T_Ai_A0,
+    const Transformation& T_B0_Bi)
+{
+  Quaternion R_error = T_Ai_A0.getRotation() * T_A0_B0.getRotation() * T_B0_Bi.getRotation();
+  Matrix3 R_Ai_B0 = T_Ai_A0.getRotationMatrix() * T_A0_B0.getRotationMatrix();
+  Matrix3 R_Bi_B0 = T_B0_Bi.getRotation().inverse().getRotationMatrix();
+  Matrix6 J = Z_6x6;
+  J.block<3,3>(0,0) = R_Ai_B0; // drt / dt
+  J.block<3,3>(0,3) = - R_Ai_B0 * skewSymmetric(T_B0_Bi.getPosition()); // drt / dR
+  J.block<3,3>(3,3) = logmapDerivativeSO3(R_error.log()) * R_Bi_B0; // drR / dR
+  return J;
+}
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_geometry/include/ze/geometry/clam.hpp b/RWR/src/ze_oss/ze_geometry/include/ze/geometry/clam.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..427ee2d4fe2927e1b7c4f7b99604eb470a395245
--- /dev/null
+++ b/RWR/src/ze_oss/ze_geometry/include/ze/geometry/clam.hpp
@@ -0,0 +1,150 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#pragma once
+
+#include <ze/common/transformation.hpp>
+#include <ze/cameras/camera_utils.hpp>
+#include <ze/cameras/camera_impl.hpp>
+#include <ze/cameras/camera_rig.hpp>
+#include <ze/geometry/robust_cost.hpp>
+#include <ze/geometry/lsq_solver.hpp>
+#include <ze/geometry/lsq_state.hpp>
+
+namespace ze {
+
+using ClamState = State<Transformation, VectorX>;
+
+struct ClamLandmarks
+{
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  Positions origin_Br; //! Br is body-frame of the reverence view.
+  Bearings f_Br;
+};
+
+//! Data required by Clam per frame.
+struct ClamFrameData
+{
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  //! @name Localization data
+  //! @{
+  //! Measurements: Bearing vectors.
+  Bearings f_C;
+
+  //! Landmark positions expressed in body frame of reference view.
+  //! Each column corresponds to a bearing measurement.
+  Positions p_Br;
+  //! @}
+
+  //! @name Mapping data
+  //! @{
+  //! Landmark measurements {ClamLandmarks-Index, Keypoint}.
+  std::vector<std::pair<uint32_t, Keypoint>> landmark_measurements;
+  //! @}
+
+  //! Extrinsic transformation between camera and body (i.e., IMU) frame.
+  Transformation T_C_B;
+};
+
+//! Coupled localization and mapping (Clam) as descripted in:
+//! Jonathan Balzer, Stefano Soatto, "CLAM: Coupled Localization and Mapping
+//! with Efficient Outlier Handling".
+class Clam : public LeastSquaresSolver<ClamState, Clam>
+{
+public:
+  using LeastSquaresSolver::HessianMatrix;
+  using LeastSquaresSolver::GradientVector;
+  using ScaleEstimator = MADScaleEstimator<real_t>;
+  using WeightFunction = TukeyWeightFunction<real_t>;
+
+  Clam(
+      const ClamLandmarks& landmarks,
+      const std::vector<ClamFrameData>& data,
+      const CameraRig& rig,
+      const Transformation& T_Bc_Br_prior,
+      const real_t prior_weight_pos,
+      const real_t prior_weight_rot);
+
+  real_t evaluateError(
+      const ClamState& state,
+      HessianMatrix* H,
+      GradientVector* g);
+
+private:
+  const ClamLandmarks& landmarks_;
+  const std::vector<ClamFrameData>& data_;
+  const CameraRig& rig_;
+  std::vector<real_t> measurement_sigma_localization_;
+  real_t measurement_sigma_mapping_ = 2.0;
+
+  // Prior:
+  const Transformation& T_Bc_Br_prior_; //!< Body-frame of (c)urrent and (r)eference view.
+  real_t prior_weight_pos_;
+  real_t prior_weight_rot_;
+};
+
+inline Vector2 reprojectionResidual(
+    const Eigen::Ref<const Bearing>& f_Br, //!< Bearing vector in reference body frame (Br).
+    const Eigen::Ref<const Position>& p_Br, //!< Reference camera center pos in Br.
+    const Camera& cam,
+    const Transformation& T_C_B,
+    const Transformation& T_Bc_Br,
+    const real_t inv_depth,
+    const Eigen::Ref<const Keypoint>& px_measured,
+    Matrix26* H1 = nullptr, //!< Jacobian dreprojectionResidual() / dT_Bc_Br
+    Matrix21* H2 = nullptr  //!< Jacobian dreprojectionResidual() / dinv_depth
+    )
+{
+  HomPosition p_Br_h;
+  p_Br_h.head<3>() = f_Br + p_Br * inv_depth;
+  p_Br_h(3) = inv_depth;
+  const Transformation T_C_Br = T_C_B * T_Bc_Br;
+  const HomPosition p_C_h = T_C_Br.transform4(p_Br_h);
+  const Keypoint px_est = cam.project(p_C_h.head<3>());
+  const Vector2 px_err = px_est - px_measured;
+
+  if (H1 || H2)
+  {
+    // H1 = dPx / dT_Bc_Br
+    Matrix23 J_proj = cam.dProject_dLandmark(p_C_h.head<3>());
+    if (inv_depth < 0.0)
+    {
+      J_proj *= -1.0;
+    }
+    Matrix36 G;
+    G.block<3,3>(0,0) = I_3x3 * inv_depth; // translation
+    G.block<3,3>(0,3) = -skewSymmetric(p_Br_h.head<3>()); // rotation
+    *H1 = J_proj * T_C_Br.getRotationMatrix() * G;
+
+    // H2 = dPx / dinv_depth
+    *H2 = J_proj * (T_C_Br.getRotation().rotate(p_Br) + T_C_Br.getPosition());
+  }
+
+  return px_err;
+}
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_geometry/include/ze/geometry/epipolar_geometry.hpp b/RWR/src/ze_oss/ze_geometry/include/ze/geometry/epipolar_geometry.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..98e8f74238eb0e59030c92faff86ab8e2eb7a8a7
--- /dev/null
+++ b/RWR/src/ze_oss/ze_geometry/include/ze/geometry/epipolar_geometry.hpp
@@ -0,0 +1,353 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#pragma once
+
+#include <ze/common/transformation.hpp>
+#include <ze/common/types.hpp>
+#include <ze/cameras/camera_rig.hpp>
+#include <ze/common/matrix.hpp>
+
+namespace ze {
+
+// ----------------------------------------------------------------------------
+//! Compute essential matrix from given camera transformation
+inline Matrix3 essentialMatrix(const Transformation& T)
+{
+  return skewSymmetric(T.getPosition()) * T.getRotationMatrix();
+}
+
+// ----------------------------------------------------------------------------
+//! Compute fundamental matrix from given camera transformation
+inline Matrix3 fundamentalMatrix(const Transformation& T_cam0_cam1,
+                                 const VectorX& projection_parameters0,
+                                 const VectorX& projection_parameters1)
+{
+  CHECK_EQ(projection_parameters0.size(), 4u);
+  CHECK_EQ(projection_parameters1.size(), 4u);
+
+  Matrix3 K0, K1;
+  K0 << projection_parameters0(0), 0, projection_parameters0(2),
+      0, projection_parameters0(1), projection_parameters0(3),
+      0, 0, 1;
+  K1 << projection_parameters1(0), 0, projection_parameters1(2),
+      0, projection_parameters1(1), projection_parameters1(3),
+      0, 0, 1;
+
+  return (K0.inverse().transpose() * essentialMatrix(T_cam0_cam1) * K1.inverse());
+}
+
+// ----------------------------------------------------------------------------
+//! Stereo Rectification
+
+using Rect = Roi<real_t, 2>;
+
+//! \brief Compute inner and outer rectangles.
+//!
+//! The inner rectangle is inscribed in the undistorted-rectified image.
+//! The outer rectangle is circumscribed about the undistorted-rectified image.
+//! \param img_size The size of the original (distorted) image.
+//! \param camera_parameters Vector of intrinsic parameters [fx, fy, cx, cy]
+//! for the original image.
+//! \param transformed_camera_parameters Vector of intrinsic parameters [fx', fy', cx', cy']
+//! for the undistorted-rectified image.
+//! \param distortion_coefficients Vector of distortion coefficients.
+//! \param H Rectifying homography matrix.
+//! \return A pair containing the rectangles inner (first) and outer (second).
+template<typename CameraModel,
+         typename DistortionModel>
+inline std::pair<Rect, Rect> innerAndOuterRectangles(
+    const Size2u& img_size,
+    const Vector4& camera_parameters,
+    const Vector4& transformed_camera_parameters,
+    const Vector4& distortion_coefficients,
+    const Matrix3& H)
+{
+  //! Sample the image in num_pts*num_pts locations
+  constexpr int num_pts{9}; //!< Number of sampling point for each image dimension
+  Matrix3X pts(3, num_pts*num_pts);     //!< Sampling points
+
+  for (int y = 0, k = 0; y < num_pts; ++y)
+  {
+    for (int x = 0; x < num_pts; ++x)
+    {
+      pts.col(k++) =
+          Vector3(
+            static_cast<real_t>(x) *
+            static_cast<real_t>(img_size[0]) /
+          static_cast<real_t>(num_pts -1),
+          static_cast<real_t>(y) *
+          static_cast<real_t>(img_size[1]) /
+          static_cast<real_t>(num_pts - 1),
+          1);
+    }
+  }
+
+  //! For every sampling point (u,v) compute the corresponding (u', v')
+  //! in the undistorted-rectified image:
+  //! x" = (u - cx)/fx
+  //! y" = (v - cy)/fy
+  //! (x', y') = undistort(distortion_coefficients, (x", y"))
+  //! [X Y W]^T = H*[x' y' 1]^T
+  //! x = X / W, y = Y / W
+  //! u' = x * fx' + cx'
+  //! v' = y * fy' + cy'
+  for (int c = 0; c < num_pts * num_pts; ++c)
+  {
+    CameraModel::backProject(camera_parameters.data(),
+                             pts.col(c).data());
+    DistortionModel::undistort(distortion_coefficients.data(),
+                               pts.col(c).data());
+    pts.col(c) = H * pts.col(c);
+    pts.col(c) /= pts.col(c)(2);
+    CameraModel::project(transformed_camera_parameters.data(),
+                         pts.col(c).data());
+  }
+
+  //! Rectangles are specified by two points.
+  real_t inner_x_left{-std::numeric_limits<real_t>::max()};
+  real_t inner_x_right{std::numeric_limits<real_t>::max()};
+  real_t inner_y_top{-std::numeric_limits<real_t>::max()};
+  real_t inner_y_bottom{std::numeric_limits<real_t>::max()};
+  real_t outer_x_left{std::numeric_limits<real_t>::max()};
+  real_t outer_x_right{-std::numeric_limits<real_t>::max()};
+  real_t outer_y_top{std::numeric_limits<real_t>::max()};
+  real_t outer_y_bottom{-std::numeric_limits<real_t>::max()};
+
+  //! Iterate over the sampling points and adjust the rectangle bounds.
+  for (int y = 0, k = 0; y < num_pts; y++)
+  {
+    for (int x = 0; x < num_pts; x++)
+    {
+      const Vector3& pt = pts.col(k++);
+      outer_x_left = std::min(outer_x_left, pt.x());
+      outer_x_right = std::max(outer_x_right, pt.x());
+      outer_y_top = std::min(outer_y_top, pt.y());
+      outer_y_bottom = std::max(outer_y_bottom, pt.y());
+
+      if (x == 0)
+      {
+        inner_x_left = std::max(inner_x_left, pt.x());
+      }
+      if (x == num_pts - 1)
+      {
+        inner_x_right = std::min(inner_x_right, pt.x());
+      }
+      if (y == 0)
+      {
+        inner_y_top = std::max(inner_y_top, pt.y());
+      }
+      if (y == num_pts - 1)
+      {
+        inner_y_bottom = std::min(inner_y_bottom, pt.y());
+      }
+    }
+  }
+
+  //! Compute and return the rectangles.
+  Rect inner(inner_x_left, inner_y_top,
+             inner_x_right - inner_x_left,
+             inner_y_bottom - inner_y_top);
+  Rect outer(outer_x_left, outer_y_top,
+             outer_x_right - outer_x_left,
+             outer_y_bottom - outer_y_top);
+
+  return std::pair<Rect, Rect>(inner, outer);
+}
+
+//!\brief Compute rectification parameters for a horizontal stereo pair
+//!
+//! The function is specific for the horizontal-stereo case.
+//! \param img_size The size of the original (distorted) image.
+//! \param cam0_parameters Vector of intrinsic parameters [fx, fy, cx, cy]
+//! for the 0-th camera of the stereo pair.
+//! \param cam0_distortion_coefficients Vector of distortion coefficients
+//! for the 0-th camera of the stereo pair.
+//! \param cam1_parameters Vector of intrinsic parameters [fx, fy, cx, cy]
+//! for the 1st camera of the stereo pair.
+//! \param cam1_distortion_coefficients Vector of distortion coefficients
+//! for the 1st camera of the stereo pair.
+//! \param T_cam0_cam1 Stereo extrinsic parameters, i.e. the transformation right-to-left.
+//! \return cam0_H rectifying homography for the 0-th camera.
+//! \return cam1_H rectifying homography for the 1st camera.
+//! \return transformed_cam0_parameters Output transformed parameters for the 0-th camera.
+//! \return transformed_cam1_parameters Output transformed parameters for the 1st camera.
+//! \return horizontal_offset Output displacement for the rectified stereo pair.
+template<typename CameraModel,
+         typename DistortionModel>
+inline std::tuple<Matrix3, Matrix3, Vector4, Vector4, real_t>
+computeHorizontalStereoParameters(const Size2u& img_size,
+                                  const Vector4& cam0_parameters,
+                                  const Vector4& cam0_distortion_coefficients,
+                                  const Vector4& cam1_parameters,
+                                  const Vector4& cam1_distortion_coefficients,
+                                  const Transformation& T_cam0_cam1)
+{
+  //! Compute the recification homographies as in
+  //! Trucco, Verry: Introductory techniques for 3D computer vision,
+  //! Prentice Hall 1998, page 160.
+  const Quaternion avg_rotation =
+      Quaternion::exp(-0.5*Quaternion::log(T_cam0_cam1.getRotation()));
+  const Vector3 transformed_t = avg_rotation.rotate(-T_cam0_cam1.getPosition());
+  const Vector3 e1 = transformed_t / transformed_t.norm();
+  Vector3 e2(-transformed_t(1), transformed_t(0), 0);
+  e2 = e2 / e2.norm();
+  const Vector3 e3 = e1.cross(e2);
+  Matrix3 rect_R;
+  rect_R.row(0) = e1.transpose();
+  rect_R.row(1) = e2.transpose();
+  rect_R.row(2) = e3.transpose();
+
+  //! Rotate both cameras according to the average rotation.
+  const Matrix3 cam0_H = rect_R * avg_rotation.getRotationMatrix().transpose();
+  const Matrix3 cam1_H = rect_R * avg_rotation.getRotationMatrix();
+
+  //! The images rectified according to cam0_H and cam1_H will not be contained
+  //! in the same region of the image plane as the original image.
+  //! Here we alter the focal lengths and the principal points to keep
+  //! all points within the original image size.
+  const Vector4* const camera_parameter_ptrs[2] = {&cam0_parameters,
+                                                   &cam1_parameters};
+  const Vector4* const distortion_coefficient_ptrs[2] = {&cam0_distortion_coefficients,
+                                                         &cam1_distortion_coefficients};
+  const Matrix3* const homography_ptrs[2] = {&cam0_H,
+                                             &cam1_H};
+
+  const real_t nx = img_size[0];
+  const real_t ny = img_size[1];
+
+  real_t transformed_focal_length = std::numeric_limits<real_t>::max();
+  for (int8_t i = 0; i < 2; ++i)
+  {
+    const Vector4& camera_parameters = *camera_parameter_ptrs[i];
+    const Vector4& distortion_coefficients = *distortion_coefficient_ptrs[i];
+    real_t focal_length = camera_parameters(1);
+    if (distortion_coefficients(0) < 0)
+    {
+      focal_length *= 1 + distortion_coefficients(0)*
+          (nx * nx + ny * ny) /
+          (4 * focal_length * focal_length);
+    }
+    transformed_focal_length = std::min(transformed_focal_length, focal_length);
+  }
+
+  Matrix22 transformed_principal_point;
+  for (int8_t i = 0; i < 2; ++i)
+  {
+    const Vector4& camera_parameters = *camera_parameter_ptrs[i];
+    const Vector4& distortion_coefficients = *distortion_coefficient_ptrs[i];
+    const Matrix3& H = *homography_ptrs[i];
+
+    Matrix34 img_corners;
+    img_corners << 0, nx, nx, 0,
+        0, 0, ny, ny,
+        1, 1, 1, 1;
+
+    Vector4 temp_cam_params;
+    temp_cam_params << transformed_focal_length, transformed_focal_length, 0, 0;
+    for (int8_t c = 0; c < 4; ++c)
+    {
+      CameraModel::backProject(camera_parameters.data(), img_corners.col(c).data());
+      DistortionModel::undistort(distortion_coefficients.data(), img_corners.col(c).data());
+      img_corners.col(c) = H * img_corners.col(c);
+      img_corners.col(c) /= img_corners.col(c)(2);
+      CameraModel::project(temp_cam_params.data(), img_corners.col(c).data());
+    }
+    transformed_principal_point.col(i) = Vector2((nx - 1) / 2, (ny - 1) / 2);
+    transformed_principal_point.col(i) -= img_corners.block(0, 0, 2, 4).rowwise().mean();
+  }
+  transformed_principal_point.col(0) =
+      transformed_principal_point.col(1) =
+      transformed_principal_point.rowwise().mean();
+
+  Vector4 transformed_cam0_parameters;
+  Vector4 transformed_cam1_parameters;
+  transformed_cam0_parameters << transformed_focal_length,
+      transformed_focal_length,
+      transformed_principal_point(0, 0),
+      transformed_principal_point(1, 0);
+  transformed_cam1_parameters << transformed_focal_length,
+      transformed_focal_length,
+      transformed_principal_point(0, 1),
+      transformed_principal_point(1, 1);
+
+  std::pair<Rect, Rect> cam0_rects =
+      innerAndOuterRectangles<CameraModel, DistortionModel>(
+        img_size, cam0_parameters,
+        transformed_cam0_parameters,
+        cam0_distortion_coefficients,
+        cam0_H);
+
+  std::pair<Rect, Rect> cam1_rects =
+      innerAndOuterRectangles<CameraModel, DistortionModel>(
+        img_size, cam1_parameters,
+        transformed_cam1_parameters,
+        cam1_distortion_coefficients,
+        cam1_H);
+
+  //! Determine s0, i.e. the scaling factor based on inner rectangles from both images.
+  //! @todo (MPI) support different scales in [0, 1].
+  //! Currently only scale = 0 is supported (s0).
+  //! Camera 0 image
+  real_t s0 = std::max(
+        transformed_principal_point(0, 0) / (transformed_principal_point(0, 0) - cam0_rects.first.x()),
+        transformed_principal_point(1, 0) / (transformed_principal_point(1, 0) - cam0_rects.first.y()));
+
+  s0 = std::max(s0,
+                (nx - transformed_principal_point(0, 0)) /
+                (cam0_rects.first.x() + cam0_rects.first.width() - transformed_principal_point(0, 0)));
+
+  s0 = std::max(s0,
+                (ny - transformed_principal_point(1, 0)) /
+                (cam0_rects.first.y() + cam0_rects.first.height() - transformed_principal_point(1, 0)));
+
+  //! Camera 1 image
+  s0 = std::max(
+        transformed_principal_point(0, 1) / (transformed_principal_point(0, 1) - cam1_rects.first.x()),
+        transformed_principal_point(1, 1) / (transformed_principal_point(1, 1) - cam1_rects.first.y()));
+
+  s0 = std::max(s0,
+                (nx - transformed_principal_point(0, 1)) /
+                (cam1_rects.first.x() + cam1_rects.first.width() - transformed_principal_point(0, 1)));
+
+  s0 = std::max(s0,
+                (ny - transformed_principal_point(1, 1)) /
+                (cam1_rects.first.y() + cam1_rects.first.height() - transformed_principal_point(1, 1)));
+
+  transformed_cam0_parameters(0) =
+      transformed_cam0_parameters(1) =
+      transformed_cam1_parameters(0) =
+      transformed_cam1_parameters(1) = transformed_focal_length * s0;
+
+  const real_t horizontal_offset = transformed_t(0) * s0;
+
+  return std::make_tuple(cam0_H,
+                         cam1_H,
+                         transformed_cam0_parameters,
+                         transformed_cam1_parameters,
+                         horizontal_offset);
+}
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_geometry/include/ze/geometry/line.hpp b/RWR/src/ze_oss/ze_geometry/include/ze/geometry/line.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..7ce495d1bae76d9c50a9f4a62867c0a08e01fcbe
--- /dev/null
+++ b/RWR/src/ze_oss/ze_geometry/include/ze/geometry/line.hpp
@@ -0,0 +1,100 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#pragma once
+
+#include <ze/common/transformation.hpp>
+#include <ze/common/types.hpp>
+
+namespace ze {
+
+// fwd
+class Camera;
+class Line;
+
+// Convenience typedefs:
+using Lines = std::vector<Line>;
+
+inline real_t distanceToLine(const Position& pos,
+                                const Position& line_anchor,
+                                const Vector3& line_direction)
+{
+  return (pos - line_anchor).cross(line_direction).norm();
+}
+
+inline LineMeasurement lineMeasurementFromBearings(const Vector3& bearing_1,
+                                                   const Vector3& bearing_2)
+{
+  return bearing_1.cross(bearing_2).normalized();
+}
+
+Matrix26 dLineMeasurement_dPose(const Transformation& T_B_W,
+                                const Transformation& T_C_B,
+                                const LineMeasurement& measurement_W,
+                                const Position& line_anchor,
+                                const Vector3& line_direction);
+
+
+std::pair<Positions, Positions> generateRandomVisibleLines(
+    const Camera& cam, const Transformation& T_W_C,
+    size_t num_lines, Lines& lines_W);
+
+Lines generateLinesFromEndpoints(const Positions& startpoints,
+                                const Positions& endpoints);
+
+class Line
+{
+public:
+  Line() = default;
+
+  Line(Quaternion orientation, real_t distance)
+  : orientation_(orientation)
+  , distance_(distance) {}
+
+
+  inline Vector3 direction() const
+  {
+    return orientation_.rotate(Vector3::UnitX());
+  }
+
+  inline Position anchorPoint() const
+  {
+    return distance_ * orientation_.rotate(Vector3::UnitZ());
+  }
+
+  inline real_t distanceToLine(const Position& pos) const
+  {
+    return ze::distanceToLine(pos, anchorPoint(), direction());
+  }
+
+  Vector2 calculateMeasurementError(const Vector3& measurement_W,
+                                    const Vector3& camera_position_W) const;
+
+private:
+  Quaternion orientation_;
+  real_t distance_ = 0.0;
+};
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_geometry/include/ze/geometry/lsq_solver-inl.hpp b/RWR/src/ze_oss/ze_geometry/include/ze/geometry/lsq_solver-inl.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..51cac70f0507010c4d3f9d63ea332e2fb07cc883
--- /dev/null
+++ b/RWR/src/ze_oss/ze_geometry/include/ze/geometry/lsq_solver-inl.hpp
@@ -0,0 +1,263 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include <ze/geometry/lsq_solver.hpp>
+
+#include <stdexcept>
+#include <ze/common/logging.hpp>
+#include <ze/common/matrix.hpp>
+
+namespace ze {
+
+template <typename T, typename Implementation>
+LeastSquaresSolver<T, Implementation>::LeastSquaresSolver(
+    const LeastSquaresSolverOptions& options)
+  : solver_options_(options)
+{
+  chi2_per_iter_.reserve(std::min(options.max_iter, 100u));
+}
+
+template <typename T, typename Implementation>
+void LeastSquaresSolver<T, Implementation>::optimize(State& state)
+{
+  // If state is of dynamic size, this resizes Hessian, dx, g.
+  allocateMemory(state);
+
+  if (solver_options_.strategy == SolverStrategy::GaussNewton)
+  {
+    optimizeGaussNewton(state);
+  }
+  else if (solver_options_.strategy == SolverStrategy::LevenbergMarquardt)
+  {
+    optimizeLevenbergMarquardt(state);
+  }
+}
+
+template <typename T, typename Implementation>
+void LeastSquaresSolver<T, Implementation>::optimizeGaussNewton(State& state)
+{
+  // Save the old model to rollback in case of unsuccessful update
+  State old_state = state;
+
+  // perform iterative estimation
+  for (iter_ = 0; iter_<solver_options_.max_iter; ++iter_)
+  {
+    rho_ = 0;
+    startIteration();
+
+    H_.setZero();
+    g_.setZero();
+
+    // compute initial error
+    real_t new_chi2 = evaluateError(state, &H_, &g_);
+
+    // solve the linear system
+    if (!solve(state, H_, g_, dx_))
+    {
+      LOG(WARNING) << "Matrix is close to singular! Stop Optimizing."
+                   << "H = " << H_ << "g = " << g_;
+      stop_ = true;
+    }
+
+    // check if error increased since last optimization
+    if ((iter_ > 0 && new_chi2 > chi2_ && solver_options_.stop_when_error_increases) || stop_)
+    {
+      VLOG(400) << "It. " << iter_
+                << "\t Failure"
+                << "\t new_chi2 = " << new_chi2
+                << "\t Error increased. Stop optimizing.";
+      state = old_state; // rollback
+      break;
+    }
+
+    // update the model
+    State new_state;
+    update(state, dx_, new_state);
+    old_state = state;
+    state = new_state;
+    chi2_ = new_chi2;
+    chi2_per_iter_.push_back(chi2_);
+    real_t x_norm = normMax(dx_);
+    VLOG(400) << "It. " << iter_
+              << "\t Success"
+              << "\t new_chi2 = " << new_chi2
+              << "\t x_norm = " << x_norm;
+    finishIteration();
+
+    // stop when converged, i.e. update step too small
+    if (x_norm < solver_options_.eps)
+    {
+      VLOG(400) << "Converged, x_norm " << x_norm << " < " << solver_options_.eps;
+      break;
+    }
+  }
+}
+
+template <typename T, typename Implementation>
+void LeastSquaresSolver<T, Implementation>::optimizeLevenbergMarquardt(State& state)
+{
+  // init parameters
+  mu_ = solver_options_.mu_init;
+  nu_ = solver_options_.nu_init;
+
+  // compute the initial error
+  chi2_ = evaluateError(state, nullptr, nullptr);
+  VLOG(400) << "init chi2 = " << chi2_;
+
+  // TODO: compute initial lambda
+  // Hartley and Zisserman: "A typical init value of lambda is 10^-3 times the
+  // average of the diagonal elements of J'J"
+  // Compute Initial Lambda
+  if (mu_ < 0)
+  {
+    real_t H_max_diag = maxAbsDiagonalElement(H_);
+    real_t tau = 1e-4;
+    mu_ = tau*H_max_diag;
+  }
+
+  // perform iterative estimation
+  for (iter_ = 0; iter_<solver_options_.max_iter; ++iter_)
+  {
+    rho_ = 0;
+    startIteration();
+
+    // try to compute and update, if it fails, try with increased mu
+    trials_ = 0;
+    do
+    {
+      // init variables
+      State new_model;
+      real_t new_chi2 = -1;
+      H_.setZero();
+      g_.setZero();
+
+      // linearize
+      evaluateError(state, &H_, &g_);
+
+      // add damping term:
+      H_ += (H_.diagonal() * mu_).asDiagonal();
+
+      // solve the linear system to obtain small perturbation in direction of gradient
+      if (solve(state, H_, g_, dx_))
+      {
+        // apply perturbation to the state
+        update(state, dx_, new_model);
+
+        // compute error with new model and compare to old error
+        new_chi2 = evaluateError(new_model, nullptr, nullptr);
+        rho_ = chi2_-new_chi2;
+      }
+      else
+      {
+        LOG(WARNING) << "Matrix is close to singular! Stop Optimizing."
+                     << "H = " << H_ << "g = " << g_;
+        rho_ = -1;
+      }
+
+      if (rho_ > 0.0)
+      {
+        // update decrased the error -> success
+        state = new_model;
+        chi2_ = new_chi2;
+        chi2_per_iter_.push_back(chi2_);
+        stop_ = normMax(dx_) < solver_options_.eps;
+        mu_ *= std::max(real_t{0.333f},
+                        std::min(real_t{1.0f - 8.0f * rho_ * rho_ * rho_},
+                                 real_t{0.666f}));
+        nu_ = 2.;
+        VLOG(400) << "It. " << iter_
+                  << "\t Trial " << trials_
+                  << "\t Success"
+                  << "\t new_chi2 = " << new_chi2
+                  << "\t mu = " << mu_
+                  << "\t nu = " << nu_;
+      }
+      else
+      {
+        // update increased the error -> fail
+        mu_ *= nu_;
+        nu_ *= 2.;
+        ++trials_;
+        if (trials_ >= solver_options_.max_trials)
+        {
+          stop_ = true;
+        }
+
+        VLOG(400) << "It. " << iter_
+                  << "\t Trial " << trials_
+                  << "\t Failure"
+                  << "\t new_chi2 = " << new_chi2
+                  << "\t mu = " << mu_
+                  << "\t nu = " << nu_;
+      }
+      finishTrial();
+
+    } while (!(rho_>0 || stop_));
+
+    if (stop_)
+    {
+      break;
+    }
+
+    finishIteration();
+  }
+}
+
+template <typename T, typename Implementation>
+void LeastSquaresSolver<T, Implementation>::reset()
+{
+  VLOG(400) << "Reset";
+  chi2_ = std::numeric_limits<real_t>::max();
+  mu_ = solver_options_.mu_init;
+  nu_ = solver_options_.nu_init;
+  iter_ = 0;
+  trials_ = 0;
+  stop_ = false;
+}
+
+template <typename T, typename Implementation>
+bool LeastSquaresSolver<T, Implementation>::solveDefaultImpl(
+    const HessianMatrix& H,
+    const GradientVector& g,
+    UpdateVector& dx)
+{
+  dx = H.ldlt().solve(g);
+  if (std::isnan(dx[0]))
+  {
+    return false;
+  }
+  return true;
+}
+
+template <typename T, typename Implementation>
+void LeastSquaresSolver<T, Implementation>::updateDefaultImpl(
+    const State& state,
+    const UpdateVector& dx,
+    State& new_state)
+{
+  new_state = traits<State>::retract(state, dx);
+}
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_geometry/include/ze/geometry/lsq_solver.hpp b/RWR/src/ze_oss/ze_geometry/include/ze/geometry/lsq_solver.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..9ef4c05b83b8e52faee483f056076a03199973b5
--- /dev/null
+++ b/RWR/src/ze_oss/ze_geometry/include/ze/geometry/lsq_solver.hpp
@@ -0,0 +1,257 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#pragma once
+
+#include <type_traits>
+#include <vector>
+
+#include <ze/common/types.hpp>
+#include <ze/common/manifold.hpp>
+
+namespace ze {
+
+enum class SolverStrategy {
+  GaussNewton,
+  LevenbergMarquardt
+};
+
+struct LeastSquaresSolverOptions
+{
+  //! Solver strategy.
+  SolverStrategy strategy = SolverStrategy::GaussNewton;
+
+  //! Damping parameter. If mu > 0, coefficient matrix is positive definite, this
+  //! ensures that x is a descent direction. If mu is large, x is a short step in
+  //! the steepest direction. This is good if the current iterate is far from the
+  //! solution. If mu is small, LM approximates gauss newton iteration and we
+  //! have (almost) quadratic convergence in the final stages.
+  real_t mu_init{0.01};
+
+  //! Increase factor of mu after fail
+  real_t nu_init{2.0};
+
+  //! Max number of iterations
+  uint32_t max_iter{15u};
+
+  //! Max number of trials (used in LevenbergMarquardt)
+  uint32_t max_trials{5u};
+
+  //! Stop when error increases.
+  bool stop_when_error_increases{false};
+
+  //! Output Statistics
+  bool verbose{false};
+
+  //! Stop if update norm is smaller than eps
+  real_t eps{1.0e-10};
+};
+
+//! Abstract Class for solving nonlinear least-squares (NLLS) problems.
+//! Template Parameters: D: dimension of the state, T: type of the model
+//! e.g. SE2, SE3
+template <typename T, typename Implementation>
+class LeastSquaresSolver
+{
+public:
+  using State = T;
+  enum Dimension : int { dimension = traits<State>::dimension };
+  using HessianMatrix = Eigen::Matrix<real_t, dimension, dimension>;
+  using GradientVector = Eigen::Matrix<real_t, dimension, 1>;
+  using UpdateVector = Eigen::Matrix<real_t, dimension, 1>;
+
+  LeastSquaresSolverOptions solver_options_;
+
+protected:
+  LeastSquaresSolver() = default;
+
+  LeastSquaresSolver(const LeastSquaresSolverOptions& options);
+
+  virtual ~LeastSquaresSolver() = default;
+
+public:
+  //! Calls the GaussNewton or LevenbergMarquardt optimization strategy.
+  void optimize(State& state);
+
+  //! Gauss Newton optimization strategy.
+  void optimizeGaussNewton(State& state);
+
+  //! Levenberg Marquardt optimization strategy.
+  void optimizeLevenbergMarquardt(State& state);
+
+  //! Reset all parameters to restart the optimization.
+  void reset();
+
+  //! Get the squared error.
+  inline real_t error() const
+  {
+    return chi2_;
+  }
+
+  //! Get error at every iteration.
+  inline const std::vector<real_t>& errors() const
+  {
+    return chi2_per_iter_;
+  }
+
+  //! The the Hessian matrix (Information Matrix).
+  inline const HessianMatrix& hessian() const
+  {
+    return H_;
+  }
+
+protected:
+  //! Get implementation (Curiously-Returning Template Pattern).
+  Implementation& impl()
+  {
+    return *static_cast<Implementation*>(this);
+  }
+
+  //! Evaluates the error at provided state. Optional return variables are
+  //! the Hessian matrix and the gradient vector (Jacobian * residual).
+  //! If these parameters are requested, the system is linearized at the current
+  //! state.
+  real_t evaluateError(
+      const State& state,
+      HessianMatrix* H,
+      GradientVector* g)
+  {
+    return impl().evaluateError(state, H, g);
+  }
+
+  //! Solve the linear system H*dx = g to obtain optimal perturbation dx.
+  bool solve(
+      const State& state,
+      const HessianMatrix& H,
+      const GradientVector& g,
+      UpdateVector& dx)
+  {
+    if(&LeastSquaresSolver::solve != &Implementation::solve)
+    {
+      return impl().solve(state, H, g, dx);
+    }
+    return solveDefaultImpl(H, g, dx);
+  }
+
+  //! Apply the perturbation dx to the state.
+  void update(
+      const State& state,
+      const UpdateVector& dx,
+      State& new_state)
+  {
+    if(&LeastSquaresSolver::update != &Implementation::update)
+    {
+      return impl().update(state, dx, new_state);
+    }
+    return updateDefaultImpl(state, dx, new_state);
+  }
+
+  void startIteration()
+  {
+    if(&LeastSquaresSolver::startIteration != &Implementation::startIteration)
+    {
+      impl().startIteration();
+    }
+  }
+
+  void finishIteration()
+  {
+    if(&LeastSquaresSolver::finishIteration != &Implementation::finishIteration)
+    {
+      impl().finishIteration();
+    }
+  }
+
+  void finishTrial()
+  {
+    if(&LeastSquaresSolver::finishTrial != &Implementation::finishTrial)
+    {
+      impl().finishTrial();
+    }
+  }
+
+private:
+  //! Default implementation to solve the linear system H*dx = g to obtain optimal perturbation dx.
+  bool solveDefaultImpl(
+      const HessianMatrix& H,
+      const GradientVector& g,
+      UpdateVector& dx);
+
+  void updateDefaultImpl(
+      const State& state,
+      const UpdateVector& dx,
+      State& new_state);
+
+  template<typename State, typename std::enable_if<(traits<State>::dimension == Eigen::Dynamic)>::type* = nullptr>
+  inline void allocateMemory(State& state)
+  {
+    const int dim = state.getDimension();
+    H_.resize(dim, dim);
+    g_.resize(dim);
+    dx_.resize(dim);
+  }
+
+  template<typename State, typename std::enable_if<(traits<State>::dimension != Eigen::Dynamic)>::type* = nullptr>
+  inline void allocateMemory(State& /*state*/)
+  {}
+
+protected:
+  //! Hessian or approximation Jacobian*Jacobian^T.
+  HessianMatrix H_;
+
+  //! Jacobian*residual.
+  GradientVector g_;
+
+  //! Update step.
+  UpdateVector dx_;
+
+  //! Whitened error / log-likelihood: 1/(2*sigma^2)*(z-h(x))^2.
+  real_t chi2_{std::numeric_limits<real_t>::max()};
+
+  //! Error reduction: chi2 - new_chi2.
+  real_t rho_{0.0};
+
+  //! Damping parameter.
+  real_t mu_{0.01};
+
+  //! Factor that specifies how much we increase mu at every trial.
+  real_t nu_{2.0};
+
+  //! Stop flag.
+  bool stop_{false};
+
+  //! Current Iteration.
+  size_t iter_{0u};
+
+  //! Current number of trials.
+  size_t trials_{0u};
+
+  // Statistics:
+  std::vector<real_t> chi2_per_iter_;
+};
+
+} // namespace ze
+
+#include <ze/geometry/lsq_solver-inl.hpp>
diff --git a/RWR/src/ze_oss/ze_geometry/include/ze/geometry/lsq_state.hpp b/RWR/src/ze_oss/ze_geometry/include/ze/geometry/lsq_state.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..5285363e8e65d23ad1203c7ef65c80778fcdafc4
--- /dev/null
+++ b/RWR/src/ze_oss/ze_geometry/include/ze/geometry/lsq_state.hpp
@@ -0,0 +1,273 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#pragma once
+
+#include <tuple>
+#include <type_traits>
+#include <typeinfo>
+#include <iostream>
+#include <glog/logging.h>
+#include <ze/common/types.hpp>
+#include <ze/common/manifold.hpp>
+
+namespace ze {
+
+namespace internal {
+
+// -----------------------------------------------------------------------------
+// Check if any element in the tuple is of dynamic size
+template<typename T> struct TupleIsFixedSize;
+
+template<typename Element>
+struct TupleIsFixedSize<std::tuple<Element>>
+{
+ static constexpr bool is_fixed_size = (traits<Element>::dimension > 0);
+};
+
+template<typename Element, typename... Elements>
+struct TupleIsFixedSize<std::tuple<Element, Elements...>>
+{
+  static constexpr bool is_fixed_size = (traits<Element>::dimension > 0)
+           && TupleIsFixedSize<std::tuple<Elements...>>::is_fixed_size;
+};
+
+// -----------------------------------------------------------------------------
+// Sum the dimensions of all elements in a tuple.
+template<typename T> struct TupleGetDimension;
+
+template<typename Element>
+struct TupleGetDimension<std::tuple<Element>>
+{
+  static constexpr int dimension = traits<Element>::dimension;
+};
+
+template<typename Element, typename... Elements>
+struct TupleGetDimension<std::tuple<Element, Elements...>>
+{
+  static constexpr int dimension = traits<Element>::dimension
+           + TupleGetDimension<std::tuple<Elements...>>::dimension;
+};
+
+} // namespace internal
+
+// -----------------------------------------------------------------------------
+// A State can contain fixed-sized elements such as Scalars, Vector3, Transformation.
+template<typename... Elements>
+class State
+{
+public:
+  using StateT = State<Elements...>;
+  using StateTuple = decltype(std::tuple<Elements...>());
+
+  enum StateSize : int
+  {
+    size = std::tuple_size<StateTuple>::value
+  };
+
+  enum StateDimension : int
+  {
+    // Dimension is -1 if State is not of fixed size (e.g. contains VectorX).
+    dimension = (internal::TupleIsFixedSize<StateTuple>::is_fixed_size)
+      ? internal::TupleGetDimension<StateTuple>::dimension : Eigen::Dynamic
+  };
+
+  using TangentVector = Eigen::Matrix<real_t, dimension, 1> ;
+  using Jacobian = Eigen::Matrix<real_t, dimension, dimension>;
+
+  // utility
+  template <size_t i>
+  using ElementType = typename std::tuple_element<i, StateTuple>::type;
+
+  StateTuple state_;
+
+  void print() const
+  {
+    printImpl<0>();
+    VLOG(1) << "--\n";
+  }
+
+  void retract(const TangentVector& v)
+  {
+    CHECK_EQ(v.size(), getDimension());
+    retractImpl<0>(v, 0);
+  }
+
+  //! Get actual dimension of the state, also if state is of dynamic size.
+  int getDimension() const
+  {
+    return dimensionImpl<0>();
+  }
+
+  //! Get reference to element.
+  template<uint32_t i>
+  inline auto at() -> decltype (std::get<i>(state_)) &
+  {
+    return std::get<i>(state_);
+  }
+
+  //! Get const reference to element.
+  template<uint32_t i>
+  inline auto at() const -> decltype (std::get<i>(state_)) &
+  {
+    return std::get<i>(state_);
+  }
+
+  //! Returns whether one element of the state is of dynamic size.
+  static constexpr bool isDynamicSize()
+  {
+    return (dimension == -1);
+  }
+
+  //! Returns whether element i is of dynamic size.
+  template<uint32_t i>
+  static constexpr bool isElementDynamicSize()
+  {
+    return (traits<ElementType<i>>::dimension == -1);
+  }
+
+private:
+  //! @name dimension recursive implementation.
+  //! @{
+  template<uint32_t i, typename std::enable_if<(i<size-1 && traits<ElementType<i>>::dimension != -1)>::type* = nullptr>
+  inline int dimensionImpl() const
+  {
+    return traits<ElementType<i>>::dimension + dimensionImpl<i+1>();
+  }
+
+  template<uint32_t i, typename std::enable_if<(i<size-1 && traits<ElementType<i>>::dimension == -1)>::type* = nullptr>
+  inline int dimensionImpl() const
+  {
+    // Element is of dynamic size.
+    return traits<ElementType<i>>::getDimension(std::get<i>(state_)) + dimensionImpl<i+1>();
+  }
+
+  template<uint32_t i,typename std::enable_if<(i==size-1 && traits<ElementType<i>>::dimension != -1)>::type* = nullptr>
+  inline int dimensionImpl() const
+  {
+    return traits<ElementType<i>>::dimension;
+  }
+
+  template<uint32_t i, typename std::enable_if<(i==size-1 && traits<ElementType<i>>::dimension == -1)>::type* = nullptr>
+  inline int dimensionImpl() const
+  {
+    // Element is of dynamic size.
+    return traits<ElementType<i>>::getDimension(std::get<i>(state_));
+  }
+  //! @}
+
+  //! @name print recursive implementation.
+  //! @{
+  template<uint32_t i, typename std::enable_if<(i<size)>::type* = nullptr>
+  inline void printImpl() const
+  {
+    using T = ElementType<i>;
+    VLOG(1) << "--\nState-Index: " << i << "\n"
+            << "Type = " << typeid(T).name() << "\n"
+            << "Dimension = " << traits<T>::dimension << "\n"
+            << "Value = \n" << std::get<i>(state_) << "\n";
+    printImpl<i+1>();
+  }
+
+  template<uint32_t i, typename std::enable_if<(i>=size)>::type* = nullptr>
+  inline void printImpl() const
+  {}
+  //! @}
+
+  //! @name retract recursive implementation.
+  //! @{
+  template<uint32_t i=0, typename std::enable_if<(i<size-1 && traits<ElementType<i>>::dimension != -1)>::type* = nullptr>
+  inline void retractImpl(const TangentVector& v, uint32_t j)
+  {
+    using T = ElementType<i>;
+    std::get<i>(state_) = traits<T>::retract(
+           std::get<i>(state_),
+           v.template segment<traits<T>::dimension>(j));
+    retractImpl<i+1>(v, j + traits<T>::dimension);
+  }
+
+  template<uint32_t i=0, typename std::enable_if<(i<size-1 && traits<ElementType<i>>::dimension == -1)>::type* = nullptr>
+  inline void retractImpl(const TangentVector& v, uint32_t j)
+  {
+    // Element is of dynamic size.
+    using T = ElementType<i>;
+    const int element_dim = traits<T>::getDimension(std::get<i>(state_));
+    std::get<i>(state_) = traits<T>::retract(
+           std::get<i>(state_), v.segment(j, element_dim));
+    retractImpl<i+1>(v, j + element_dim);
+  }
+
+  template<uint32_t i=0, typename std::enable_if<(i==size-1 && traits<ElementType<i>>::dimension != -1)>::type* = nullptr>
+  inline void retractImpl(const TangentVector& v, uint32_t j)
+  {
+    using T = ElementType<i>;
+    std::get<i>(state_) = traits<T>::retract(
+           std::get<i>(state_),
+           v.template segment<traits<T>::dimension>(j));
+  }
+
+  template<uint32_t i=0, typename std::enable_if<(i==size-1 && traits<ElementType<i>>::dimension == -1)>::type* = nullptr>
+  inline void retractImpl(const TangentVector& v, uint32_t j)
+  {
+    // Element is of dynamic size.
+    using T = ElementType<i>;
+    const int element_dim = traits<T>::getDimension(std::get<i>(state_));
+    std::get<i>(state_) = traits<T>::retract(
+           std::get<i>(state_), v.segment(j, element_dim));
+  }
+  //! @}
+
+};
+
+// -----------------------------------------------------------------------------
+// Manifold traits for State.
+template<typename T> struct traits;
+
+template<typename... Elements>
+struct traits<State<Elements...>>
+{
+  typedef State<Elements...> StateT;
+  enum { dimension = StateT::dimension };
+
+  typedef Eigen::Matrix<real_t, dimension, 1> TangentVector;
+  typedef Eigen::Matrix<real_t, dimension, dimension> Jacobian;
+
+  static StateT retract(
+           const StateT& origin, const TangentVector& v,
+           Jacobian* H1 = nullptr, Jacobian* H2 = nullptr)
+  {
+    if(H1 || H2)
+    {
+      LOG(FATAL) << "Not implemented.";
+    }
+    StateT origin_perturbed = origin;
+    origin_perturbed.retract(v);
+    return origin_perturbed;
+  }
+
+  //! @todo(cfo): Implement Retract, Equals.
+};
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_geometry/include/ze/geometry/pose_optimizer.hpp b/RWR/src/ze_oss/ze_geometry/include/ze/geometry/pose_optimizer.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..b6fe0caf9b7661ce5d530ad66a8a0356965ca34f
--- /dev/null
+++ b/RWR/src/ze_oss/ze_geometry/include/ze/geometry/pose_optimizer.hpp
@@ -0,0 +1,217 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#pragma once
+
+#include <ze/common/transformation.hpp>
+#include <ze/common/manifold.hpp>
+#include <ze/cameras/camera_utils.hpp>
+#include <ze/cameras/camera_impl.hpp>
+#include <ze/geometry/line.hpp>
+#include <ze/geometry/robust_cost.hpp>
+#include <ze/geometry/lsq_solver.hpp>
+
+namespace ze {
+
+enum class PoseOptimizerResidualType
+{
+  Bearing,
+  UnitPlane,
+  UnitPlaneEdgelet,
+  Line
+};
+
+//! Data required by the pose-optimizer per frame.
+struct PoseOptimizerFrameData
+{
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  PoseOptimizerResidualType type = PoseOptimizerResidualType::UnitPlane;
+
+  uint32_t camera_idx = 0u;
+
+  //! Measurements: Bearing vectors corresponding to keypoints.
+  Bearings f;
+
+  //! Gradients: Used only for edgelets.
+  Gradients grad;
+
+  //! At which level was the keypoint extracted.
+  VectorX scale;
+
+  //! Line measurements. The normalized plane normals through the camera and
+  //! the measured endpoints.
+  LineMeasurements line_measurements_C;
+
+  //! Measurements bookkeeping: Corresponding indices. (Not used by the actual algorithm).
+  KeypointIndices kp_idx;
+
+  //! Landmark positions. Each column corresponds to a bearing measurement.
+  //! @todo(cfo): Use inverse depth parametrization or homogeneous points.
+  Positions p_W;
+
+  //! Line. Each entry corresponds to a line measurement.
+  Lines lines_W;
+
+  //! Extrinsic transformation between camera and body (i.e., imu) frame.
+  Transformation T_C_B;
+
+  //! @name Internal buffer
+  //! @{
+  //! Measurements: Projected on unit-plane. (computed internally).
+  Keypoints uv;
+
+  real_t measurement_sigma;
+  //! @}
+};
+using PoseOptimizerFrameDataVec = std::vector<PoseOptimizerFrameData>;
+
+//! Optimizes body pose by minimizing difference between bearing vectors.
+class PoseOptimizer :
+    public LeastSquaresSolver<Transformation, PoseOptimizer>
+{
+public:
+  using LeastSquaresSolver::HessianMatrix;
+  using LeastSquaresSolver::GradientVector;
+  using ScaleEstimator = MADScaleEstimator<real_t>;
+  using WeightFunction = TukeyWeightFunction<real_t>;
+
+  PoseOptimizer(
+      const LeastSquaresSolverOptions& options,
+      std::vector<PoseOptimizerFrameData>& data);
+
+  PoseOptimizer(
+      const LeastSquaresSolverOptions& options,
+      std::vector<PoseOptimizerFrameData>& data,
+      const Transformation& T_B_W_prior,
+      const real_t prior_weight_pos,
+      const real_t prior_weight_rot);
+
+  static LeastSquaresSolverOptions getDefaultSolverOptions();
+
+  void setPrior(
+      const Transformation& T_B_W_prior,
+      const real_t prior_weight_pos,
+      const real_t prior_weight_rot);
+
+  real_t evaluateError(
+      const Transformation& T_B_W,
+      HessianMatrix* H,
+      GradientVector* g);
+
+  void update(
+      const Transformation& state,
+      const UpdateVector& dx,
+      Transformation& new_state)
+  {
+    new_state = traits<Transformation>::retract(state, dx);
+    new_state.getRotation().normalize();
+  }
+
+private:
+  //! Checks whether given data is valid. Throws if not.
+  void checkData() const;
+
+  std::vector<PoseOptimizerFrameData>& data_;
+
+  //! @name Prior
+  //! @{
+  Transformation T_B_W_prior_;
+  real_t prior_weight_pos_ {0.0};
+  real_t prior_weight_rot_ {0.0};
+  //! @}
+};
+
+//! Returns sum of chi2 errors (weighted and whitened errors) and
+//! a vector of withened errors for each error term (used for outlier removal).
+std::pair<real_t, VectorX> evaluateBearingErrors(
+    const Transformation& T_B_W,
+    const bool compute_measurement_sigma,
+    PoseOptimizerFrameData& data,
+    PoseOptimizer::HessianMatrix* H,
+    PoseOptimizer::GradientVector* g);
+
+//! Returns sum of chi2 errors (weighted and whitened errors) and
+//! a vector of withened errors for each error term (used for outlier removal).
+std::pair<real_t, VectorX> evaluateUnitPlaneErrors(
+    const Transformation& T_B_W,
+    const bool compute_measurement_sigma,
+    PoseOptimizerFrameData& data,
+    PoseOptimizer::HessianMatrix* H,
+    PoseOptimizer::GradientVector* g);
+
+std::pair<real_t, VectorX> evaluateLineErrors(
+    const Transformation& T_B_W,
+    const bool compute_measurement_sigma,
+    PoseOptimizerFrameData& data,
+    PoseOptimizer::HessianMatrix* H,
+    PoseOptimizer::GradientVector* g);
+
+std::vector<KeypointIndex> getOutlierIndices(
+    PoseOptimizerFrameData& data,
+    const Camera& cam,
+    const Transformation& T_B_W,
+    const real_t pixel_threshold);
+
+/*!
+ * @brief Jacobian of bearing vector w.r.t. landmark in camera coordinates.
+ *
+ * f = p / norm(p) = p / sqrt(x^2 + y^2 + z^2), with p = [x, y, z].
+ *
+ *                                       | y^2 + z^2, -xy, -xz |
+ * df/dp = 1 / (x^2 + y^2 + z^2)^(3/2) * | -xy, x^2 + z^2, -yz |
+ *                                       | -xz, -yz, x^2 + z^2 |.
+ */
+inline Matrix3 dBearing_dLandmark(const Eigen::Ref<const Position>& p_C)
+{
+  const real_t x2 = p_C(0) * p_C(0);
+  const real_t y2 = p_C(1) * p_C(1);
+  const real_t z2 = p_C(2) * p_C(2);
+  const real_t xy = p_C(0) * p_C(1);
+  const real_t xz = p_C(0) * p_C(2);
+  const real_t yz = p_C(1) * p_C(2);
+  const real_t x2_y2_z2 = x2 + y2 + z2;
+  Matrix3 J;
+  J << y2 + z2, -xy, -xz,
+       -xy, x2 + z2, -yz,
+       -xz, -yz, x2 + y2;
+  J /= (x2_y2_z2 * std::sqrt(x2_y2_z2));
+  return J;
+}
+
+/*!
+ * @brief Jacobian of unit-plane coordinates uv w.r.t. landmark in camera coordinates.
+ */
+inline Matrix23 dUv_dLandmark(const Eigen::Ref<const Position>& p_C)
+{
+  const real_t z_sq = p_C(2) * p_C(2);
+  const real_t z_inv = real_t{1.0} / p_C(2);
+  Matrix23 J;
+  J << z_inv, 0.0, -p_C(0) / z_sq,
+       0.0, z_inv, -p_C(1) / z_sq;
+  return J;
+}
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_geometry/include/ze/geometry/pose_prior.hpp b/RWR/src/ze_oss/ze_geometry/include/ze/geometry/pose_prior.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..5421d12a0f31ca9a51498e2bdba3f16e4260ca1d
--- /dev/null
+++ b/RWR/src/ze_oss/ze_geometry/include/ze/geometry/pose_prior.hpp
@@ -0,0 +1,88 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#pragma once
+
+#include <ze/common/types.hpp>
+#include <ze/common/transformation.hpp>
+
+namespace ze {
+
+//! A heuristic to add a pose prior to a 6x6 Hessian and gradient vector block.
+inline void applyPosePrior(
+    const Transformation& T_estimate,
+    const Transformation& T_prior,
+    const real_t prior_weight_rot,
+    const real_t prior_weight_pos,
+    Eigen::Ref<Matrix6> H,
+    Eigen::Ref<Vector6> g)
+{
+  if (prior_weight_rot > 0)
+  {
+    real_t H_max_diag = 0;
+    for(int i = 3; i < 6; ++i)
+    {
+      H_max_diag = std::max(H_max_diag, std::abs(H(i,i)));
+    }
+
+    // Residual of prior:
+    Vector3 r = (T_prior.getRotation().inverse() * T_estimate.getRotation()).log();
+
+    // Jacobian w.r.t. prior:
+    Matrix3 J = logmapDerivativeSO3(r);
+    real_t weight = H_max_diag * prior_weight_rot;
+
+    // Add to normal equations:
+    H.bottomRightCorner<3,3>().noalias() += J.transpose() * J * weight;
+    g.tail<3>().noalias() -= J.transpose() * r * weight;
+
+    VLOG(500) << "Rot. prior Hessian = J^T * J = \n" << J.transpose() * J;
+  }
+
+  // Position prior.
+  if (prior_weight_pos > 0)
+  {
+    real_t H_max_diag = 0;
+    for(int i = 0; i < 3; ++i)
+    {
+      H_max_diag = std::max(H_max_diag, std::abs(H(i,i)));
+    }
+
+    // Residual of prior:
+    Vector3 r = T_estimate.getPosition() - T_prior.getPosition();
+
+    // Jacobian w.r.t. prior:
+    Matrix3 J = T_estimate.getRotationMatrix();
+    real_t weight = H_max_diag * prior_weight_pos;
+
+    // Add to normal equations:
+    H.topLeftCorner<3,3>().noalias() += I_3x3 * weight; // J^T * J = I_3x3
+    g.head<3>().noalias() -= J.transpose() * r * weight;
+
+    VLOG(500) << "Pos. prior Hessian = J^T * J = \n" << J.transpose() * J;
+  }
+}
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_geometry/include/ze/geometry/ransac_relative_pose.hpp b/RWR/src/ze_oss/ze_geometry/include/ze/geometry/ransac_relative_pose.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..e0808dcd3b3a039adcc131fa870bc2e3eeb50eb5
--- /dev/null
+++ b/RWR/src/ze_oss/ze_geometry/include/ze/geometry/ransac_relative_pose.hpp
@@ -0,0 +1,114 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#pragma once
+
+#include <ze/common/types.hpp>
+#include <ze/common/transformation.hpp>
+
+namespace ze {
+
+// fwd
+class Camera;
+
+using BearingsVector =
+  std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>>;
+
+// utility
+inline BearingsVector bearingsVectorFromBearings(const Bearings& f)
+{
+  BearingsVector v;
+  v.reserve(f.cols());
+  for (int i = 0; i < f.cols(); ++i)
+  {
+    v.push_back(f.col(i).cast<double>());
+  }
+  return v;
+}
+
+enum class RelativePoseAlgorithm {
+  FivePoint,               //!< Nister 5-point relative pose.
+  TwoPointTranslationOnly, //!< 2-point relative pose, assumes known rotation between frames.
+  TwoPointRotationOnly     //!< 2-point relative pose, assumes no translation between frames.
+};
+
+class RansacRelativePose
+{
+public:
+  RansacRelativePose() = delete;
+
+  RansacRelativePose(
+      const Camera& cam,
+      const real_t& reprojection_threshold_px);
+
+  bool solve(
+      const Bearings& f_ref,
+      const Bearings& f_cur,
+      const RelativePoseAlgorithm method,
+      Transformation& T_cur_ref);
+
+  bool solve(
+      const BearingsVector& f_ref,
+      const BearingsVector& f_cur,
+      const RelativePoseAlgorithm method,
+      Transformation& T_cur_ref);
+
+  bool solveRelativePose(
+      const BearingsVector& f_ref,
+      const BearingsVector& f_cur,
+      Transformation& T_cur_ref);
+
+  bool solveTranslationOnly(
+      const BearingsVector& f_ref,
+      const BearingsVector& f_cur,
+      Transformation& T_cur_ref);
+
+  bool solveRotationOnly(
+      const BearingsVector& f_ref,
+      const BearingsVector& f_cur,
+      Transformation& T_cur_ref);
+
+  inline uint32_t numIterations() const { return num_iterations_; }
+
+  inline const std::vector<int>& inliers() const { return inliers_; }
+
+  std::vector<int> outliers();
+
+  //! @name: OpenGV settings.
+  //! @{
+  real_t ogv_threshold_;
+  uint32_t ogv_max_iterations_ = 100;
+  real_t ogv_init_probability_ = 0.999;
+  uint32_t ogv_verbosity_level_ = 0u;
+  //! @}
+
+private:
+  uint32_t num_measurements_ = 0u;
+  uint32_t num_iterations_ = 0u;
+  real_t result_probability_;
+  std::vector<int> inliers_;
+};
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_geometry/include/ze/geometry/robust_cost.hpp b/RWR/src/ze_oss/ze_geometry/include/ze/geometry/robust_cost.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..940d40383cfd20ebeda6205f3b77bb7dadcb3cfb
--- /dev/null
+++ b/RWR/src/ze_oss/ze_geometry/include/ze/geometry/robust_cost.hpp
@@ -0,0 +1,151 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#pragma once
+
+#include <vector>
+#include <memory>
+#include <ze/common/logging.hpp>
+#include <Eigen/Core>
+
+#include <ze/common/types.hpp>
+#include <ze/common/statistics.hpp>
+
+namespace ze {
+
+// -----------------------------------------------------------------------------
+// Scale Estimators
+
+//! Dummy scale estimator.
+template <typename Scalar>
+struct UnitScaleEstimator
+{
+  static constexpr Scalar compute(const VectorX& /*errors*/)
+  {
+    return Scalar{1.0};
+  }
+};
+
+//! Estimates scale by computing the median absolute deviation (MAD).
+template <typename Scalar>
+struct MADScaleEstimator
+{
+  using VectorX = Eigen::Matrix<Scalar, Eigen::Dynamic, 1>;
+  static Scalar compute(const VectorX& errors)
+  {
+    VectorX absolute_error(errors.size());
+    absolute_error = errors.array().abs();
+    std::pair<Scalar, bool> res = median(absolute_error);
+    CHECK(res.second);
+    return Scalar{1.48} * res.first; // 1.48f / 0.6745
+  }
+};
+
+//! Estimates scale by computing the standard deviation.
+template <typename Scalar>
+struct NormalDistributionScaleEstimator
+{
+  using VectorX = Eigen::Matrix<Scalar, Eigen::Dynamic, 1>;
+  static Scalar compute(const VectorX& errors)
+  {
+    // normed_errors should not have absolute values.
+    const int n = errors.size();
+    CHECK(n > 1);
+    const Scalar mean = errors.sum() / n;
+    Scalar sum2 = Scalar{0.0};
+    for(int i = 0; i < errors.size(); ++i)
+    {
+      sum2 += (errors(i) - mean) * (errors(i) - mean);
+    }
+    return std::sqrt(sum2 / (n - 1));
+  }
+};
+
+
+// -----------------------------------------------------------------------------
+// Scale Estimators
+// Weight-Functions for M-Estimators
+// http://research.microsoft.com/en-us/um/people/zhang/inria/publis/tutorial-estim/node24.html
+
+template <typename Scalar, typename Implementation>
+struct WeightFunction
+{
+  using VectorX = Eigen::Matrix<Scalar, Eigen::Dynamic, 1>;
+  static VectorX weightVectorized(const VectorX& error_vec)
+  {
+    VectorX weights(error_vec.size());
+    for(int i = 0; i < error_vec.size(); ++i)
+    {
+      weights(i) = Implementation::weight(error_vec(i));
+    }
+    return weights;
+  }
+
+  static real_t weight(const real_t error)
+  {
+    return Implementation::weight(error);
+  }
+};
+
+template <typename Scalar>
+struct UnitWeightFunction : public WeightFunction<Scalar, UnitWeightFunction<Scalar>>
+{
+  static constexpr Scalar weight(const Scalar& /*normed_error*/)
+  {
+    return Scalar{1.0};
+  }
+};
+
+template <typename Scalar>
+struct TukeyWeightFunction : public WeightFunction<Scalar, TukeyWeightFunction<Scalar>>
+{
+  static constexpr Scalar b_square = 4.6851 * 4.6851;
+  static Scalar weight(const Scalar& error)
+  {
+    const Scalar x_square = error * error;
+    if(x_square <= b_square)
+    {
+      const Scalar tmp = Scalar{1.0} - x_square / b_square;
+      return tmp * tmp;
+    }
+    else
+    {
+      return Scalar{0.0};
+    }
+  }
+};
+
+template <typename Scalar>
+struct HuberWeightFunction : public WeightFunction<Scalar, HuberWeightFunction<Scalar>>
+{
+  static constexpr Scalar k = 1.345;
+  static Scalar weight(const Scalar& normed_error)
+  {
+    const Scalar abs_error = std::abs(normed_error);
+    return (abs_error < k) ? Scalar{1.0} : k / abs_error;
+  }
+};
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_geometry/include/ze/geometry/triangulation.hpp b/RWR/src/ze_oss/ze_geometry/include/ze/geometry/triangulation.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..261e46116313724ee5b43e8a8feae1dcb9a05537
--- /dev/null
+++ b/RWR/src/ze_oss/ze_geometry/include/ze/geometry/triangulation.hpp
@@ -0,0 +1,86 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#pragma once
+
+#include <tuple>
+
+#include <ze/common/types.hpp>
+#include <ze/common/transformation.hpp>
+
+namespace ze {
+
+//! Return depth in reference frame.
+inline std::pair<real_t, bool> depthFromTriangulation(
+    const Transformation& T_cur_ref,
+    const Eigen::Ref<const Bearing>& f_ref,
+    const Eigen::Ref<const Bearing>& f_cur)
+{
+  Matrix32 A;
+  A << T_cur_ref.getRotation().rotate(f_ref), f_cur;
+  Matrix2 AtA = A.transpose() * A;
+  if(AtA.determinant() < 0.000001)
+  {
+    return std::make_pair(0.0, false);
+  }
+  Vector2 depths = - AtA.inverse() * A.transpose() * T_cur_ref.getPosition();
+  return std::make_pair(std::abs(depths(0)), true);
+}
+
+//! Compute the position of a 3D point seen from two viewpoints. Fast
+//! non-linear approximation (closed-form), as in Davide's book.
+//! The resulting point is expressed in coordinate frame A.
+Position triangulateNonLinear(
+    const Transformation& T_A_B,
+    const Eigen::Ref<const Bearing>& f_A,
+    const Eigen::Ref<const Bearing>& f_B);
+
+//! Triangulate multiple 3d points using triangulateNonLinear and compute
+//! the corresponding reprojection errors.
+void triangulateManyAndComputeAngularErrors(
+    const Transformation& T_A_B,
+    const Bearings& f_A_vec,
+    const Bearings& f_B_vec,
+    Positions& p_A,
+    VectorX& reprojection_erors);
+
+//! DLT triangulation [Hartley and Zisserman, 2nd edition, p. 312].
+//! @param T_C_W vector of camera poses (camera in world coordinates).
+//! @param f_C bearing vectors in camera frame.
+//! @param rank_tol SVD rank tolerance.
+//! @return Triangulated point, in homogeneous coordinates.
+//! @return Success.
+std::pair<Vector4, bool> triangulateHomogeneousDLT(
+    const TransformationVector& T_C_W,
+    const Bearings& p_C,
+    const real_t rank_tol = 1e-9);
+
+//! Non-linear least squares refinement of the triangulation using Gauss-Newton.
+void triangulateGaussNewton(
+    const TransformationVector& T_C_W,
+    const Bearings& p_C,
+    Eigen::Ref<Position> p_W);
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_geometry/package.xml b/RWR/src/ze_oss/ze_geometry/package.xml
new file mode 100644
index 0000000000000000000000000000000000000000..12049bb09037f1139cfd78426db1a66da41e51f1
--- /dev/null
+++ b/RWR/src/ze_oss/ze_geometry/package.xml
@@ -0,0 +1,22 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>ze_geometry</name>
+  <version>0.1.4</version>
+  <description>The ze_geometry package</description>
+
+  <maintainer email="christian.forster@WyssZurich.ch">Christian Forster</maintainer>
+  <license>ZE</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <buildtool_depend>catkin_simple</buildtool_depend>
+
+  <depend>eigen_catkin</depend>
+  <depend>glog_catkin</depend>
+  <depend>minkindr</depend>
+  <depend>opengv</depend>
+  <depend>ze_cmake</depend>
+  <depend>ze_common</depend>
+  <depend>ze_cameras</depend>
+
+  <test_depend>gtest</test_depend>
+</package>
diff --git a/RWR/src/ze_oss/ze_geometry/src/align_points.cpp b/RWR/src/ze_oss/ze_geometry/src/align_points.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..59ba2eed0ee42dcb76ce724dde3c389538dd62fd
--- /dev/null
+++ b/RWR/src/ze_oss/ze_geometry/src/align_points.cpp
@@ -0,0 +1,175 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include <ze/geometry/align_points.hpp>
+
+#include <cmath>
+#include <numeric>
+#include <algorithm>
+#include <ze/common/logging.hpp>
+
+#include <ze/common/matrix.hpp>
+#include <ze/common/stl_utils.hpp>
+
+namespace ze {
+
+PointAligner::PointAligner(
+    const Positions& p_A,
+    const Positions& p_B)
+  : p_A_(p_A)
+  , p_B_(p_B)
+{
+  CHECK_EQ(p_A_.cols(), p_B_.cols());
+}
+
+// -----------------------------------------------------------------------------
+double PointAligner::evaluateError(
+    const Transformation& T_A_B,
+    HessianMatrix* H,
+    GradientVector* g)
+{
+  double chi2 = 0.0;
+
+  // Compute prediction error.
+  Positions residuals = p_A_ - T_A_B.transformVectorized(p_B_);
+  VectorX residuals_norm = residuals.colwise().norm();
+
+  // At the first iteration, compute the scale of the error.
+  if(iter_ == 0)
+  {
+    measurement_sigma_ = ScaleEstimator::compute(residuals_norm);
+    VLOG(1) << "measurement sigma = " << measurement_sigma_;
+  }
+
+  // Robust cost function.
+  VectorX weights =
+      WeightFunction::weightVectorized(residuals_norm / measurement_sigma_);
+
+  // Whiten error.
+  residuals_norm /= measurement_sigma_;
+
+  if(H && g)
+  {
+    for(int i = 0; i < p_A_.cols(); ++i)
+    {
+      // Compute Jacobian (if necessary, this can be optimized a lot).
+      Matrix36 J = dPointdistance_dRelpose(T_A_B, p_A_.col(i), p_B_.col(i));
+
+      // Whiten Jacobian.
+      J /= measurement_sigma_;
+
+      // Compute Hessian and Gradient Vector.
+      H->noalias() += J.transpose() * J * weights(i);
+      g->noalias() -= J.transpose() * residuals.col(i) * weights(i);
+    }
+  }
+
+  // Compute log-likelihood : 1/(2*sigma^2)*(z-h(x))^2 = 1/2*e'R'*R*e
+  chi2 += 0.5 * weights.dot(residuals.colwise().squaredNorm());
+
+  return chi2;
+}
+
+// -----------------------------------------------------------------------------
+Transformation alignSE3(const Positions& pts_A, const Positions& pts_B)
+{
+  CHECK_GE(pts_A.cols(), 0u);
+  CHECK_EQ(pts_A.cols(), pts_B.cols());
+
+  // Compute T_B_A using the method by K. S. Arun et al.:
+  // Least-Squares Fitting of Two 3-D Point Sets
+  // IEEE Trans. Pattern Anal. Mach. Intell., 9, NO. 5, SEPTEMBER 1987
+  const Position mean_pts_A = pts_A.rowwise().mean();
+  const Position mean_pts_B = pts_B.rowwise().mean();
+  const Positions zero_mean_pts_A = pts_A.colwise() - mean_pts_A;
+  const Positions zero_mean_pts_B = pts_B.colwise() - mean_pts_B;
+  const Matrix3 Sigma_AB =
+      zero_mean_pts_B * zero_mean_pts_A.transpose() / pts_A.cols();
+
+  Eigen::JacobiSVD<Matrix3> svd(
+        Sigma_AB, Eigen::ComputeFullU | Eigen::ComputeFullV);
+  const int rank_Sigma_AB = svd.rank();
+  CHECK_GE(rank_Sigma_AB, 2);
+  const Matrix3 svd_U = svd.matrixU();
+  const Matrix3 svd_V = svd.matrixV();
+  Matrix3 S = Matrix3::Identity();
+  if ((rank_Sigma_AB == 3 &&
+       Sigma_AB.determinant() < 0.0) ||
+      (rank_Sigma_AB == 2 &&
+       svd_U.determinant() * svd_V.determinant() < 0.0))
+  {
+    S(2, 2) = -1.0;
+  }
+
+  const Matrix3 R_B_A = svd_U * S * svd_V.transpose();
+  const Position t_B_A = mean_pts_B - R_B_A * mean_pts_A;
+  return Transformation(t_B_A, Quaternion(R_B_A));
+}
+
+// -----------------------------------------------------------------------------
+std::pair<real_t, Transformation> alignSim3(
+    const Positions& pts_A, const Positions& pts_B)
+{
+  CHECK_NE(pts_A.cols(), 0u);
+  CHECK_EQ(pts_A.cols(), pts_B.cols());
+
+  // Compute (scale, T_B_A) using the method by
+  // S. Umeyama: Least-Squares Estimation
+  // of Transformation Parameters Between Two Point Patterns,
+  // IEEE Trans. Pattern Anal. Mach. Intell., vol. 13, no. 4, 1991.
+  const real_t n = static_cast<real_t>(pts_A.cols());
+  const Position mean_pts_A = pts_A.rowwise().mean();
+  const Position mean_pts_B = pts_B.rowwise().mean();
+  const Positions zero_mean_pts_A = pts_A.colwise() - mean_pts_A;
+  const Positions zero_mean_pts_B = pts_B.colwise() - mean_pts_B;
+  const Matrix3 Sigma_AB =
+      zero_mean_pts_B * zero_mean_pts_A.transpose() / n;
+  const real_t sigma_sq_A
+      = zero_mean_pts_A.rowwise().squaredNorm().sum() / n;
+
+  Eigen::JacobiSVD<Matrix3> svd(
+        Sigma_AB, Eigen::ComputeFullU | Eigen::ComputeFullV);
+  const int rank_Sigma_AB = svd.rank();
+  CHECK_GE(rank_Sigma_AB, 2);
+  const Matrix3 svd_U = svd.matrixU();
+  const Matrix3 svd_V = svd.matrixV();
+  const Matrix3 svd_D = svd.singularValues().asDiagonal();
+  Matrix3 S = Matrix3::Identity();
+  if ((rank_Sigma_AB == 3 &&
+       Sigma_AB.determinant() < 0.0) ||
+      (rank_Sigma_AB == 2 &&
+       svd_U.determinant() * svd_V.determinant() < 0.0))
+  {
+    S(2, 2) = -1.0;
+  }
+
+  const Matrix3 R_B_A = svd_U * S * svd_V.transpose();
+  const real_t c = (svd_D * S).trace() / sigma_sq_A;
+  const Vector3 t_B_A = mean_pts_B - c * R_B_A * mean_pts_A;
+  return std::pair<real_t, Transformation>(
+        c, Transformation(t_B_A, Quaternion(R_B_A)));
+}
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_geometry/src/align_poses.cpp b/RWR/src/ze_oss/ze_geometry/src/align_poses.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..213dda20eb695114f4237919fc7c94184d94be80
--- /dev/null
+++ b/RWR/src/ze_oss/ze_geometry/src/align_poses.cpp
@@ -0,0 +1,109 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include <ze/geometry/align_poses.hpp>
+
+#include <cmath>
+#include <numeric>
+#include <algorithm>
+#include <ze/common/logging.hpp>
+
+#include <ze/common/matrix.hpp>
+#include <ze/common/stl_utils.hpp>
+
+namespace ze {
+
+PoseAligner::PoseAligner(
+    const TransformationVector& T_W_A,
+    const TransformationVector& T_W_B,
+    const real_t measurement_sigma_pos,
+    const real_t measurement_sigma_rot)
+  : T_W_A_(T_W_A)
+  , T_W_B_(T_W_B)
+  , measurement_sigma_pos_(measurement_sigma_pos)
+  , measurement_sigma_rot_(measurement_sigma_rot)
+{
+  CHECK_EQ(T_W_A_.size(), T_W_B_.size());
+}
+
+double PoseAligner::evaluateError(
+    const Transformation& T_A0_B0,
+    HessianMatrix* H,
+    GradientVector* g)
+{
+  double chi2 = 0.0;
+
+  // Compute prediction error.
+  Matrix6X residuals(6, T_W_A_.size());
+
+
+  for (size_t i = 0; i < T_W_A_.size(); ++i)
+  {
+    Transformation T_A0_Ai = T_W_A_[0].inverse() * T_W_A_[i];
+    Transformation T_Bi_A0 = T_W_B_[i].inverse() * T_W_A_[0];
+    Transformation T_Bi_Ai = T_Bi_A0 * T_A0_B0 * T_A0_Ai;
+    residuals.col(i) = T_Bi_Ai.log();
+  }
+
+  // Whiten the error.
+  residuals.topRows<3>()    /= measurement_sigma_pos_;
+  residuals.bottomRows<3>() /= measurement_sigma_rot_;
+
+  // Robust cost function.
+  VectorX residuals_norm = residuals.colwise().norm();
+  VectorX weights = WeightFunction::weightVectorized(residuals_norm);
+
+  if (H && g)
+  {
+    for (size_t i = 0; i < T_W_A_.size(); ++i)
+    {
+      // Compute Jacobian (if necessary, this can be optimized a lot).
+      Transformation T_A0_Ai = T_W_A_[0].inverse() * T_W_A_[i];
+      Transformation T_Bi_A0 = T_W_B_[i].inverse() * T_W_A_[0];
+      Matrix6 J = dRelpose_dTransformation(T_A0_B0, T_Bi_A0, T_A0_Ai);
+
+      // Compute square-root of inverse covariance:
+      Matrix6 R =
+          (Vector6() << Vector3::Ones() / measurement_sigma_pos_,
+                        Vector3::Ones() / measurement_sigma_rot_).finished().asDiagonal();
+
+      // Whiten Jacobian.
+      J *= R;
+
+      // Compute Hessian and Gradient Vector.
+      H->noalias() += J.transpose() * J * weights(i);
+      g->noalias() -= J.transpose() * residuals.col(i) * weights(i);
+    }
+  }
+
+  // Compute log-likelihood : 1/(2*sigma^2)*(z-h(x))^2 = 1/2*e'R'*R*e
+  chi2 += 0.5 * weights.dot(residuals.colwise().squaredNorm());
+
+  return chi2;
+}
+
+} // namespace ze
+
+
diff --git a/RWR/src/ze_oss/ze_geometry/src/clam.cpp b/RWR/src/ze_oss/ze_geometry/src/clam.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f1f55ba49f279835602d609520339365aa05769c
--- /dev/null
+++ b/RWR/src/ze_oss/ze_geometry/src/clam.cpp
@@ -0,0 +1,179 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include <ze/geometry/clam.hpp>
+#include <ze/geometry/pose_optimizer.hpp>
+#include <ze/geometry/pose_prior.hpp>
+
+namespace ze {
+
+Clam::Clam(
+    const ClamLandmarks& landmarks,
+    const std::vector<ClamFrameData>& data,
+    const CameraRig& rig,
+    const Transformation& T_Bc_Br_prior,
+    const real_t prior_weight_pos,
+    const real_t prior_weight_rot)
+  : landmarks_(landmarks)
+  , data_(data)
+  , rig_(rig)
+  , T_Bc_Br_prior_(T_Bc_Br_prior)
+  , prior_weight_pos_(prior_weight_pos)
+  , prior_weight_rot_(prior_weight_rot)
+{
+  measurement_sigma_localization_.resize(data.size());
+  CHECK_EQ(landmarks_.f_Br.cols(), landmarks_.origin_Br.cols());
+}
+
+real_t Clam::evaluateError(
+    const ClamState& state, HessianMatrix* H, GradientVector* g)
+{
+  CHECK_EQ(data_.size(), measurement_sigma_localization_.size());
+  real_t chi2 = 0.0;
+
+  const Transformation& T_Bc_Br = state.at<0>();
+  const VectorX& inv_depth = state.at<1>();
+
+  // ---------------------------------------------------------------------------
+  // Localization
+
+  // Loop over all cameras in rig.
+  for (size_t i = 0; i < data_.size(); ++i)
+  {
+    const ClamFrameData& data = data_[i];
+    real_t& measurement_sigma = measurement_sigma_localization_[i];
+
+    // Continue if we have no landmarks to localize with.
+    if(data.p_Br.cols() == 0)
+    {
+      VLOG(300) << "Cam " << i << " has no landmarks to localize.";
+      continue;
+    }
+
+    // Transform points from reference coordinates to camera coordinates.
+    const Transformation T_C_Br = data.T_C_B * T_Bc_Br; //! @todo(cfo): use inverse-depth coordinates!
+    const Positions p_C = T_C_Br.transformVectorized(data.p_Br);
+
+    // Normalize points to obtain estimated bearing vectors.
+    Bearings f_est = p_C;
+    normalizeBearings(f_est);
+
+    // Compute difference between bearing vectors.
+    Bearings f_err = f_est - data.f_C;
+    const VectorX f_err_norm = f_err.colwise().norm();
+
+    // At the first iteration, compute the scale of the error.
+    if(iter_ == 0)
+    {
+      measurement_sigma = ScaleEstimator::compute(f_err_norm);
+    }
+
+    // Robust cost function.
+    const VectorX weights =
+        WeightFunction::weightVectorized(f_err_norm / measurement_sigma);
+
+    // Whiten error.
+    f_err /= measurement_sigma;
+
+    if (H && g)
+    {
+      const Matrix3 R_C_Br = T_C_Br.getRotationMatrix();
+      const int n = data.f_C.cols();
+      Matrix36 G;
+      G.block<3,3>(0,0) = I_3x3;
+      for (int i = 0; i < n; ++i)
+      {
+        // Jacobian computation.
+        G.block<3,3>(0,3) = - skewSymmetric(data.p_Br.col(i));
+        Matrix3 J_normalization = dBearing_dLandmark(p_C.col(i));
+        Matrix36 J = J_normalization * R_C_Br * G;
+
+        // Whiten Jacobian.
+        J /= measurement_sigma;
+
+        // Compute Hessian and Gradient Vector.
+        H->topLeftCorner<6,6>().noalias() += J.transpose() * J * weights(i);
+        g->head<6>().noalias() -= J.transpose() * f_err.col(i) * weights(i);
+      }
+    }
+
+    // Compute log-likelihood : 1/(2*sigma^2)*(z-h(x))^2 = 1/2*e'R'*R*e
+    chi2 += 0.5 * weights.dot(f_err.colwise().squaredNorm());
+  }
+
+  // ---------------------------------------------------------------------------
+  // Mapping
+
+  //! @todo(cfo): This can be optimized a lot!
+  for (size_t i = 0; i < data_.size(); ++i)
+  {
+    const ClamFrameData& data = data_[i];
+    const Camera& cam = rig_.at(i);
+    for (const std::pair<uint32_t, Keypoint>& m : data.landmark_measurements)
+    {
+      Matrix26 H1;
+      Matrix21 H2;
+      CHECK_LT(m.first, landmarks_.f_Br.cols());
+      CHECK_LT(m.first, inv_depth.size());
+      Vector2 err = reprojectionResidual(
+            landmarks_.f_Br.col(m.first), landmarks_.origin_Br.col(m.first),
+            cam, data.T_C_B, T_Bc_Br, inv_depth(m.first), m.second, &H1, &H2);
+
+      // Robust cost function.
+      const real_t weight = 1.0; //!< @todo(cfo)
+
+      // Whiten error
+      err /= measurement_sigma_mapping_;
+
+      Matrix2X J(2, g->size());
+      J.setZero();
+      J.block<2,6>(0, 0) = H1;
+      J.block<2,1>(0, 6 + m.first) = H2;
+
+      // Whiten Jacobian.
+      J /= measurement_sigma_mapping_;
+
+      // Compute Hessian and Gradient Vector.
+      H->noalias() += J.transpose() * J * weight;
+      g->noalias() -= J.transpose() * err * weight;
+
+      // Compute log-likelihood : 1/(2*sigma^2)*(z-h(x))^2 = 1/2*e'R'*R*e
+      chi2 += 0.5 * weight * err.squaredNorm();
+    }
+  }
+
+  // ---------------------------------------------------------------------------
+  // Prior
+  if (prior_weight_rot_ > 0.0f || prior_weight_pos_ > 0.0f)
+  {
+    applyPosePrior(
+          T_Bc_Br, T_Bc_Br_prior_, prior_weight_rot_, prior_weight_pos_,
+          H->block<6,6>(0,0), g->segment<6>(0));
+  }
+
+  return chi2;
+}
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_geometry/src/line.cpp b/RWR/src/ze_oss/ze_geometry/src/line.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..3014f6cfb9a40e980843f1bae2d493794259df3f
--- /dev/null
+++ b/RWR/src/ze_oss/ze_geometry/src/line.cpp
@@ -0,0 +1,124 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include <ze/geometry/line.hpp>
+
+#include <ze/cameras/camera_utils.hpp>
+
+namespace ze {
+
+// -----------------------------------------------------------------------------
+Vector2 Line::calculateMeasurementError(const Vector3& measurement_W,
+                                        const Vector3& camera_position_W) const
+{
+  const Vector3 anchor_to_camera = camera_position_W - anchorPoint();
+  DEBUG_CHECK_GT(anchor_to_camera.norm(), 0.0);
+  return Vector2(measurement_W.dot(direction()),
+                 measurement_W.dot(anchor_to_camera) / anchor_to_camera.norm());
+}
+
+// -----------------------------------------------------------------------------
+Matrix26 dLineMeasurement_dPose(const Transformation& T_B_W,
+                                const Transformation& T_C_B,
+                                const LineMeasurement& measurement_W,
+                                const Position& line_anchor,
+                                const Vector3& line_direction)
+{
+  //! @todo Can be optimized.
+  const Transformation T_C_W = T_C_B * T_B_W;
+  const Matrix13 measurement_W_transpose = measurement_W.transpose();
+  const Vector3 anchor_to_cam = T_C_W.inverse().getPosition() - line_anchor;
+  DEBUG_CHECK_NE(anchor_to_cam.norm(), 0.0);
+  const real_t inverse_distance = 1.0 / anchor_to_cam.norm();
+  const Vector3 anchor_to_cam_normalized = anchor_to_cam * inverse_distance;
+  const Matrix3 d_anchor_to_cam_normalized_d_campos =
+      inverse_distance * (Matrix3::Identity() -
+                          anchor_to_cam_normalized *
+                          anchor_to_cam_normalized.transpose());
+
+  Matrix26 J;
+  J.block<1, 3>(0, 0).setZero();
+  J.block<1, 3>(0, 3) =
+      -measurement_W_transpose * skewSymmetric(line_direction);
+
+  J.block<1, 3>(1, 0) =
+      -measurement_W_transpose * d_anchor_to_cam_normalized_d_campos;
+  J.block<1, 3>(1, 3) =
+      -measurement_W_transpose * (
+        d_anchor_to_cam_normalized_d_campos *
+        skewSymmetric(T_C_W.getRotation().inverse().rotate(T_C_B.getPosition()) +
+         T_B_W.getRotation().inverse().rotate(T_B_W.getPosition()))
+        + skewSymmetric(anchor_to_cam_normalized));
+
+  return J;
+}
+
+// -----------------------------------------------------------------------------
+std::pair<Positions, Positions> generateRandomVisibleLines(
+    const Camera& cam, const Transformation& T_W_C,
+    size_t num_lines, Lines& lines_W)
+{
+  auto visible_points_C =
+      generateRandomVisible3dPoints(cam, num_lines * 2, 10, 1.0, 20.0);
+
+  Positions start_W =
+      T_W_C.transformVectorized(
+        std::get<2>(visible_points_C).topLeftCorner(3, num_lines));
+  Positions end_W =
+      T_W_C.transformVectorized(
+        std::get<2>(visible_points_C).topRightCorner(3, num_lines));
+
+  lines_W = generateLinesFromEndpoints(start_W, end_W);
+
+  return std::make_pair(start_W, end_W);
+}
+
+// -----------------------------------------------------------------------------
+Lines generateLinesFromEndpoints(const Positions& startpoints,
+                                const Positions& endpoints)
+{
+  CHECK_EQ(startpoints.cols(), endpoints.cols());
+  const size_t n = startpoints.cols();
+  Lines lines;
+  lines.reserve(n);
+  for (size_t i = 0; i < n; ++i)
+  {
+    const Vector3& start = startpoints.col(i);
+    const Vector3& end = endpoints.col(i);
+    Vector3 direction = (end - start).normalized();
+    const double anchor_point_distance = (start.squaredNorm() - start.dot(end)) /
+                                         (end - start).norm();
+    const Vector3 anchor_point = start + anchor_point_distance * direction;
+    const Vector3 anchor_point_direction = anchor_point.normalized();
+
+    Matrix3 rotation;
+    rotation << direction, anchor_point_direction.cross(direction), anchor_point_direction;
+    const Quaternion orientation(rotation);
+    lines.emplace_back(orientation, anchor_point.norm());
+  }
+  return lines;
+}
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_geometry/src/pose_optimizer.cpp b/RWR/src/ze_oss/ze_geometry/src/pose_optimizer.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b35e3baa2e4c0fbafa0cbf8d3f96e76976ed6b76
--- /dev/null
+++ b/RWR/src/ze_oss/ze_geometry/src/pose_optimizer.cpp
@@ -0,0 +1,369 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include <ze/geometry/pose_optimizer.hpp>
+
+#include <cmath>
+#include <numeric>
+#include <algorithm>
+
+#include <ze/cameras/camera.hpp>
+#include <ze/common/logging.hpp>
+#include <ze/common/matrix.hpp>
+#include <ze/common/stl_utils.hpp>
+#include <ze/geometry/pose_prior.hpp>
+
+namespace ze {
+
+//------------------------------------------------------------------------------
+PoseOptimizer::PoseOptimizer(
+    const LeastSquaresSolverOptions& options,
+    std::vector<PoseOptimizerFrameData>& data)
+  : LeastSquaresSolver<Transformation, PoseOptimizer>(options)
+  , data_(data)
+{
+  checkData();
+}
+
+//------------------------------------------------------------------------------
+PoseOptimizer::PoseOptimizer(
+    const LeastSquaresSolverOptions& options,
+    std::vector<PoseOptimizerFrameData>& data,
+    const Transformation& T_B_W_prior,
+    const real_t prior_weight_pos,
+    const real_t prior_weight_rot)
+  : LeastSquaresSolver<Transformation, PoseOptimizer>(options)
+  , data_(data)
+  , T_B_W_prior_(T_B_W_prior)
+  , prior_weight_pos_(prior_weight_pos)
+  , prior_weight_rot_(prior_weight_rot)
+{
+  checkData();
+}
+
+//------------------------------------------------------------------------------
+LeastSquaresSolverOptions PoseOptimizer::getDefaultSolverOptions()
+{
+  LeastSquaresSolverOptions options;
+  options.strategy = SolverStrategy::GaussNewton;
+  options.max_iter = 10;
+  options.eps = 0.000001;
+  return options;
+}
+
+void PoseOptimizer::checkData() const
+{
+  DEBUG_CHECK(!data_.empty());
+  for (auto& data_entry : data_)
+  {
+    DEBUG_CHECK_EQ(data_entry.f.cols(), data_entry.kp_idx.size());
+  }
+}
+
+//------------------------------------------------------------------------------
+void PoseOptimizer::setPrior(
+    const Transformation& T_B_W_prior,
+    const real_t prior_weight_pos,
+    const real_t prior_weight_rot)
+{
+  T_B_W_prior_ = T_B_W_prior;
+  prior_weight_pos_ = prior_weight_pos;
+  prior_weight_rot_ = prior_weight_rot;
+}
+
+//------------------------------------------------------------------------------
+real_t PoseOptimizer::evaluateError(
+    const Transformation& T_B_W, HessianMatrix* H, GradientVector* g)
+{
+  real_t chi2 = real_t{0.0};
+
+  // Loop over all cameras in rig.
+  VLOG(400) << "Num residual blocks = " << data_.size();
+  for (auto& residual_block : data_)
+  {
+    VLOG(400) << "Process residual block " << residual_block.camera_idx;
+    if (residual_block.kp_idx.size() == 0 && residual_block.lines_W.empty())
+    {
+      VLOG(40) << "Residual block has no measurements.";
+      continue;
+    }
+
+    switch (residual_block.type)
+    {
+      case PoseOptimizerResidualType::Bearing:
+        chi2 += evaluateBearingErrors(T_B_W, iter_ == 0, residual_block, H, g).first;
+        break;
+      case PoseOptimizerResidualType::UnitPlane:
+        chi2 += evaluateUnitPlaneErrors(T_B_W, iter_ == 0, residual_block, H, g).first;
+        break;
+      case PoseOptimizerResidualType::Line:
+        chi2 += evaluateLineErrors(T_B_W, iter_ == 0, residual_block, H, g).first;
+        break;
+      default:
+        LOG(FATAL) << "Residual type not implemented.";
+        break;
+    }
+  }
+
+  // Apply prior.
+  if (prior_weight_rot_ > real_t{0.0} || prior_weight_pos_ > real_t{0.0})
+  {
+    applyPosePrior(T_B_W, T_B_W_prior_, prior_weight_rot_, prior_weight_pos_, *H, *g);
+  }
+
+  return chi2;
+}
+
+//------------------------------------------------------------------------------
+std::pair<real_t, VectorX> evaluateBearingErrors(
+    const Transformation& T_B_W,
+    const bool first_iteration,
+    PoseOptimizerFrameData& data,
+    PoseOptimizer::HessianMatrix* H,
+    PoseOptimizer::GradientVector* g)
+{
+  // Transform points from world coordinates to camera coordinates.
+  const Transformation T_C_W = data.T_C_B * T_B_W;
+  Positions p_C = T_C_W.transformVectorized(data.p_W);
+
+  // Normalize points to obtain estimated bearing vectors.
+  Bearings f_est = p_C;
+  normalizeBearings(f_est);
+
+  // Compute difference between bearing vectors.
+  Bearings f_err = f_est - data.f;
+  VectorX f_err_norm = f_err.colwise().norm();
+
+  // Account that features at higher levels have higher uncertainty.
+  f_err_norm.array() /= data.scale.array();
+
+  // At the first iteration, compute the scale of the error.
+  if (first_iteration)
+  {
+    data.measurement_sigma = PoseOptimizer::ScaleEstimator::compute(f_err_norm);
+  }
+
+  // Robust cost function.
+  VectorX weights = PoseOptimizer::WeightFunction::weightVectorized(
+                      f_err_norm.array() / data.measurement_sigma);
+
+  // Instead of whitening the error and the Jacobian, we apply sigma to the weights:
+  weights.array() /= (data.scale.array() * data.measurement_sigma * data.measurement_sigma);
+
+  if (H && g)
+  {
+    const Matrix3 R_C_W = T_C_W.getRotationMatrix();
+    const int n = data.f.cols();
+    Matrix36 G;
+    G.block<3,3>(0,0) = I_3x3;
+    for (int i = 0; i < n; ++i)
+    {
+      // Jacobian computation.
+      G.block<3,3>(0,3) = -skewSymmetric(data.p_W.col(i));
+      Matrix3 J_normalization = dBearing_dLandmark(p_C.col(i));
+      Matrix36 J = J_normalization * R_C_W * G;
+
+      // Compute Hessian and Gradient Vector.
+      H->noalias() += J.transpose() * J * weights(i);
+      g->noalias() -= J.transpose() * f_err.col(i) * weights(i);
+    }
+  }
+
+  // Compute log-likelihood : 1/(2*sigma^2)*(z-h(x))^2 = 1/2*e'R'*R*e
+  return std::make_pair(real_t{0.5} * weights.dot(f_err.colwise().squaredNorm()),
+                        f_err_norm);
+}
+
+//------------------------------------------------------------------------------
+std::pair<real_t, VectorX> evaluateUnitPlaneErrors(
+    const Transformation& T_B_W,
+    const bool first_iteration,
+    PoseOptimizerFrameData& data,
+    PoseOptimizer::HessianMatrix* H,
+    PoseOptimizer::GradientVector* g)
+{
+  if (first_iteration)
+  {
+    data.uv = project2Vectorized(data.f);
+  }
+
+  // Transform points from world coordinates to camera coordinates.
+  const Transformation T_C_W = data.T_C_B * T_B_W;
+  Positions p_C = T_C_W.transformVectorized(data.p_W);
+
+  // Compute difference on unit plane.
+  Keypoints uv_err = project2Vectorized(p_C) - data.uv;
+  VectorX uv_err_norm = uv_err.colwise().norm();
+
+  // Account that features at higher levels have higher uncertainty.
+  uv_err_norm.array() /= data.scale.array();
+
+  // At the first iteration, compute the scale of the error.
+  if (first_iteration)
+  {
+    data.measurement_sigma =
+        PoseOptimizer::ScaleEstimator::compute(uv_err_norm);
+  }
+
+  // Robust cost function.
+  VectorX weights = PoseOptimizer::WeightFunction::weightVectorized(
+                      uv_err_norm.array() / data.measurement_sigma);
+
+  // Instead of whitening the error and the Jacobian, we apply sigma to the weights:
+  weights.array() /= (data.scale.array() * data.measurement_sigma * data.measurement_sigma);
+
+  if (H && g)
+  {
+    const Matrix3 R_C_W = T_C_W.getRotationMatrix();
+    const int n = data.f.cols();
+    Matrix36 G;
+    G.block<3,3>(0,0) = I_3x3;
+    for (int i = 0; i < n; ++i)
+    {
+      // Jacobian computation.
+      G.block<3,3>(0,3) = -skewSymmetric(data.p_W.col(i));
+      Matrix23 J_proj = dUv_dLandmark(p_C.col(i));
+      Matrix26 J = J_proj * R_C_W * G;
+
+      // Compute Hessian and Gradient Vector.
+      H->noalias() += J.transpose() * J * weights(i);
+      g->noalias() -= J.transpose() * uv_err.col(i) * weights(i);
+    }
+  }
+
+  // Compute log-likelihood : 1/(2*sigma^2)*(z-h(x))^2 = 1/2*e'R'*R*e
+  return std::make_pair(real_t{0.5} * weights.dot(uv_err.colwise().squaredNorm()),
+                        uv_err_norm);
+}
+
+//------------------------------------------------------------------------------
+std::pair<real_t, VectorX> evaluateLineErrors(
+    const Transformation& T_B_W,
+    const bool first_iteration,
+    PoseOptimizerFrameData& data,
+    PoseOptimizer::HessianMatrix* H,
+    PoseOptimizer::GradientVector* g)
+{
+  const Transformation T_C_W = data.T_C_B * T_B_W;
+  const Matrix3 R_C_W = T_C_W.getRotationMatrix();
+  const Vector3 camera_pos_W = T_C_W.inverse().getPosition();
+  // Compute error.
+  const Matrix3X line_measurements_W = R_C_W.transpose() * data.line_measurements_C;
+  const size_t n = data.line_measurements_C.cols();
+  Matrix2X error(2, n);
+  for (size_t i = 0; i < n; ++i)
+  {
+    error.col(i) = data.lines_W[i].calculateMeasurementError(line_measurements_W.col(i),
+                                                             camera_pos_W);
+  }
+  VectorX error_norm = error.colwise().norm();
+
+  // At the first iteration, compute the scale of the error.
+  if (first_iteration)
+  {
+    data.measurement_sigma = PoseOptimizer::ScaleEstimator::compute(error_norm);
+  }
+
+  // Robust cost function.
+  VectorX weights(n);
+  weights.setOnes();
+
+  // Instead of whitening the error and the Jacobian, we apply sigma to the weights:
+  // weights.array() /= (data.measurement_sigma * data.measurement_sigma);
+
+  if (H && g)
+  {
+    for (size_t i = 0; i < n; ++i)
+    {
+      // Jacobian computation.
+      Matrix26 J = dLineMeasurement_dPose(T_B_W, data.T_C_B,
+                                          line_measurements_W.col(i),
+                                          data.lines_W[i].anchorPoint(),
+                                          data.lines_W[i].direction());
+
+      // Compute Hessian and Gradient Vector.
+      H->noalias() += J.transpose() * J * weights(i);
+      g->noalias() -= J.transpose() * error.col(i) * weights(i);
+    }
+  }
+
+  return std::make_pair(real_t{0.5} * weights.dot(error.colwise().squaredNorm()),
+                        error_norm);
+}
+
+//------------------------------------------------------------------------------
+std::vector<KeypointIndex> getOutlierIndices(
+    PoseOptimizerFrameData& data,
+    const Camera& cam,
+    const Transformation& T_B_W,
+    const real_t pixel_threshold)
+{
+  if (data.kp_idx.size() == 0)
+  {
+    VLOG(40) << "Residual block has no measurements.";
+    return {};
+  }
+
+  real_t chi2;
+  VectorX err_norm_vec;
+  real_t error_multiplier = 1.0;
+  real_t threshold = pixel_threshold; //! @todo: multiple thresholds for multiple residual blocks!
+  switch (data.type)
+  {
+    case PoseOptimizerResidualType::Bearing:
+      std::tie(chi2, err_norm_vec) =
+          evaluateBearingErrors(T_B_W, false, data, nullptr, nullptr);
+      //! @todo: check Zichao's threshold.
+      threshold =
+          std::abs(2.0 * std::sin(0.5*cam.getApproxBearingAngleFromPixelDifference(pixel_threshold)));
+      break;
+    case PoseOptimizerResidualType::UnitPlane:
+      std::tie(chi2, err_norm_vec) =
+          evaluateUnitPlaneErrors(T_B_W, false, data, nullptr, nullptr);
+      error_multiplier = 1.0 / std::abs(cam.projectionParameters()(0));
+      threshold = pixel_threshold * error_multiplier;
+      break;
+    default:
+      LOG(FATAL) << "Residual type not implemented.";
+      break;
+  }
+
+  std::vector<KeypointIndex> outliers;
+  outliers.reserve(err_norm_vec.size() / 2);
+  VLOG(100) << "Reproj. threshold = " << threshold;
+  for (int i = 0; i < err_norm_vec.size(); ++i)
+  {
+    if (err_norm_vec(i) > threshold)
+    {
+      VLOG(100) << "Outlier: Reproj. error = " << err_norm_vec(i) / error_multiplier << "px";
+      outliers.push_back(data.kp_idx(i));
+    }
+  }
+  return outliers;
+}
+
+} // namespace ze
+
+
diff --git a/RWR/src/ze_oss/ze_geometry/src/ransac_relative_pose.cpp b/RWR/src/ze_oss/ze_geometry/src/ransac_relative_pose.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a498061b62de8d1d2ff32fec55da38a09785f960
--- /dev/null
+++ b/RWR/src/ze_oss/ze_geometry/src/ransac_relative_pose.cpp
@@ -0,0 +1,213 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include <ze/geometry/ransac_relative_pose.hpp>
+
+#include <glog/logging.h>
+
+#include <opengv/sac/Ransac.hpp>
+#include <opengv/sac_problems/relative_pose/CentralRelativePoseSacProblem.hpp>
+#include <opengv/sac_problems/relative_pose/TranslationOnlySacProblem.hpp>
+#include <opengv/sac_problems/relative_pose/RotationOnlySacProblem.hpp>
+#include <opengv/relative_pose/methods.hpp>
+#include <opengv/relative_pose/CentralRelativeAdapter.hpp>
+#include <opengv/triangulation/methods.hpp>
+
+#include <ze/cameras/camera.hpp>
+#include <ze/common/combinatorics.hpp>
+
+namespace ze {
+
+RansacRelativePose::RansacRelativePose(
+    const Camera& cam,
+    const real_t& reprojection_threshold_px)
+  : ogv_threshold_(
+      1.0 - std::cos(cam.getApproxAnglePerPixel() * reprojection_threshold_px))
+{
+  VLOG(3) << "RANSAC THRESHOLD = " << cam.getApproxAnglePerPixel() * reprojection_threshold_px;
+}
+
+bool RansacRelativePose::solve(
+      const Bearings& f_ref,
+      const Bearings& f_cur,
+      const RelativePoseAlgorithm method,
+      Transformation& T_cur_ref)
+{
+  BearingsVector f_ref_v = bearingsVectorFromBearings(f_ref);
+  BearingsVector f_cur_v = bearingsVectorFromBearings(f_cur);
+  return solve(f_ref_v, f_cur_v, method, T_cur_ref);
+}
+
+bool RansacRelativePose::solve(
+    const BearingsVector& f_ref,
+    const BearingsVector& f_cur,
+    const RelativePoseAlgorithm method,
+    Transformation& T_cur_ref)
+{
+  CHECK_EQ(f_ref.size(), f_cur.size());
+  switch(method)
+  {
+    case RelativePoseAlgorithm::FivePoint:
+      return solveRelativePose(f_ref, f_cur, T_cur_ref);
+      break;
+    case RelativePoseAlgorithm::TwoPointTranslationOnly:
+      return solveTranslationOnly(f_ref, f_cur, T_cur_ref);
+      break;
+    case RelativePoseAlgorithm::TwoPointRotationOnly:
+      return solveRotationOnly(f_ref, f_cur, T_cur_ref);
+      break;
+    default:
+      LOG(FATAL) << "Algorithm not implemented";
+      break;
+  }
+  return false;
+}
+
+// -----------------------------------------------------------------------------
+bool RansacRelativePose::solveRelativePose(
+    const BearingsVector& f_ref,
+    const BearingsVector& f_cur,
+    Transformation& T_cur_ref)
+{
+  // Setup problem.
+  //! @todo: Unify all the repetitive code.
+  using Problem = opengv::sac_problems::relative_pose::CentralRelativePoseSacProblem;
+  using Adapter = opengv::relative_pose::CentralRelativeAdapter;
+  Adapter adapter(f_cur, f_ref);
+  boost::shared_ptr<Problem> problem(new Problem(adapter, Problem::NISTER));
+  opengv::sac::Ransac<Problem> ransac;
+  ransac.sac_model_ = problem;
+  ransac.threshold_ = ogv_threshold_;
+  ransac.max_iterations_ = ogv_max_iterations_;
+  ransac.probability_ = ogv_init_probability_;
+
+  // Solve.
+  if(!ransac.computeModel(ogv_verbosity_level_))
+  {
+    LOG(WARNING) << "5Pt RANSAC could not find a solution";
+    return false;
+  }
+  VLOG(10) << "5Pt RANSAC:"
+           << ", #iter = " << ransac.iterations_
+           << ", #inliers = " << ransac.inliers_.size();
+
+  // Process results.
+  Matrix3 R = ransac.model_coefficients_.leftCols<3>().cast<real_t>();
+  Vector3 t = ransac.model_coefficients_.rightCols<1>().cast<real_t>();
+  T_cur_ref = Transformation(Eigen::Quaternion<real_t>(R).normalized(), t);
+
+  result_probability_ = ransac.probability_;
+  num_iterations_ = ransac.iterations_;
+  inliers_ = ransac.inliers_;
+  num_measurements_ = f_ref.size();
+  return true;
+}
+
+// -----------------------------------------------------------------------------
+bool RansacRelativePose::solveTranslationOnly(
+    const BearingsVector& f_ref,
+    const BearingsVector& f_cur,
+    Transformation& T_cur_ref)
+{
+  // Setup Problem.
+  //! @todo: Unify all the repetitive code.
+  using Problem = opengv::sac_problems::relative_pose::TranslationOnlySacProblem;
+  using Adapter = opengv::relative_pose::CentralRelativeAdapter;
+  Adapter adapter(f_cur, f_ref, T_cur_ref.getRotationMatrix().cast<double>());
+  boost::shared_ptr<Problem> problem(new Problem(adapter));
+  opengv::sac::Ransac<Problem> ransac;
+  ransac.sac_model_ = problem;
+  ransac.threshold_ = ogv_threshold_;
+  ransac.max_iterations_ = ogv_max_iterations_;
+  ransac.probability_ = ogv_init_probability_;
+
+  // Solve.
+  if(!ransac.computeModel(ogv_verbosity_level_))
+  {
+    LOG(WARNING) << "2Pt RANSAC could not find a solution";
+    return false;
+  }
+  VLOG(10) << "2pt RANSAC:"
+           << ", #iter = " << ransac.iterations_
+           << ", #inliers = " << ransac.inliers_.size();
+
+  // Process results.
+  Matrix3 R = ransac.model_coefficients_.leftCols<3>().cast<real_t>();
+  Vector3 t = ransac.model_coefficients_.rightCols<1>().cast<real_t>();
+  T_cur_ref = Transformation(Eigen::Quaternion<real_t>(R).normalized(), t);
+
+  result_probability_ = ransac.probability_;
+  num_iterations_ = ransac.iterations_;
+  inliers_ = ransac.inliers_;
+  num_measurements_ = f_ref.size();
+  return true;
+}
+
+// -----------------------------------------------------------------------------
+bool RansacRelativePose::solveRotationOnly(
+    const BearingsVector& f_ref,
+    const BearingsVector& f_cur,
+    Transformation& T_cur_ref)
+{
+  // Setup Problem.
+  //! @todo: Unify all the repetitive code.
+  using Problem = opengv::sac_problems::relative_pose::RotationOnlySacProblem;
+  opengv::relative_pose::CentralRelativeAdapter adapter(f_cur, f_ref);
+  boost::shared_ptr<Problem> problem(new Problem(adapter));
+  opengv::sac::Ransac<Problem> ransac;
+  ransac.sac_model_ = problem;
+  ransac.threshold_ = ogv_threshold_;
+  ransac.max_iterations_ = ogv_max_iterations_;
+  ransac.probability_ = ogv_init_probability_;
+
+  // Solve.
+  if(!ransac.computeModel(ogv_verbosity_level_))
+  {
+    LOG(WARNING) << "2Pt RANSAC could not find a solution";
+    return false;
+  }
+  VLOG(10) << "2pt Rotation-Only RANSAC:"
+           << ", #iter = " << ransac.iterations_
+           << ", #inliers = " << ransac.inliers_.size();
+
+  // Process results.
+  Matrix3 R = ransac.model_coefficients_.leftCols<3>().cast<real_t>();
+  Vector3 t = ransac.model_coefficients_.rightCols<1>().cast<real_t>();
+  T_cur_ref = Transformation(Eigen::Quaternion<real_t>(R).normalized(), t);
+
+  result_probability_ = ransac.probability_;
+  num_iterations_ = ransac.iterations_;
+  inliers_ = ransac.inliers_;
+  num_measurements_ = f_ref.size();
+  return true;
+}
+
+// -----------------------------------------------------------------------------
+std::vector<int> RansacRelativePose::outliers()
+{
+  return getOutlierIndicesFromInlierIndices<int>(inliers_, num_measurements_);
+}
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_geometry/src/triangulation.cpp b/RWR/src/ze_oss/ze_geometry/src/triangulation.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6162465d9cf96033f6e290458509573c390b9dbc
--- /dev/null
+++ b/RWR/src/ze_oss/ze_geometry/src/triangulation.cpp
@@ -0,0 +1,186 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include <ze/geometry/triangulation.hpp>
+
+#include <algorithm>
+#include <ze/common/logging.hpp>
+#include <ze/common/matrix.hpp>
+#include <ze/common/manifold.hpp>
+#include <ze/geometry/pose_optimizer.hpp>
+
+namespace ze {
+
+//------------------------------------------------------------------------------
+Position triangulateNonLinear(
+    const Transformation& T_A_B,
+    const Eigen::Ref<const Bearing>& f_A,
+    const Eigen::Ref<const Bearing>& f_B)
+{
+  Bearing f_A_hat = T_A_B.getRotation().rotate(f_B);
+  Vector2 b(
+        T_A_B.getPosition().dot(f_A),
+        T_A_B.getPosition().dot(f_A_hat));
+  Matrix2 A;
+  A(0,0) = f_A.dot(f_A);
+  A(1,0) = f_A.dot(f_A_hat);
+  A(0,1) = -A(1,0);
+  A(1,1) = -f_A_hat.dot(f_A_hat);
+  Vector2 lambda = A.inverse() * b;
+  Position xm = lambda(0) * f_A;
+  Position xn = T_A_B.getPosition() + lambda(1) * f_A_hat;
+  Position p_A(0.5 * ( xm + xn ));
+  return p_A;
+}
+
+//------------------------------------------------------------------------------
+void triangulateManyAndComputeAngularErrors(
+    const Transformation& T_A_B,
+    const Bearings& f_A_vec,
+    const Bearings& f_B_vec,
+    Positions& p_A,
+    VectorX& reprojection_erors)
+{
+  CHECK_EQ(f_A_vec.cols(), f_B_vec.cols());
+  CHECK_EQ(f_A_vec.cols(), p_A.cols());
+  CHECK_EQ(f_A_vec.cols(), reprojection_erors.size());
+  const Transformation T_B_A = T_A_B.inverse();
+  for (int i = 0; i < f_A_vec.cols(); ++i)
+  {
+    p_A.col(i) = triangulateNonLinear(T_A_B, f_A_vec.col(i), f_B_vec.col(i));
+    Bearing f_A_predicted = p_A.col(i).normalized();
+    Bearing f_B_predicted = (T_B_A * p_A.col(i)).normalized();
+
+    // Bearing-vector based outlier criterium (select threshold accordingly):
+    // 1 - (f1' * f2) = 1 - cos(alpha) as used in OpenGV.
+    real_t reproj_error_1 = 1.0 - (f_A_vec.col(i).dot(f_A_predicted));
+    real_t reproj_error_2 = 1.0 - (f_B_vec.col(i).dot(f_B_predicted));
+    reprojection_erors(i) = reproj_error_1 + reproj_error_2;
+  }
+}
+
+//------------------------------------------------------------------------------
+std::pair<Vector4, bool> triangulateHomogeneousDLT(
+    const TransformationVector& T_C_W,
+    const Bearings& p_C,
+    const real_t rank_tol)
+{
+  // Number of observations.
+  size_t m = T_C_W.size();
+  CHECK_GE(m, 2u);
+
+  // Compute unit-plane coorinates (divide by z) from bearing vectors.
+  const Matrix2X uv = p_C.topRows<2>().array().rowwise() / p_C.bottomRows<1>().array();
+
+  // Allocate DLT matrix.
+  MatrixX4 A(m * 2, 4);
+  A.setZero();
+
+  // Fill DLT matrix.
+  for (size_t i = 0; i < m; ++i)
+  {
+    //! @todo: Think if this can be optimized, e.g. without computing the Matrix44 and uv.
+    size_t row = i * 2;
+    Matrix44 projection = T_C_W[i].getTransformationMatrix();
+    A.row(row)     = uv(0, i) * projection.row(2) - projection.row(0);
+    A.row(row + 1) = uv(1, i) * projection.row(2) - projection.row(1);
+  }
+  int rank;
+  real_t error;
+  VectorX v;
+  std::tie(rank, error, v) = directLinearTransform(A, rank_tol);
+
+  // Return homogeneous coordinates and success.
+  Vector4 p_W_homogeneous = v;
+  bool success = (rank < 3) ? false : true;
+  return std::make_pair(p_W_homogeneous, success);
+}
+
+//------------------------------------------------------------------------------
+void triangulateGaussNewton(
+    const TransformationVector& T_C_W,
+    const Bearings& p_C,
+    Eigen::Ref<Position> p_W)
+{
+  Position p_W_old = p_W;
+  real_t chi2{0.0};
+  Matrix3 A;
+  Vector3 b;
+
+  if(T_C_W.size() < 2u)
+  {
+    LOG(ERROR) << "Optimizing point with less than two observations";
+    return;
+  }
+
+  constexpr real_t eps{1e-7};
+  for (uint32_t iter = 0; iter < 5u; ++iter)
+  {
+    A.setZero();
+    b.setZero();
+    real_t new_chi2{0.0};
+
+    // compute residuals
+    for (size_t i = 0; i < T_C_W.size(); ++i)
+    {
+      const Position p_C_estimated = T_C_W[i] * p_W;
+      Matrix23 J = dUv_dLandmark(p_C_estimated) * T_C_W[i].getRotationMatrix();
+      Vector2 e(ze::project2(p_C_estimated) - ze::project2(p_C.col(i)));
+      A.noalias() += J.transpose() * J;
+      b.noalias() -= J.transpose() * e;
+      new_chi2 += e.squaredNorm();
+    }
+
+    // solve linear system
+    const Vector3 dp(A.ldlt().solve(b));
+
+    // check if error increased
+    if ((iter > 0 && new_chi2 > chi2) || std::isnan(dp[0]))
+    {
+      VLOG(100) << "it " << iter
+                << "\t FAILURE \t new_chi2 = " << new_chi2;
+
+      p_W = p_W_old; // roll-back
+      break;
+    }
+
+    // update the model
+    Position new_point = traits<Position>::retract(p_W, dp);
+    p_W_old = p_W;
+    p_W = new_point;
+    chi2 = new_chi2;
+
+    VLOG(100) << "it " << iter
+              << "\t Success \t new_chi2 = " << new_chi2;
+
+    // stop when converged
+    if (ze::normMax(dp) <= eps)
+    {
+      break;
+    }
+  }
+}
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_geometry/test/test_align_points.cpp b/RWR/src/ze_oss/ze_geometry/test/test_align_points.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..82e1e715299db3939bea4454759264bb71efd24b
--- /dev/null
+++ b/RWR/src/ze_oss/ze_geometry/test/test_align_points.cpp
@@ -0,0 +1,161 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include <cmath>
+#include <functional>
+#include <utility>
+
+#include <ze/common/numerical_derivative.hpp>
+#include <ze/common/test_entrypoint.hpp>
+#include <ze/common/transformation.hpp>
+#include <ze/common/types.hpp>
+#include <ze/common/random.hpp>
+#include <ze/geometry/align_points.hpp>
+
+#ifndef ZE_SINGLE_PRECISION_FLOAT
+ze::real_t tol = 1e-10;
+#else
+ze::real_t tol = 1e-6;
+#endif
+
+TEST(AlignPointsTest, testJacobian)
+{
+#ifndef ZE_SINGLE_PRECISION_FLOAT
+  using namespace ze;
+
+  Vector3 p_A = Vector3::Random();
+  Vector3 p_B = Vector3::Random();
+  Transformation T_A_B;
+  T_A_B.setRandom(1.0);
+
+  auto residualLambda = [&](const Transformation& T_A_B) {
+    return Vector3(p_A - T_A_B * p_B);
+  };
+
+  Matrix36 J_numeric = numericalDerivative<Vector3, Transformation>(residualLambda, T_A_B);
+  Matrix36 J_analytic = dPointdistance_dRelpose(T_A_B, p_A, p_B);
+
+  EXPECT_TRUE(EIGEN_MATRIX_EQUAL_DOUBLE(J_numeric, J_analytic));
+#else
+  LOG(WARNING) << "Numerical derivative test ignored for single precision float.";
+#endif
+}
+
+TEST(AlignPosesTest, testOptimization)
+{
+  using namespace ze;
+
+  const size_t n_points = 100;
+
+  // Generate random points.
+  Positions p_B(3, n_points);
+  for (size_t i = 0; i < n_points; ++i)
+  {
+    p_B.col(i) = Vector3::Random();
+  }
+
+  // Random transformation between trajectories.
+  Transformation T_A_B;
+  T_A_B.setRandom();
+
+  // Compute transformed points.
+  Positions p_A = T_A_B.transformVectorized(p_B);
+
+  // Align trajectories.
+  PointAligner problem(p_A, p_B);
+  Vector6 perturbation = Vector6::Ones() * 0.1;
+  Transformation T_A_B_estimate =
+      T_A_B * Transformation::exp(perturbation);
+  problem.optimize(T_A_B_estimate);
+
+  // Compute error.
+  Transformation T_err = T_A_B.inverse() * T_A_B_estimate;
+  EXPECT_LT(T_err.log().norm(), tol);
+}
+
+TEST(AlignPosesTest, testAlignSE3)
+{
+  using namespace ze;
+
+  const size_t n_points = 100;
+
+  // Generate random points.
+  Positions p_B(3, n_points);
+  for (size_t i = 0; i < n_points; ++i)
+  {
+    p_B.col(i) = Vector3::Random();
+  }
+
+  // Random transformation between trajectories.
+  Transformation T_A_B;
+  T_A_B.setRandom();
+
+  // Compute transformed points.
+  Positions p_A = T_A_B.transformVectorized(p_B);
+
+  // Align trajectories
+  Transformation T_A_B_estimate = alignSE3(p_B, p_A);
+
+  // Compute error.
+  Transformation T_err = T_A_B.inverse() * T_A_B_estimate;
+  EXPECT_LT(T_err.log().norm(), tol);
+}
+
+TEST(AlignPosesTest, testAlignSim3)
+{
+  using namespace ze;
+
+  const size_t n_points = 100;
+
+  // Generate random points.
+  Positions p_B(3, n_points);
+  for (size_t i = 0; i < n_points; ++i)
+  {
+    p_B.col(i) = Vector3::Random();
+  }
+  real_t scale =
+      sampleUniformRealDistribution<real_t>(false, 0.1, 10.0);
+
+  // Random transformation between trajectories.
+  Transformation T_A_B;
+  T_A_B.setRandom();
+
+  // Compute transformed points.
+  Positions p_A =
+      (scale * T_A_B.getRotation().rotateVectorized(p_B)).colwise()
+      + T_A_B.getPosition();
+
+  // Align trajectories
+  std::pair<real_t, Transformation> sim3_estimate = alignSim3(p_B, p_A);
+  Transformation T_A_B_estimate = sim3_estimate.second;
+  real_t scale_estimate = sim3_estimate.first;
+
+  // Compute error.
+  Transformation T_err = T_A_B.inverse() * T_A_B_estimate;
+  EXPECT_LT(T_err.log().norm(), tol);
+  EXPECT_LT(std::abs(scale - scale_estimate), tol);
+}
+
+ZE_UNITTEST_ENTRYPOINT
diff --git a/RWR/src/ze_oss/ze_geometry/test/test_align_poses.cpp b/RWR/src/ze_oss/ze_geometry/test/test_align_poses.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..7badde6d30181f01bd2737d749ee0fb410721112
--- /dev/null
+++ b/RWR/src/ze_oss/ze_geometry/test/test_align_poses.cpp
@@ -0,0 +1,103 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include <cmath>
+#include <functional>
+#include <random>
+#include <utility>
+
+#include <ze/common/numerical_derivative.hpp>
+#include <ze/common/test_entrypoint.hpp>
+#include <ze/common/transformation.hpp>
+#include <ze/common/types.hpp>
+#include <ze/geometry/align_poses.hpp>
+
+TEST(AlignPosesTest, testJacobian)
+{
+#ifndef ZE_SINGLE_PRECISION_FLOAT
+  using namespace ze;
+
+  Transformation T_A0_B0, T_Ai_A0, T_B0_Bi;
+  T_A0_B0.setRandom(1.0);
+  T_Ai_A0.setRandom(1.0);
+  T_B0_Bi.setRandom(1.0);
+
+  auto residualLambda = [&](const Transformation& T_A0_B0) {
+      return (T_Ai_A0 * T_A0_B0 * T_B0_Bi).log();
+    };
+  Matrix6 J_numeric = numericalDerivative<Vector6, Transformation>(residualLambda, T_A0_B0);
+  Matrix6 J_analytic = dRelpose_dTransformation(T_A0_B0, T_Ai_A0, T_B0_Bi);
+
+  EXPECT_TRUE(EIGEN_MATRIX_EQUAL_DOUBLE(J_numeric, J_analytic));
+#else
+  LOG(WARNING) << "Numerical derivative test ignored for single precision float.";
+#endif
+}
+
+TEST(AlignPosesTest, testOptimization)
+{
+  using namespace ze;
+
+  const size_t n_poses = 20;
+
+  // Generate random trajectory.
+  TransformationVector T_W_A(n_poses);
+  for (Transformation& T : T_W_A)
+  {
+    T.setRandom(2.0);
+  }
+
+  // Random transformation between trajectories.
+  Transformation T_A0_B0;
+  T_A0_B0.setRandom();
+
+  // Compute transformed trajectory.
+  TransformationVector T_W_B(n_poses);
+  for (size_t i = 0; i < n_poses; ++i)
+  {
+    Transformation T_A0_Ai = T_W_A[0].inverse() * T_W_A[i];
+    T_W_B[i] = T_W_A[0] * T_A0_B0 * T_A0_Ai;
+  }
+
+  // Perturb estimated pose.
+  real_t sigma_pos = 0.05;
+  real_t sigma_rot = 5.0 / 180 * M_PI;
+  Vector3 pos_pert = Vector3::Random();
+  pos_pert = pos_pert.normalized() * sigma_pos;
+  Vector3 rot_pert = Vector3::Random();
+  rot_pert = rot_pert.normalized() * sigma_rot;
+  Transformation T_pert = Transformation::exp((Vector6() << pos_pert, rot_pert).finished());
+  Transformation T_A0_B0_estimate = T_A0_B0 * T_pert;
+
+  // Optimize.
+  PoseAligner problem(T_W_A, T_W_B, sigma_pos, sigma_rot);
+  problem.optimize(T_A0_B0_estimate);
+
+  // Compute error.
+  Transformation T_err = T_A0_B0.inverse() * T_A0_B0_estimate;
+  EXPECT_LT(T_err.log().norm(), 1.5e-5);
+}
+
+ZE_UNITTEST_ENTRYPOINT
diff --git a/RWR/src/ze_oss/ze_geometry/test/test_clam.cpp b/RWR/src/ze_oss/ze_geometry/test/test_clam.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..da0f872e70ee65eba218486a67f99cd1dee7f7da
--- /dev/null
+++ b/RWR/src/ze_oss/ze_geometry/test/test_clam.cpp
@@ -0,0 +1,254 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include <random>
+#include <ze/common/test_entrypoint.hpp>
+#include <ze/common/test_utils.hpp>
+#include <ze/common/matrix.hpp>
+#include <ze/common/numerical_derivative.hpp>
+#include <ze/common/timer.hpp>
+#include <ze/common/types.hpp>
+#include <ze/common/transformation.hpp>
+#include <ze/cameras/camera_utils.hpp>
+#include <ze/cameras/camera_impl.hpp>
+#include <ze/geometry/clam.hpp>
+
+TEST(ClamTests, testJacobians)
+{
+#ifndef ZE_SINGLE_PRECISION_FLOAT
+  using namespace ze;
+
+  Transformation T_C_B, T_Bc_Br;
+  T_C_B.setRandom(); // Random camera to imu/body transformation.
+  T_Bc_Br = Transformation::exp((Vector6() << 0.2, 0.2, 0.2, 0.1, 0.1, 0.1).finished());
+
+  PinholeCamera cam = createPinholeCamera(640, 480, 329.11, 329.11, 320.0, 240.0);
+  Keypoint px_Cr(230.4, 325.6);
+
+  Bearing f_Cr = cam.backProject(px_Cr);
+  Bearing f_Br = T_C_B.getRotation().inverse().rotate(f_Cr);
+  Bearing p_Br = T_C_B.inverse().getPosition();
+
+  real_t inv_depth = 0.455;
+
+  Keypoint px_Cc = cam.project(T_C_B * T_Bc_Br * T_C_B.inverse() * (f_Cr * (1.0 / inv_depth)));
+
+  Matrix26 H1;
+  Matrix21 H2;
+  Vector2 res = reprojectionResidual(
+        f_Br, p_Br, cam, T_C_B, T_Bc_Br, inv_depth, px_Cc, &H1, &H2);
+  EXPECT_TRUE(EIGEN_MATRIX_ZERO(res, 1e-10));
+
+  Matrix26 H1_numeric = numericalDerivative<Vector2, Transformation>(
+        std::bind(&reprojectionResidual, f_Br, p_Br, cam, T_C_B,
+                  std::placeholders::_1, inv_depth, px_Cc, nullptr, nullptr), T_Bc_Br);
+  EXPECT_TRUE(EIGEN_MATRIX_NEAR(H1, H1_numeric, 1e-6));
+
+  Matrix21 H2_numeric = numericalDerivative<Vector2, real_t>(
+        std::bind(&reprojectionResidual, f_Br, p_Br, cam, T_C_B,
+                  T_Bc_Br, std::placeholders::_1, px_Cc, nullptr, nullptr), inv_depth);
+  EXPECT_TRUE(EIGEN_MATRIX_NEAR(H2, H2_numeric, 1e-6));
+#else
+  LOG(WARNING) << "Numerical derivative test ignored for single precision float.";
+#endif
+}
+
+
+TEST(ClamTests, testExperiment)
+{
+  using namespace ze;
+
+  Transformation T_C_B, T_Bc_Br;
+  T_C_B.setRandom(); // Random camera to imu/body transformation.
+  T_Bc_Br = Transformation::exp((Vector6() << 0.2, 0.2, 0.2, 0.1, 0.1, 0.1).finished());
+
+  const size_t n = 120;
+  std::string data_dir = ze::getTestDataDir("synthetic_room_pinhole");
+  CameraRig::Ptr rig = ze::cameraRigFromYaml(data_dir + "/calib_rig.yaml");
+  Keypoints px_Cr_true = generateRandomKeypoints(rig->at(0).size(), 10, n);
+  Bearings f_Cr = rig->at(0).backProjectVectorized(px_Cr_true);
+  Positions p_Cr = f_Cr;
+
+  // Obtain the 3D points by applying a random scaling between 1 and 3 meters.
+  std::ranlux24 gen;
+  std::uniform_real_distribution<real_t> scale(1.0, 3.0);
+  VectorX depth_true(n);
+  for(size_t i = 0; i < n; ++i)
+  {
+    depth_true(i) = scale(gen);
+    p_Cr.col(i) *= depth_true(i);
+  }
+
+  Positions p_Br = T_C_B.inverse().transformVectorized(p_Cr);
+  Positions p_Cc = (T_C_B * T_Bc_Br).transformVectorized(p_Br);
+  Keypoints px_Cc_true = rig->at(0).projectVectorized(p_Cc);
+
+  // Apply some noise to the keypoints to simulate measurements.
+  Keypoints px_Cc_noisy = px_Cc_true;
+  const real_t stddev = 1.0;
+  std::normal_distribution<real_t> px_noise(0.0, stddev);
+  for(size_t i = 0; i < n; ++i)
+  {
+    px_Cc_noisy(0,i) += px_noise(gen);
+    px_Cc_noisy(1,i) += px_noise(gen);
+  }
+  Bearings f_Cc_noisy = rig->at(0).backProjectVectorized(px_Cc_noisy);
+
+  // Perturb pose:
+  Transformation T_Bc_Br_perturbed =
+      T_Bc_Br * Transformation::exp((Vector6() << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1).finished());
+
+  // Test Localization, without prior:
+  {
+    ClamFrameData data;
+    data.f_C = f_Cc_noisy;
+    data.p_Br = p_Br;
+    data.T_C_B = T_C_B;
+    std::vector<ClamFrameData> data_vec = { data };
+
+    ClamLandmarks landmarks;
+
+    real_t pos_prior_weight = 0.0;
+    real_t rot_prior_weight = 0.0;
+    ze::Timer t;
+    Clam optimizer(
+          landmarks, data_vec, *rig, T_Bc_Br, pos_prior_weight, rot_prior_weight);
+    Transformation T_Bc_Br_estimate = T_Bc_Br_perturbed;
+
+    ClamState state;
+    state.at<0>() = T_Bc_Br_estimate;
+    optimizer.optimize(state);
+    VLOG(1) << "optimization took " << t.stopAndGetMilliseconds() << " ms\n";
+
+    // Compute error:
+    T_Bc_Br_estimate = state.at<0>();
+    Transformation T_err = T_Bc_Br * T_Bc_Br_estimate.inverse();
+    real_t pos_error = T_err.getPosition().norm();
+    real_t ang_error = T_err.getRotation().log().norm();
+    CHECK_LT(pos_error, 0.005);
+    CHECK_LT(ang_error, 0.005);
+    VLOG(1) << "ang error = " << ang_error;
+    VLOG(1) << "pos error = " << pos_error;
+  }
+
+  // Test Mapping, with rotation prior:
+  {
+    ClamFrameData data;
+    data.landmark_measurements.reserve(n);
+    for(size_t i = 0; i < n; ++i)
+    {
+      if(isVisible(rig->at(0).width(), rig->at(0).height(), px_Cc_noisy.col(i)))
+      {
+        data.landmark_measurements.push_back(std::make_pair(i, px_Cc_noisy.col(i)));
+      }
+    }
+    VLOG(1) << "# Visible Landmarks = " << data.landmark_measurements.size();
+    data.T_C_B = T_C_B;
+    std::vector<ClamFrameData> data_vec = { data };
+
+    ClamLandmarks landmarks;
+    landmarks.f_Br = T_C_B.getRotation().inverse().rotateVectorized(f_Cr);
+    Vector3 t_Br_Cr = T_C_B.inverse().getPosition();
+    landmarks.origin_Br = t_Br_Cr.replicate(1, landmarks.f_Br.cols());
+
+    real_t pos_prior_weight = 0.2;
+    real_t rot_prior_weight = 10.0;
+    ze::Timer t;
+    Clam optimizer(
+          landmarks, data_vec, *rig, T_Bc_Br, pos_prior_weight, rot_prior_weight);
+    Transformation T_Bc_Br_estimate = T_Bc_Br_perturbed;
+
+    ClamState state;
+    state.at<0>() = T_Bc_Br_estimate;
+    VectorX& inv_depths = state.at<1>();
+    inv_depths.resize(landmarks.f_Br.cols());
+    inv_depths.setConstant(1.0 / 1.5);
+    optimizer.optimize(state);
+    VLOG(1) << "optimization took " << t.stopAndGetMilliseconds() << " ms\n";
+
+    // Compute error:
+    T_Bc_Br_estimate = state.at<0>();
+    Transformation T_err = T_Bc_Br * T_Bc_Br_estimate.inverse();
+    real_t pos_error = T_err.getPosition().norm();
+    real_t ang_error = T_err.getRotation().log().norm();
+    CHECK_LT(pos_error, 0.005);
+    CHECK_LT(ang_error, 0.005);
+    VLOG(1) << "ang error = " << ang_error;
+    VLOG(1) << "pos error = " << pos_error;
+  }
+
+  // Test Localization AND mapping without prior:
+  {
+    ClamFrameData data;
+    data.f_C = f_Cc_noisy.leftCols<10>();
+    data.p_Br = p_Br.leftCols<10>();
+    data.landmark_measurements.reserve(n);
+    for(size_t i = 0; i < n; ++i)
+    {
+      if(isVisible(rig->at(0).width(), rig->at(0).height(), px_Cc_noisy.col(i)))
+      {
+        data.landmark_measurements.push_back(std::make_pair(i, px_Cc_noisy.col(i)));
+      }
+    }
+    VLOG(1) << "# Visible Landmarks = " << data.landmark_measurements.size();
+    data.T_C_B = T_C_B;
+    std::vector<ClamFrameData> data_vec = { data };
+
+    ClamLandmarks landmarks;
+    landmarks.f_Br = T_C_B.getRotation().inverse().rotateVectorized(f_Cr);
+    Vector3 t_Br_Cr = T_C_B.inverse().getPosition();
+    landmarks.origin_Br = t_Br_Cr.replicate(1, landmarks.f_Br.cols());
+
+    real_t pos_prior_weight = 0.0;
+    real_t rot_prior_weight = 0.0;
+    ze::Timer t;
+    Clam optimizer(
+          landmarks, data_vec, *rig, T_Bc_Br, pos_prior_weight, rot_prior_weight);
+    Transformation T_Bc_Br_estimate = T_Bc_Br_perturbed;
+
+    ClamState state;
+    state.at<0>() = T_Bc_Br_estimate;
+    VectorX& inv_depths = state.at<1>();
+    inv_depths.resize(landmarks.f_Br.cols());
+    inv_depths.setConstant(1.0 / 1.5);
+    optimizer.optimize(state);
+    VLOG(1) << "optimization took " << t.stopAndGetMilliseconds() << " ms\n";
+
+    // Compute error:
+    T_Bc_Br_estimate = state.at<0>();
+    Transformation T_err = T_Bc_Br * T_Bc_Br_estimate.inverse();
+    real_t pos_error = T_err.getPosition().norm();
+    real_t ang_error = T_err.getRotation().log().norm();
+    EXPECT_LT(pos_error, 0.007);
+    EXPECT_LT(ang_error, 0.005);
+    VLOG(1) << "ang error = " << ang_error;
+    VLOG(1) << "pos error = " << pos_error;
+  }
+}
+
+
+
+
+ZE_UNITTEST_ENTRYPOINT
diff --git a/RWR/src/ze_oss/ze_geometry/test/test_line.cpp b/RWR/src/ze_oss/ze_geometry/test/test_line.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..834163f31d73b2b981414c9a06631c24ee099bef
--- /dev/null
+++ b/RWR/src/ze_oss/ze_geometry/test/test_line.cpp
@@ -0,0 +1,105 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include <functional>
+
+#include <ze/cameras/camera_impl.hpp>
+#include <ze/common/numerical_derivative.hpp>
+#include <ze/common/test_entrypoint.hpp>
+#include <ze/geometry/line.hpp>
+
+TEST(LineTests, testLineGeneration)
+{
+  using namespace ze;
+  // Create lines.
+  size_t n = 100;
+  Lines lines;
+  Positions start, end;
+  PinholeCamera cam = createTestPinholeCamera();
+  Transformation T_W_C;
+  T_W_C.setRandom();
+  std::tie(start, end) = generateRandomVisibleLines(cam, T_W_C, n, lines);
+
+  for (size_t i = 0; i < n; ++i)
+  {
+    Vector3 direction = lines[i].direction();
+    Position anchor_point = lines[i].anchorPoint();
+    // Check whether anchor point is really the closest point available.
+    EXPECT_NEAR(direction.dot(anchor_point), 0.0, 1e-10);
+    // Direction should be a normalized vector.
+    EXPECT_NEAR(direction.norm(), 1.0, 1e-12);
+    // Anchor, start and end point should be on the line.
+    EXPECT_NEAR(lines[i].distanceToLine(anchor_point), 0.0, 1e-12);
+    EXPECT_NEAR(lines[i].distanceToLine(start.col(i)), 0.0, 1e-12);
+    EXPECT_NEAR(lines[i].distanceToLine(end.col(i)), 0.0, 1e-12);
+  }
+}
+
+TEST(LineTests, testJacobian)
+{
+  using namespace ze;
+  Transformation T_B_W, T_C_B;
+  T_B_W.setRandom();
+  T_C_B.setRandom();
+  const Transformation T_C_W = T_C_B * T_B_W;
+  const Transformation T_W_C = T_C_W.inverse();
+
+  size_t num_tests = 100;
+  Lines lines_W;
+  Positions start_W, end_W;
+  ze::PinholeCamera cam = createTestPinholeCamera();
+  std::tie(start_W, end_W) = generateRandomVisibleLines(cam, T_W_C, num_tests, lines_W);
+
+  // Create measurements.
+  LineMeasurements measurements_C(3, num_tests);
+  size_t i;
+  for (i = 0; i < num_tests; ++i)
+  {
+    measurements_C.col(i) =
+        lineMeasurementFromBearings(T_C_W * start_W.col(i),
+                                    T_C_W * end_W.col(i));
+  }
+
+  auto measurementError = [&](const Transformation& T_B_W_in_lambda) {
+    Transformation T_W_C_in_lambda = (T_C_B * T_B_W_in_lambda).inverse();
+    Position camera_pos_W = T_W_C_in_lambda.getPosition();
+    Vector3 measurement_W = T_W_C_in_lambda.getRotation().rotate(measurements_C.col(i));
+    return lines_W[i].calculateMeasurementError(measurement_W, camera_pos_W);
+  };
+
+  LineMeasurements measurements_W = T_W_C.getRotationMatrix() * measurements_C;
+  for (i = 0; i < num_tests; ++i)
+  {
+    Matrix26 J_numeric =
+        numericalDerivative<Vector2, Transformation>(measurementError, T_B_W);
+    Matrix26 J_analytic =
+        dLineMeasurement_dPose(T_B_W, T_C_B, measurements_W.col(i),
+                               lines_W[i].anchorPoint(), lines_W[i].direction());
+    EXPECT_TRUE(EIGEN_MATRIX_NEAR(J_numeric, J_analytic, 1e-9));
+  }
+
+}
+
+ZE_UNITTEST_ENTRYPOINT
diff --git a/RWR/src/ze_oss/ze_geometry/test/test_lsq_state.cpp b/RWR/src/ze_oss/ze_geometry/test/test_lsq_state.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..62c2073446ec70910fabadcf5e3def64ebe8a177
--- /dev/null
+++ b/RWR/src/ze_oss/ze_geometry/test/test_lsq_state.cpp
@@ -0,0 +1,84 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include <random>
+#include <ze/common/test_entrypoint.hpp>
+#include <ze/common/matrix.hpp>
+#include <ze/common/manifold.hpp>
+#include <ze/common/types.hpp>
+#include <ze/common/transformation.hpp>
+#include <ze/geometry/lsq_state.hpp>
+
+TEST(StateTests, testTupleFixedSize)
+{
+  using namespace ze;
+  using Tuple1 = std::tuple<Transformation, real_t, Vector3>;
+  using Tuple2 = std::tuple<Transformation, VectorX>;
+  EXPECT_TRUE(internal::TupleIsFixedSize<Tuple1>::is_fixed_size);
+  EXPECT_FALSE(internal::TupleIsFixedSize<Tuple2>::is_fixed_size);
+}
+
+TEST(StateTests, testStateFixedSize)
+{
+  using namespace ze;
+
+  using MyState = State<Transformation,Vector3,real_t>;
+  EXPECT_EQ(MyState::dimension, 10);
+
+  MyState state;
+  state.print();
+
+  MyState::TangentVector v;
+  state.retract(v);
+
+  EXPECT_EQ(State<Transformation>::dimension, 6);
+}
+
+TEST(StateTests, testStateDynamicSize)
+{
+  using namespace ze;
+  using MyState = State<Transformation,VectorX>;
+
+  // Test constructor of dynamic-sized state.
+  MyState state;
+  VectorX& x = state.at<1>();
+  x.resize(5);
+  x.setConstant(0.5);
+  state.print();
+
+  EXPECT_EQ(state.getDimension(), 11);
+  EXPECT_TRUE(state.isDynamicSize());
+  EXPECT_FALSE(state.isElementDynamicSize<0>());
+  EXPECT_TRUE(state.isElementDynamicSize<1>());
+
+  // Test retract.
+  traits<MyState>::TangentVector v;
+  v.resize(state.getDimension());
+  v.setConstant(1.0);
+  state.retract(v);
+  state.print();
+}
+
+ZE_UNITTEST_ENTRYPOINT
diff --git a/RWR/src/ze_oss/ze_geometry/test/test_pose_optimizer.cpp b/RWR/src/ze_oss/ze_geometry/test/test_pose_optimizer.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..572041f07990cec6b4f0008c8fe5f5c243b99827
--- /dev/null
+++ b/RWR/src/ze_oss/ze_geometry/test/test_pose_optimizer.cpp
@@ -0,0 +1,211 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include <random>
+#include <ze/common/benchmark.hpp>
+#include <ze/common/test_entrypoint.hpp>
+#include <ze/common/matrix.hpp>
+#include <ze/common/timer.hpp>
+#include <ze/common/types.hpp>
+#include <ze/common/transformation.hpp>
+#include <ze/cameras/camera_utils.hpp>
+#include <ze/cameras/camera_impl.hpp>
+#include <ze/geometry/line.hpp>
+#include <ze/geometry/pose_optimizer.hpp>
+#include <ze/geometry/robust_cost.hpp>
+
+namespace ze {
+
+void testPoseOptimizer(
+    const real_t pos_prior_weight,
+    const real_t rot_prior_weight,
+    const Transformation& T_B_W,
+    const Transformation& T_B_W_perturbed,
+    const PoseOptimizerFrameData& data,
+    const std::string& description)
+{
+  SCOPED_TRACE(description);
+  PoseOptimizerFrameDataVec data_vec = { data };
+  Transformation T_B_W_estimate;
+  auto fun = [&]()
+  {
+    PoseOptimizer optimizer(
+          PoseOptimizer::getDefaultSolverOptions(),
+          data_vec, T_B_W, pos_prior_weight, rot_prior_weight);
+    T_B_W_estimate = T_B_W_perturbed;
+    optimizer.optimize(T_B_W_estimate);
+  };
+  runTimingBenchmark(fun, 1, 10, description, true);
+
+  // Compute error:
+  Transformation T_err = T_B_W * T_B_W_estimate.inverse();
+  real_t pos_error = T_err.getPosition().norm();
+  real_t ang_error = T_err.getRotation().log().norm();
+  EXPECT_LT(pos_error, 0.0075);
+  EXPECT_LT(ang_error, 0.005);
+  VLOG(1) << "ang error = " << ang_error;
+  VLOG(1) << "pos error = " << pos_error;
+}
+
+} // namespace ze
+
+TEST(PoseOptimizerTests, testSolver)
+{
+  using namespace ze;
+
+  Transformation T_C_B, T_B_W;
+  T_C_B.setRandom(); // Random camera to imu/body transformation.
+  T_B_W.setRandom(); // Random body transformation.
+
+  const size_t n = 120;
+  PinholeCamera cam = createTestPinholeCamera();
+  Keypoints px_true = generateRandomKeypoints(cam.size(), 10, n);
+
+  Positions pos_C = cam.backProjectVectorized(px_true);
+
+  // Obtain the 3D points by applying a random scaling between 1 and 3 meters.
+  std::ranlux24 gen;
+  std::uniform_real_distribution<double> scale(1.0, 3.0);
+  for(size_t i = 0; i < n; ++i)
+  {
+    pos_C.col(i) *= scale(gen);
+  }
+
+  // Transform points to world coordinates.
+  Positions pos_W = (T_B_W.inverse() * T_C_B.inverse()).transformVectorized(pos_C);
+
+  // Apply some noise to the keypoints to simulate measurements.
+  Keypoints px_noisy = px_true;
+  VectorX pyr_scale(n);
+  const double stddev = 1.0;
+  std::normal_distribution<double> px_noise(0.0, stddev);
+  for(size_t i = 0; i < n; ++i)
+  {
+    // Features distribute among all levels. features on higher levels have more
+    // uncertainty.
+    pyr_scale(i) = (1 << (i % 4));
+    px_noisy(0,i) += pyr_scale(i) * px_noise(gen);
+    px_noisy(1,i) += pyr_scale(i) * px_noise(gen);
+  }
+  Bearings bearings_noisy = cam.backProjectVectorized(px_noisy);
+
+  // Perturb pose:
+  Transformation T_B_W_perturbed =
+      T_B_W * Transformation::exp((Vector6() << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1).finished());
+
+  // Optimize using bearing vectors:
+  PoseOptimizerFrameData data;
+  data.f = bearings_noisy;
+  data.kp_idx = KeypointIndices(n, 1);
+  data.p_W = pos_W;
+  data.T_C_B = T_C_B;
+  data.scale = pyr_scale;
+  data.type = PoseOptimizerResidualType::UnitPlane;
+
+  testPoseOptimizer(
+        0.0, 0.0, T_B_W, T_B_W_perturbed, data, "UnitPlane, No Prior");
+  testPoseOptimizer(
+        10.0, 0.0, T_B_W, T_B_W_perturbed, data, "UnitPlane, Rotation Prior");
+  testPoseOptimizer(
+        10.0, 10.0, T_B_W, T_B_W_perturbed, data, "UnitPlane, Rotation and Position Prior");
+
+  data.type = PoseOptimizerResidualType::Bearing;
+
+  testPoseOptimizer(
+        0.0, 0.0, T_B_W, T_B_W_perturbed, data, "Bearing, No Prior");
+  testPoseOptimizer(
+        10.0, 0.0, T_B_W, T_B_W_perturbed, data, "Bearing, Rotation Prior");
+  testPoseOptimizer(
+        10.0, 10.0, T_B_W, T_B_W_perturbed, data, "Bearing, Rotation and Position Prior");
+}
+
+TEST(PoseOptimizerTests, testSolver_withLines)
+{
+  using namespace ze;
+
+  Transformation T_C_B, T_B_W;
+  T_C_B.setRandom(); // Random camera to imu/body transformation.
+  T_B_W.setRandom(); // Random body transformation.
+
+  const size_t n = 130;
+  PinholeCamera cam = createTestPinholeCamera();
+  Keypoints endpoints_image;
+  Bearings bearings_truth;
+  Positions endpoints_C;
+  std::tie(endpoints_image, bearings_truth, endpoints_C) =
+      generateRandomVisible3dPoints(cam, 2 * n, 10, 1.0, 3.0);
+
+  Positions endpoints_W =
+      (T_B_W.inverse() * T_C_B.inverse()).transformVectorized(endpoints_C);
+
+  Lines lines_W = generateLinesFromEndpoints(endpoints_W.block(0, 0, 3, n),
+                                             endpoints_W.block(0, n, 3, n));
+
+  // Apply some noise to the endpoints to simulate measurements.
+  Keypoints endpoints_noisy = endpoints_image;
+  const double stddev = 1.0;
+  std::ranlux24 gen;
+  std::normal_distribution<double> endpoints_noise(0.0, stddev);
+  for (size_t i = 0; i < 2 * n; ++i)
+  {
+    endpoints_noisy(0, i) += endpoints_noise(gen);
+    endpoints_noisy(1, i) += endpoints_noise(gen);
+  }
+  Bearings bearings_noisy = cam.backProjectVectorized(endpoints_noisy);
+  LineMeasurements line_measurements_noisy(3, n);
+  LineMeasurements line_measurements_truth(3, n);
+  for (size_t i = 0; i < n; ++i)
+  {
+    line_measurements_noisy.col(i) =
+        lineMeasurementFromBearings(bearings_noisy.col(i), bearings_noisy.col(n + i));
+    line_measurements_truth.col(i) =
+        lineMeasurementFromBearings(bearings_truth.col(i), bearings_truth.col(n + i));
+  }
+
+  // Check if error for truth is zero.
+  PoseOptimizerFrameData data;
+  data.line_measurements_C = line_measurements_truth;
+  data.lines_W = lines_W;
+  data.T_C_B = T_C_B;
+  data.type = PoseOptimizerResidualType::Line;
+
+  PoseOptimizerFrameDataVec data_vec = { data };
+  PoseOptimizer optimizer(
+        PoseOptimizer::getDefaultSolverOptions(),
+        data_vec, T_B_W, 0.0, 0.0);
+  real_t error = optimizer.evaluateError(T_B_W, nullptr, nullptr);
+  EXPECT_NEAR(error, 0.0, 1e-5);
+
+  // Perturb pose:
+  Transformation T_B_W_perturbed =
+      T_B_W * Transformation::exp((Vector6() << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1).finished());
+
+  // Optimize using noisy lines:
+  data.line_measurements_C = line_measurements_noisy;
+  testPoseOptimizer(
+        0.0, 0.0, T_B_W, T_B_W_perturbed, data, "Line, No Prior");
+}
+
+ZE_UNITTEST_ENTRYPOINT
diff --git a/RWR/src/ze_oss/ze_geometry/test/test_ransac_relative_pose.cpp b/RWR/src/ze_oss/ze_geometry/test/test_ransac_relative_pose.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..98886ec2e87dc6a6e5264bfe610434823022e0c3
--- /dev/null
+++ b/RWR/src/ze_oss/ze_geometry/test/test_ransac_relative_pose.cpp
@@ -0,0 +1,126 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include <random>
+#include <ze/common/test_entrypoint.hpp>
+#include <ze/common/matrix.hpp>
+#include <ze/common/timer.hpp>
+#include <ze/common/types.hpp>
+#include <ze/common/transformation.hpp>
+#include <ze/cameras/camera_utils.hpp>
+#include <ze/cameras/camera_impl.hpp>
+#include <ze/geometry/ransac_relative_pose.hpp>
+
+TEST(RansacRelativePoseTests, testCopyBearings)
+{
+  using namespace ze;
+
+  size_t n = 100;
+  Bearings f(3, n);
+  f.setRandom();
+  BearingsVector v = bearingsVectorFromBearings(f);
+  for(size_t i = 0; i < n; ++i)
+  {
+    EXPECT_TRUE(EIGEN_MATRIX_EQUAL_DOUBLE(v[i], f.col(i)));
+  }
+}
+
+TEST(RansacRelativePoseTests, testFivePointAndTranslationOnly)
+{
+  using namespace ze;
+
+  size_t n_points = 100;
+  PinholeCamera cam = createPinholeCamera(640, 480, 329.11, 329.11, 320.0, 240.0);
+  Keypoints px_ref;
+  Bearings f_ref;
+  Positions pos_ref;
+  std::tie(px_ref, f_ref, pos_ref) = generateRandomVisible3dPoints(cam, n_points, 0, 2.0, 8.0);
+
+  Transformation T_cur_ref;
+  T_cur_ref.setRandom(1.5, 20.0 / 180.0 * M_PI);
+
+  Bearings f_cur = T_cur_ref.transformVectorized(pos_ref);
+  normalizeBearings(f_cur);
+
+  // Insert outliers.
+  size_t n_outliers = 20;
+  for(size_t i = 0; i < n_outliers; ++i)
+  {
+    f_cur.col(i).swap(f_ref.col(i));
+  }
+
+  // Some outliers, Five-Point
+  Transformation T;
+  RansacRelativePose ransac(cam, 1.0);
+  ransac.ogv_verbosity_level_ = 2;
+  bool success = ransac.solve(f_ref, f_cur, RelativePoseAlgorithm::FivePoint, T);
+  EXPECT_TRUE(success);
+  EXPECT_EQ(ransac.inliers().size(), n_points - n_outliers);
+  std::cout << "Error = " << (T.inverse() * T_cur_ref).log().transpose() << std::endl;
+
+  // Some outliers, Estimate relative translation.
+  T.getRotation() = T_cur_ref.getRotation();
+  success = ransac.solve(f_ref, f_cur, RelativePoseAlgorithm::TwoPointTranslationOnly, T);
+  EXPECT_TRUE(success);
+  EXPECT_EQ(ransac.inliers().size(), n_points - n_outliers);
+  std::cout << "Error = " << (T.inverse() * T_cur_ref).log().transpose() << std::endl;
+}
+
+TEST(RansacRelativePoseTests, testRotationOnly)
+{
+  using namespace ze;
+
+  size_t n_points = 100;
+  PinholeCamera cam = createPinholeCamera(640, 480, 329.11, 329.11, 320.0, 240.0);
+  Keypoints px_ref;
+  Bearings f_ref;
+  Positions pos_ref;
+  std::tie(px_ref, f_ref, pos_ref) = generateRandomVisible3dPoints(cam, n_points, 0, 2.0, 8.0);
+
+  Transformation T_cur_ref;
+  T_cur_ref.setRandom(1.5, 20.0 / 180.0 * M_PI);
+  T_cur_ref.getPosition() = Vector3::Zero();
+
+  Bearings f_cur = T_cur_ref.transformVectorized(pos_ref);
+  normalizeBearings(f_cur);
+
+  // Insert outliers.
+  size_t n_outliers = 20;
+  for(size_t i = 0; i < n_outliers; ++i)
+  {
+    f_cur.col(i).swap(f_ref.col(i));
+  }
+
+  // Some outliers, Five-Point
+  Transformation T;
+  RansacRelativePose ransac(cam, 1.0);
+  ransac.ogv_verbosity_level_ = 2;
+  bool success = ransac.solve(f_ref, f_cur, RelativePoseAlgorithm::TwoPointRotationOnly, T);
+  EXPECT_TRUE(success);
+  EXPECT_EQ(ransac.inliers().size(), n_points - n_outliers);
+  std::cout << "Error = " << (T.inverse() * T_cur_ref).log().transpose() << std::endl;
+}
+
+ZE_UNITTEST_ENTRYPOINT
diff --git a/RWR/src/ze_oss/ze_geometry/test/test_robust_cost.cpp b/RWR/src/ze_oss/ze_geometry/test/test_robust_cost.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..2a0b16eeef21a6ac93de849019c0c44db5e2073b
--- /dev/null
+++ b/RWR/src/ze_oss/ze_geometry/test/test_robust_cost.cpp
@@ -0,0 +1,78 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include <cmath>
+#include <random>
+#include <utility>
+#include <ze/common/test_entrypoint.hpp>
+#include <ze/common/types.hpp>
+#include <ze/geometry/robust_cost.hpp>
+
+TEST(RobustCostTest, testScaleEstimators)
+{
+  using namespace ze;
+
+  // Generate normally distributed errors with standard deviation 3.0
+  std::ranlux24 gen;
+  std::normal_distribution<real_t> noise(0.0, 3.0);
+  constexpr int n = 1000;
+  VectorX errors(n);
+  errors.setZero();
+  for(int i = 0; i < n; ++i)
+    errors(i) = noise(gen);
+
+  double s1 = UnitScaleEstimator<real_t>::compute(errors);
+  EXPECT_FLOATTYPE_EQ(s1, 1.0);
+
+  double s2 = NormalDistributionScaleEstimator<real_t>::compute(errors);
+  EXPECT_TRUE(std::abs(s2 - 3.0) < 0.2);
+
+  double s3 = MADScaleEstimator<real_t>::compute(errors);
+  EXPECT_TRUE(std::abs(s3 - 3.0) < 0.2);
+}
+
+TEST(RobustCostTest, testWeightFunctions)
+{
+  using namespace ze;
+
+  // Generate normally distributed errors with standard deviation 3.0
+  std::ranlux24 gen;
+  std::normal_distribution<real_t> noise(0.0, 3.0);
+  constexpr int n = 10;
+  VectorX errors(n);
+  errors.setZero();
+  for (int i = 0; i < n; ++i)
+  {
+    errors(i) = noise(gen);
+  }
+
+  VectorX errors_scaled = HuberWeightFunction<real_t>::weightVectorized(errors);
+
+  //! @todo: add checks.
+  VLOG(1) << errors.transpose();
+  VLOG(1) << errors_scaled.transpose();
+}
+
+ZE_UNITTEST_ENTRYPOINT
diff --git a/RWR/src/ze_oss/ze_geometry/test/test_triangulation.cpp b/RWR/src/ze_oss/ze_geometry/test/test_triangulation.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..2120feb28991dfbf8c1d9caa771fde874727458c
--- /dev/null
+++ b/RWR/src/ze_oss/ze_geometry/test/test_triangulation.cpp
@@ -0,0 +1,121 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include <random>
+#include <ze/common/test_entrypoint.hpp>
+#include <ze/common/matrix.hpp>
+#include <ze/common/timer.hpp>
+#include <ze/common/types.hpp>
+#include <ze/common/transformation.hpp>
+#include <ze/cameras/camera_impl.hpp>
+#include <ze/cameras/camera_utils.hpp>
+#include <ze/geometry/triangulation.hpp>
+
+namespace ze {
+
+#ifndef ZE_SINGLE_PRECISION_FLOAT
+  real_t tol = 1e-10;
+#else
+  real_t tol = 3e-6;
+#endif
+
+std::tuple<Position, TransformationVector, Bearings>
+generateObservingCameras()
+{
+  Transformation T_W_C;
+  T_W_C.setRandom(); // Random camera to world transformation.
+
+  PinholeCamera cam = createPinholeCamera(640, 480, 329.11, 329.11, 320.0, 240.0);
+  Keypoint px(349, 210);
+  Position p_W_true = T_W_C * (cam.backProject(px) * 2.0);
+
+  TransformationVector T_C_W_vec;
+  Bearings f_C(3, 10);
+  int n = 0;
+  std::ranlux24 gen;
+  std::normal_distribution<real_t> noise_rot(0, 0.1);
+  std::normal_distribution<real_t> noise_pos(0, 0.5);
+  for(int i = 0; i < 10; ++i)
+  {
+    // Perturb pose:
+    Vector6 pert;
+    pert.head<3>() = Vector3::Constant(noise_pos(gen));
+    pert.tail<3>() = Vector3::Constant(noise_rot(gen));
+    Transformation T_C_W_perturbed = (T_W_C * Transformation::exp(pert)).inverse();
+
+    Position p_C = T_C_W_perturbed * p_W_true;
+    Keypoint px = cam.project(p_C);
+    if(isVisible(cam.size(), px))
+    {
+      T_C_W_vec.push_back(T_C_W_perturbed);
+      f_C.col(n++) = p_C.normalized();
+    }
+  }
+  f_C.conservativeResize(3, T_C_W_vec.size());
+  CHECK_GE(T_C_W_vec.size(), 2u);
+  return std::make_tuple(p_W_true, T_C_W_vec, f_C);
+}
+
+} // namespace ze
+
+TEST(TriangulationTests, testSolver)
+{
+  using namespace ze;
+
+  // Generate data.
+  Position p_W_true;
+  TransformationVector T_C_W_vec;
+  Bearings f_C;
+  std::tie(p_W_true, T_C_W_vec, f_C) = ze::generateObservingCameras();
+
+  // Triangulate.
+  Vector4 p_W_homogeneous;
+  bool success;
+  std::tie(p_W_homogeneous, success) = triangulateHomogeneousDLT(T_C_W_vec, f_C);
+  Vector3 p_W_estimated = p_W_homogeneous.head<3>() / p_W_homogeneous(3);
+
+  // Compare error.
+  EXPECT_LT((p_W_estimated - p_W_true).norm(), tol);
+}
+
+TEST(TriangulationTests, testNonlinearRefinement)
+{
+  using namespace ze;
+
+  // Generate data.
+  Position p_W_true;
+  TransformationVector T_C_W_vec;
+  Bearings f_C;
+  std::tie(p_W_true, T_C_W_vec, f_C) = ze::generateObservingCameras();
+
+  // Triangulate.
+  Position p_W_estimated = p_W_true + Vector3(0.1, 0.05, 0.1);
+  triangulateGaussNewton(T_C_W_vec, f_C, p_W_estimated);
+
+  // Compare error.
+  EXPECT_LT((p_W_estimated - p_W_true).norm(), tol);
+}
+
+ZE_UNITTEST_ENTRYPOINT
diff --git a/RWR/src/ze_oss/ze_imu/CATKIN_IGNORE b/RWR/src/ze_oss/ze_imu/CATKIN_IGNORE
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/RWR/src/ze_oss/ze_imu/CMakeLists.txt b/RWR/src/ze_oss/ze_imu/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..f2375bb3cc80415511dca76f237bef266ac816f1
--- /dev/null
+++ b/RWR/src/ze_oss/ze_imu/CMakeLists.txt
@@ -0,0 +1,73 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(ze_imu)
+
+find_package(catkin_simple REQUIRED)
+catkin_simple(ALL_DEPS_REQUIRED)
+
+include(ze_setup)
+
+# This macro ensures modules and global scripts declared therein get installed
+# See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+catkin_python_setup()
+
+#############
+# LIBRARIES #
+#############
+set(HEADERS
+  include/ze/imu/imu_model.hpp
+  include/ze/imu/imu_rig.hpp
+  include/ze/imu/imu_intrinsic_model.hpp
+  include/ze/imu/imu_noise_model.hpp
+  include/ze/imu/accelerometer_model.hpp
+  include/ze/imu/gyroscope_model.hpp
+  include/ze/imu/imu_yaml_serialization.hpp
+  include/ze/imu/imu_buffer.hpp
+  include/ze/imu/imu_types.hpp
+  )
+
+set(SOURCES
+    src/imu_model.cpp
+    src/imu_rig.cpp
+    src/imu_intrinsic_model.cpp
+    src/imu_noise_model.cpp
+    src/accelerometer_model.cpp
+    src/gyroscope_model.cpp
+    src/imu_yaml_serialization.cpp
+    src/imu_buffer.cpp
+  )
+
+cs_add_library(${PROJECT_NAME} ${SOURCES} ${HEADERS})
+
+##########
+# GTESTS #
+##########
+
+catkin_add_gtest(test_accelerometer_model test/test_accelerometer_model.cpp)
+target_link_libraries(test_accelerometer_model ${PROJECT_NAME})
+
+catkin_add_gtest(test_gyroscope_model test/test_gyroscope_model.cpp)
+target_link_libraries(test_gyroscope_model ${PROJECT_NAME})
+
+catkin_add_gtest(test_imu_model test/test_imu_model.cpp)
+target_link_libraries(test_imu_model ${PROJECT_NAME})
+
+catkin_add_gtest(test_yaml_serialization test/test_yaml_serialization.cpp)
+target_link_libraries(test_yaml_serialization ${PROJECT_NAME})
+
+catkin_add_gtest(test_intrinsic_model test/test_intrinsic_model.cpp)
+target_link_libraries(test_intrinsic_model ${PROJECT_NAME})
+
+catkin_add_gtest(test_noise_model test/test_noise_model.cpp)
+target_link_libraries(test_noise_model ${PROJECT_NAME})
+
+catkin_add_gtest(test_imu_buffer test/test_imu_buffer.cpp)
+target_link_libraries(test_imu_buffer ${PROJECT_NAME})
+
+catkin_add_gtest(test_imu_types test/test_imu_types.cpp)
+target_link_libraries(test_imu_types ${PROJECT_NAME})
+
+##########
+# EXPORT #
+##########
+cs_install()
+cs_export()
diff --git a/RWR/src/ze_oss/ze_imu/include/ze/imu/accelerometer_model.hpp b/RWR/src/ze_oss/ze_imu/include/ze/imu/accelerometer_model.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..9b26fb04b53b051071548c70a0c7228f9133493e
--- /dev/null
+++ b/RWR/src/ze_oss/ze_imu/include/ze/imu/accelerometer_model.hpp
@@ -0,0 +1,69 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#pragma once
+
+#include <ze/imu/imu_intrinsic_model.hpp>
+#include <ze/imu/imu_noise_model.hpp>
+#include <ze/common/macros.hpp>
+#include <ze/common/types.hpp>
+
+namespace ze {
+
+//! Accelerometer Model
+class AccelerometerModel
+{
+public:
+  ZE_POINTER_TYPEDEFS(AccelerometerModel);
+
+  typedef VectorX measurement_t;
+
+  AccelerometerModel() = delete;
+
+  AccelerometerModel(ImuIntrinsicModel::Ptr intrinsicModel,
+                     ImuNoiseModel::Ptr noiseModel);
+
+  //! These models may depend on both angular and linear quantities as well as
+  //! higher order time derivatives of the quantities. A measurement is
+  //! composed exclusively of either angular or linear quantities and features
+  //! time derivatives in increasing order starting from 0.
+  Vector3 distort(const Eigen::Ref<const measurement_t>& a,
+                  const Eigen::Ref<const measurement_t>& w) const;
+  Vector3 undistort(const Eigen::Ref<const measurement_t>& a,
+                    const Eigen::Ref<const measurement_t>& w) const;
+
+  // getters
+  inline const ImuNoiseModel::Ptr noiseModel() const { return noiseModel_; }
+  inline const ImuIntrinsicModel::Ptr intrinsicModel() const
+  {
+    return intrinsicModel_;
+  }
+
+private:
+  const ImuIntrinsicModel::Ptr intrinsicModel_;
+  const ImuNoiseModel::Ptr noiseModel_;
+};
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_imu/include/ze/imu/gyroscope_model.hpp b/RWR/src/ze_oss/ze_imu/include/ze/imu/gyroscope_model.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..b84ee74abb4b06a8a74f88bd63c06e585f7a7f05
--- /dev/null
+++ b/RWR/src/ze_oss/ze_imu/include/ze/imu/gyroscope_model.hpp
@@ -0,0 +1,65 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#pragma once
+
+#include <ze/imu/imu_intrinsic_model.hpp>
+#include <ze/imu/imu_noise_model.hpp>
+#include <ze/common/macros.hpp>
+#include <ze/common/types.hpp>
+namespace ze {
+
+//! Gyroscope Model
+class GyroscopeModel
+{
+public:
+  ZE_POINTER_TYPEDEFS(GyroscopeModel);
+
+  typedef VectorX measurement_t;
+
+  GyroscopeModel() = delete;
+
+  GyroscopeModel(ImuIntrinsicModel::Ptr intrinsicModel,
+                 ImuNoiseModel::Ptr noiseModel);
+
+  //! These models may depend on both angular and linear quantities as well as
+  //! higher order time derivatives of the quantities. A measurement is
+  //! composed exclusively of either angular or linear quantities and features
+  //! time derivatives in increasing order starting from 0.
+  Vector3 distort(const Eigen::Ref<const measurement_t>& w,
+                  const Eigen::Ref<const measurement_t>& a) const;
+  Vector3 undistort(const Eigen::Ref<const measurement_t>& w,
+                    const Eigen::Ref<const measurement_t>& a) const;
+
+  // getters
+  inline const ImuNoiseModel::Ptr noiseModel() const { return noiseModel_; }
+  inline const ImuIntrinsicModel::Ptr intrinsicModel() const { return intrinsicModel_; }
+
+private:
+  const ImuIntrinsicModel::Ptr intrinsicModel_;
+  const ImuNoiseModel::Ptr noiseModel_;
+};
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_imu/include/ze/imu/imu_buffer.hpp b/RWR/src/ze_oss/ze_imu/include/ze/imu/imu_buffer.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..0892c86378ff0d509cb70f62d7b3312a27fff1b2
--- /dev/null
+++ b/RWR/src/ze_oss/ze_imu/include/ze/imu/imu_buffer.hpp
@@ -0,0 +1,150 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#pragma once
+
+#include <mutex>
+
+#include <ze/imu/imu_model.hpp>
+#include <ze/common/ringbuffer.hpp>
+
+namespace ze {
+
+struct InterpolatorDifferentiatorLinear
+{
+  typedef Vector6 return_t;
+
+  //! The name could be more descriptive, but the current naming allows for
+  //! using the interpolators defined in ringbuffer.h
+  template<typename Ringbuffer_T>
+  static return_t interpolate(
+      Ringbuffer_T* buffer,
+      int64_t time,
+      const typename Ringbuffer_T::timering_t::iterator it_before)
+  {
+    // the end value
+    const auto it_after = it_before + 1;
+    if (it_after == buffer->times().end())
+    {
+      LOG(WARNING) << "Interpolation hit end of buffer.";
+      return (return_t() << buffer->data().col(it_before.container_index()),
+          Vector3::Zero()).finished();
+    }
+
+    const real_t offset = static_cast<real_t>(time - *it_before);
+    const real_t duration = static_cast<real_t>(*it_after - *it_before);
+    const Vector3 before = buffer->data().col(it_before.container_index());
+    const Vector3 after = buffer->data().col(it_after.container_index());
+
+    return (return_t() << before + (after - before) * offset / duration,
+        (after - before) / duration).finished();
+  }
+
+  template<typename Ringbuffer_T>
+  static return_t interpolate(
+      Ringbuffer_T* buffer,
+      int64_t time)
+  {
+    const auto it_before = buffer->iterator_equal_or_before(time);
+    // caller should check the bounds:
+    CHECK(it_before != buffer->times().end());
+
+    return interpolate(buffer, time, it_before);
+  }
+};
+using DefaultInterpolator = InterpolatorLinear;
+
+//! An IMU Buffer with an underlying Gyro and Accel model that also corrects
+//! measurement timestamps. The timestamps are corrected when inserted into the
+//! buffers.
+template<int BufferSize, typename GyroInterp,
+typename AccelInterp = GyroInterp>
+class ImuBuffer
+{
+public:
+  ZE_POINTER_TYPEDEFS(ImuBuffer);
+
+  ImuBuffer(ImuModel::Ptr imu_model);
+
+  void insertGyroscopeMeasurement(time_t stamp, const Vector3);
+  void insertAccelerometerMeasurement(time_t stamp, const Vector3);
+
+  //! Insert an IMU measurement at a given timestamp: First three values refer
+  //! to the accelerometer, last 3 the gyroscope.
+  void insertImuMeasurement(int64_t time, const ImuAccGyr value);
+
+  //! Get the rectified values of the IMU at a given timestamp. Interpolates
+  //! if necessary.
+  //! Return flag if successful
+  bool get(int64_t time, Eigen::Ref<ImuAccGyr> out);
+
+  //! Get all values between two timestamps, synchronize Accelerometer and
+  //! Gyroscope, interpolate edges to fit start and end. Interpolates Gyro
+  //! and Accel measurements to have equal timestamps. Rectify all measurements.
+  std::pair<ImuStamps, ImuAccGyrContainer>
+  getBetweenValuesInterpolated(int64_t stamp_from, int64_t stamp_to);
+
+  //! Get the oldest and newest timestamps for which both Accelerometers
+  //! and Gyroscopes have measurements.
+  std::tuple<int64_t, int64_t, bool> getOldestAndNewestStamp() const;
+
+  //! Get the delay corrected timestamps (Delays are negative if in the past).
+  inline int64_t correctStampGyro(int64_t t)
+  {
+    return t + gyro_delay_;
+  }
+  inline int64_t correctStampAccel(int64_t t)
+  {
+     return t + accel_delay_;
+  }
+
+protected:
+  bool getAccelerometerDistorted(int64_t time, Eigen::Ref<Vector3> out);
+  bool getGyroscopeDistorted(int64_t time, Eigen::Ref<Vector3> out);
+
+private:
+  //! The underlying storage structures for accelerometer and gyroscope
+  //! measurements.
+  Ringbuffer<real_t, 3, BufferSize> acc_buffer_;
+  Ringbuffer<real_t, 3, BufferSize> gyr_buffer_;
+
+  ImuModel::Ptr imu_model_;
+
+  //! Store the accelerometer and gyroscope delays in nanoseconds
+  int64_t gyro_delay_;
+  int64_t accel_delay_;
+};
+
+// A set of explicit declarations
+typedef ImuBuffer<2000, InterpolatorLinear> ImuBufferLinear2000;
+typedef ImuBuffer<5000, InterpolatorLinear> ImuBufferLinear5000;
+typedef ImuBuffer<2000, InterpolatorNearest> ImuBufferNearest2000;
+typedef ImuBuffer<5000, InterpolatorNearest> ImuBufferNearest5000;
+typedef ImuBuffer<2000, InterpolatorDifferentiatorLinear, InterpolatorLinear>
+ImuBufferDiff2000;
+typedef ImuBuffer<5000, InterpolatorDifferentiatorLinear, InterpolatorLinear>
+ImuBufferDiff5000;
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_imu/include/ze/imu/imu_intrinsic_model.hpp b/RWR/src/ze_oss/ze_imu/include/ze/imu/imu_intrinsic_model.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..4523525f4ddaa5e1589b285a16517c9ffc9adb61
--- /dev/null
+++ b/RWR/src/ze_oss/ze_imu/include/ze/imu/imu_intrinsic_model.hpp
@@ -0,0 +1,215 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#pragma once
+
+#include <Eigen/LU>
+
+#include <ze/common/logging.hpp>
+#include <ze/common/macros.hpp>
+#include <ze/common/matrix.hpp>
+#include <ze/common/types.hpp>
+
+namespace ze {
+
+enum class ImuIntrinsicType
+{
+  Calibrated,
+  ScaleMisalignment,
+  ScaleMisalignmentGSensitivity,
+  ScaleMisalignmentSizeEffect
+};
+
+
+//! Base Class for Intrinsic Models for both Accels and Gyros
+class ImuIntrinsicModel
+{
+public:
+  ZE_POINTER_TYPEDEFS(ImuIntrinsicModel);
+
+  //! The inertial measurement models potentially depend on angular as well as
+  //! linear quantities. Depending on whether a gyroscope or an accelerometer
+  //! is modelled, the roles might be reversed. Accordingly, the 'primary'
+  //! refers to the domain of the measurement that is being modelled (angular
+  //! or linear) and the secondary on a measurement of the other domain
+  //! respectively.
+
+  typedef VectorX primary_measurement_t;
+  typedef VectorX secondary_measurement_t;
+
+  explicit ImuIntrinsicModel(ImuIntrinsicType type);
+  ImuIntrinsicModel(ImuIntrinsicType type, real_t delay, real_t range);
+
+  static constexpr real_t UndefinedRange = -1.;
+
+  inline ImuIntrinsicType type() const { return type_; }
+  std::string typeAsString() const;
+
+  virtual Vector3 distort(const Eigen::Ref<const primary_measurement_t>& primary,
+                          const Eigen::Ref<const secondary_measurement_t>& secondary) const = 0;
+
+  virtual Vector3 undistort(const Eigen::Ref<const primary_measurement_t>& primary,
+                            const Eigen::Ref<const secondary_measurement_t>& secondary) const = 0;
+
+  // getters
+  inline real_t delay() const { return delay_; }
+  inline real_t range() const { return range_; }
+
+private:
+  ImuIntrinsicType type_;
+  real_t delay_;
+  real_t range_;
+};
+
+//------------------------------------------------------------------------------
+// Calibrated
+class ImuIntrinsicModelCalibrated: public ImuIntrinsicModel
+{
+public:
+  ZE_POINTER_TYPEDEFS(ImuIntrinsicModelCalibrated);
+  static constexpr ImuIntrinsicType Type = ImuIntrinsicType::Calibrated;
+
+  using ImuIntrinsicModel::primary_measurement_t;
+  using ImuIntrinsicModel::secondary_measurement_t;
+
+  ImuIntrinsicModelCalibrated();
+  ImuIntrinsicModelCalibrated(real_t delay, real_t range);
+
+  virtual Vector3 distort(const Eigen::Ref<const primary_measurement_t>& primary,
+			  const Eigen::Ref<const secondary_measurement_t>& secondary) const;
+
+  virtual Vector3 undistort(const Eigen::Ref<const primary_measurement_t>& primary,
+			    const Eigen::Ref<const secondary_measurement_t>& secondary) const;
+};
+
+//------------------------------------------------------------------------------
+// Scale Misalignment
+class ImuIntrinsicModelScaleMisalignment : public ImuIntrinsicModel
+{
+public:
+  ZE_POINTER_TYPEDEFS(ImuIntrinsicModelScaleMisalignment);
+  static constexpr ImuIntrinsicType Type = ImuIntrinsicType::ScaleMisalignment;
+
+  using ImuIntrinsicModel::primary_measurement_t;
+  using ImuIntrinsicModel::secondary_measurement_t;
+
+  //! delay, range, bias, scale misalignment matrix
+  ImuIntrinsicModelScaleMisalignment(real_t delay, real_t range,
+                                     const Vector3& b, const Matrix3& M);
+
+  virtual Vector3 distort(const Eigen::Ref<const primary_measurement_t>& primary,
+			  const Eigen::Ref<const secondary_measurement_t>& secondary) const;
+
+  virtual Vector3 undistort(const Eigen::Ref<const primary_measurement_t>& primary,
+			    const Eigen::Ref<const secondary_measurement_t>& secondary) const;
+
+  // getters
+  inline const Vector3& b() const { return b_; }
+  inline const Matrix3& M() const { return M_; }
+
+private:
+  Vector3 b_;
+  Matrix3 M_;
+  Matrix3 M_inverse_;
+};
+
+//------------------------------------------------------------------------------
+// Scale Misalignment g-Sensitivity
+class ImuIntrinsicModelScaleMisalignmentGSensitivity : public ImuIntrinsicModel
+{
+public:
+  ZE_POINTER_TYPEDEFS(ImuIntrinsicModelScaleMisalignmentGSensitivity);
+
+  static constexpr ImuIntrinsicType Type =
+      ImuIntrinsicType::ScaleMisalignmentGSensitivity;
+
+  using ImuIntrinsicModel::primary_measurement_t;
+  using ImuIntrinsicModel::secondary_measurement_t;
+
+  //! This model applies exclusively to gyroscopes.
+  //! delay, range, bias, scale misalignment matrix, g-sensitivity matrix
+  ImuIntrinsicModelScaleMisalignmentGSensitivity(real_t delay,
+                                                 real_t range,
+                                                 const Vector3& b,
+                                                 const Matrix3& M,
+                                                 const Matrix3& Ma);
+
+  virtual Vector3 distort(const Eigen::Ref<const primary_measurement_t>& primary,
+			  const Eigen::Ref<const secondary_measurement_t>& secondary) const;
+
+  virtual Vector3 undistort(const Eigen::Ref<const primary_measurement_t>& primary,
+			    const Eigen::Ref<const secondary_measurement_t>& secondary) const;
+
+  // getters
+  inline const Vector3& b() const { return b_; }
+  inline const Matrix3& M() const { return M_; }
+  inline const Matrix3& Ma() const { return Ma_; }
+
+private:
+  Vector3 b_;
+  Matrix3 M_;
+  Matrix3 M_inverse_;
+  Matrix3 Ma_;
+};
+
+//------------------------------------------------------------------------------
+// Scale MisalignmentSize Effect
+class ImuIntrinsicModelScaleMisalignmentSizeEffect : public ImuIntrinsicModel
+{
+public:
+  ZE_POINTER_TYPEDEFS(ImuIntrinsicModelScaleMisalignmentSizeEffect);
+  static constexpr ImuIntrinsicType Type =
+      ImuIntrinsicType::ScaleMisalignmentSizeEffect;
+
+  using ImuIntrinsicModel::primary_measurement_t;
+  using ImuIntrinsicModel::secondary_measurement_t;
+
+  //! This model applies exclusively to accelerometers.
+  //! delay, range, bias, scale misalignment matrix, accel. column position vectors
+  ImuIntrinsicModelScaleMisalignmentSizeEffect(real_t delay,
+                                               real_t range,
+                                               const Vector3& b,
+                                               const Matrix3& M,
+                                               const Matrix3& R);
+
+  virtual Vector3 distort(const Eigen::Ref<const primary_measurement_t>& primary,
+			  const Eigen::Ref<const secondary_measurement_t>& secondary) const;
+
+  virtual Vector3 undistort(const Eigen::Ref<const primary_measurement_t>& primary,
+			    const Eigen::Ref<const secondary_measurement_t>& secondary) const;
+
+  // getters
+  inline const Vector3& b() const { return b_; }
+  inline const Matrix3& M() const { return M_; }
+  inline const Matrix3& R() const { return R_; }
+
+private:
+  Vector3 b_;
+  Matrix3 M_;
+  Matrix3 M_inverse_;
+  Matrix3 R_;
+};
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_imu/include/ze/imu/imu_model.hpp b/RWR/src/ze_oss/ze_imu/include/ze/imu/imu_model.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..c02f8500bf0fe0ad437c16136db7065049e4bc80
--- /dev/null
+++ b/RWR/src/ze_oss/ze_imu/include/ze/imu/imu_model.hpp
@@ -0,0 +1,82 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#pragma once
+
+#include <string>
+
+#include <ze/imu/accelerometer_model.hpp>
+#include <ze/imu/gyroscope_model.hpp>
+#include <ze/imu/imu_yaml_serialization.hpp>
+#include <ze/common/macros.hpp>
+#include <ze/common/types.hpp>
+
+namespace ze {
+
+//! Imu Model
+class ImuModel
+{
+public:
+  ZE_POINTER_TYPEDEFS(ImuModel);
+
+  typedef VectorX measurement_t;
+
+  ImuModel() = delete;
+
+  ImuModel(const AccelerometerModel::Ptr accelerometer,
+           const GyroscopeModel::Ptr gyroscope);
+
+  //! Load an imu from a yaml file. Returns a nullptr if the loading fails.
+  static Ptr loadFromYaml(const std::string& path);
+
+  void setLabel(const std::string& label) { label_ = label; }
+  void setId(const std::string& id) { id_ = id; }
+  std::string label() const { return label_; }
+  std::string id() const { return id_; }
+
+  Vector6 distort(const Eigen::Ref<const measurement_t>& primary,
+                  const Eigen::Ref<const measurement_t>& secondary) const;
+  Vector6 undistort(const Eigen::Ref<const measurement_t>& primary,
+                    const Eigen::Ref<const measurement_t>& secondary) const;
+
+  // getters
+  inline const AccelerometerModel::Ptr accelerometerModel() const
+  {
+    return accelerometerModel_;
+  }
+  inline const GyroscopeModel::Ptr gyroscopeModel() const
+  {
+    return gyroscopeModel_;
+  }
+
+private:
+  std::string id_;
+  std::string label_;
+
+  const AccelerometerModel::Ptr accelerometerModel_;
+  const GyroscopeModel::Ptr gyroscopeModel_;
+};
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_imu/include/ze/imu/imu_noise.hpp b/RWR/src/ze_oss/ze_imu/include/ze/imu/imu_noise.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..5735bb85a3fe8aa819e4098cdf82ccdf318bfc67
--- /dev/null
+++ b/RWR/src/ze_oss/ze_imu/include/ze/imu/imu_noise.hpp
@@ -0,0 +1,25 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
diff --git a/RWR/src/ze_oss/ze_imu/include/ze/imu/imu_noise_model.hpp b/RWR/src/ze_oss/ze_imu/include/ze/imu/imu_noise_model.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..5266a552f894a7f8fdbbba246c3a9d1b3e6165e0
--- /dev/null
+++ b/RWR/src/ze_oss/ze_imu/include/ze/imu/imu_noise_model.hpp
@@ -0,0 +1,92 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#pragma once
+
+#include <string>
+
+#include <ze/common/logging.hpp>
+#include <ze/common/macros.hpp>
+#include <ze/common/types.hpp>
+
+namespace ze {
+
+enum class ImuNoiseType
+{
+  None,
+  WhiteBrownian
+};
+
+//------------------------------------------------------------------------------
+// Noise model base class
+class ImuNoiseModel
+{
+public:
+  ZE_POINTER_TYPEDEFS(ImuNoiseModel);
+
+  explicit ImuNoiseModel(ImuNoiseType type);
+
+  inline ImuNoiseType type() const { return type_; }
+  std::string typeAsString() const;
+
+ private:
+  ImuNoiseType type_;
+};
+
+//------------------------------------------------------------------------------
+// No noise model
+class ImuNoiseNone: public ImuNoiseModel
+{
+public:
+  ZE_POINTER_TYPEDEFS(ImuNoiseNone);
+  static constexpr ImuNoiseType Type = ImuNoiseType::None;
+
+  ImuNoiseNone();
+};
+
+//------------------------------------------------------------------------------
+// White brownian noise model
+class ImuNoiseWhiteBrownian: public ImuNoiseModel
+{
+public:
+  ZE_POINTER_TYPEDEFS(ImuNoiseWhiteBrownian);
+  static constexpr ImuNoiseType Type = ImuNoiseType::WhiteBrownian;
+
+  ImuNoiseWhiteBrownian(real_t noise_density,
+                        real_t bandwidth,
+                        real_t bias_noise_density);
+
+  // getters
+  inline real_t noiseDensity() const { return noise_density_; }
+  inline real_t bandwidth() const { return bandwidth_; }
+  inline real_t biasNoiseDensity() const { return bias_noise_density_; }
+
+private:
+  real_t noise_density_;
+  real_t bandwidth_;
+  real_t bias_noise_density_;
+};
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_imu/include/ze/imu/imu_rig.hpp b/RWR/src/ze_oss/ze_imu/include/ze/imu/imu_rig.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..aa67719d6dedf3b2ff850304751438e493c29f28
--- /dev/null
+++ b/RWR/src/ze_oss/ze_imu/include/ze/imu/imu_rig.hpp
@@ -0,0 +1,132 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#pragma once
+
+#include <string>
+#include <vector>
+
+#include <ze/imu/imu_model.hpp>
+#include <ze/common/macros.hpp>
+#include <ze/common/types.hpp>
+#include <ze/common/transformation.hpp>
+
+namespace ze {
+
+using ImuVector = std::vector<ImuModel::Ptr>;
+
+class ImuRig
+{
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  ZE_POINTER_TYPEDEFS(ImuRig);
+
+  ImuRig() = delete;
+
+  ImuRig(
+      const TransformationVector& T_B_S,
+      const ImuVector& imus,
+      const std::string& label);
+
+  //! Load a imu rig form a yaml file. Returns a nullptr if the loading fails.
+  static ImuRig::Ptr loadFromYaml(const std::string& yaml_file);
+
+  //! @name Imu poses with respect to body frame.
+  //! @{
+  inline const Transformation& T_S_B(size_t imu_index) const
+  {
+    DEBUG_CHECK_LT(imu_index, T_S_B_.size());
+    return T_S_B_[imu_index];
+  }
+
+  inline const TransformationVector& T_S_B_vec() const
+  {
+    return T_S_B_;
+  }
+
+  inline const Transformation& T_B_S(size_t imu_index) const
+  {
+    DEBUG_CHECK_LT(imu_index, T_B_S_.size());
+    return T_B_S_[imu_index];
+  }
+
+  inline const TransformationVector& T_B_S_vec() const
+  {
+    return T_B_S_;
+  }
+  //! @}
+
+  //! @name Imu accessors.
+  //! @{
+  inline const ImuModel& at(size_t imu_index) const
+  {
+    return *imus_.at(imu_index);
+  }
+
+  inline ImuModel::Ptr atShared(size_t imu_index)
+  {
+    return imus_.at(imu_index);
+  }
+
+  inline std::shared_ptr<const ImuModel> atShared(size_t imu_index) const
+  {
+    return imus_.at(imu_index);
+  }
+
+  inline const ImuVector& imus() const { return imus_; }
+  //! @}
+
+  inline size_t size() const { return imus_.size(); }
+
+  inline const std::string& label() const { return label_; }
+
+  //! @name Imu iteration.
+  //! @{
+  typedef ImuVector::value_type value_type;
+  typedef ImuVector::iterator iterator;
+  typedef ImuVector::const_iterator const_iterator;
+  ImuVector::iterator begin() { return imus_.begin(); }
+  ImuVector::iterator end() { return imus_.end(); }
+  ImuVector::const_iterator begin() const { return imus_.begin(); }
+  ImuVector::const_iterator end() const { return imus_.end(); }
+  ImuVector::const_iterator cbegin() const { return imus_.cbegin(); }
+  ImuVector::const_iterator cend() const { return imus_.cend(); }
+  //! @}
+
+private:
+  //! The mounting transformations.
+  TransformationVector T_S_B_;
+  TransformationVector T_B_S_;
+
+  //! The imu models.
+  ImuVector imus_;
+
+  //! The rig label
+  std::string label_;
+
+};
+
+} // namespace ze
+
diff --git a/RWR/src/ze_oss/ze_imu/include/ze/imu/imu_types.hpp b/RWR/src/ze_oss/ze_imu/include/ze/imu/imu_types.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..9455234a0f370ce5fc9382798cc0d3c0068a9d5f
--- /dev/null
+++ b/RWR/src/ze_oss/ze_imu/include/ze/imu/imu_types.hpp
@@ -0,0 +1,56 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#pragma once
+
+#include <ze/common/types.hpp>
+
+namespace ze {
+
+//! Extract the Accelerometer component of an ImuAccGyr block.
+inline Eigen::VectorBlock<const Vector6, 3> getAcc(const ImuAccGyr& imu)
+{
+    return imu.head<3>();
+}
+
+//! Extract the Gyroscope component of an ImuAccGyr block.
+inline Eigen::VectorBlock<const Vector6, 3> getGyr(const ImuAccGyr& imu)
+{
+  return imu.tail<3>();
+}
+
+//! Extract the Accelerometer component of an ImuAccGyr block.
+inline Eigen::VectorBlock<Vector6, 3> getAcc(ImuAccGyr& imu)
+{
+    return imu.head<3>();
+}
+
+//! Extract the Gyroscope component of an ImuAccGyr block.
+inline Eigen::VectorBlock<Vector6, 3> getGyr(ImuAccGyr& imu)
+{
+  return imu.tail<3>();
+}
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_imu/include/ze/imu/imu_yaml_serialization.hpp b/RWR/src/ze_oss/ze_imu/include/ze/imu/imu_yaml_serialization.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..49fc5361c4147d1cad36059f82a3858b9d52f31d
--- /dev/null
+++ b/RWR/src/ze_oss/ze_imu/include/ze/imu/imu_yaml_serialization.hpp
@@ -0,0 +1,69 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#pragma once
+
+#include <memory>
+
+#include <ze/imu/accelerometer_model.hpp>
+#include <ze/imu/gyroscope_model.hpp>
+#include <ze/common/logging.hpp>
+#include <ze/common/transformation.hpp>
+#include <yaml-cpp/yaml.h>
+
+namespace ze {
+ class ImuModel;
+ class ImuRig;
+}
+
+namespace YAML {
+
+template<>
+struct convert<std::shared_ptr<ze::ImuModel>>
+{
+  static bool decode(const Node& node,  std::shared_ptr<ze::ImuModel>& imu);
+  static Node encode(const std::shared_ptr<ze::ImuModel>& imu);
+};
+
+template<>
+struct convert<std::shared_ptr<ze::ImuRig>>
+{
+  static bool decode(const Node& node,  std::shared_ptr<ze::ImuRig>& imu);
+  static Node encode(const std::shared_ptr<ze::ImuRig>& imu);
+};
+
+struct internal
+{
+  static typename std::shared_ptr<ze::ImuIntrinsicModel> decodeIntrinsics(
+      const Node& node);
+
+  static typename std::shared_ptr<ze::ImuNoiseModel> decodeNoise(
+      const Node& node);
+
+  static bool validateNoise(const std::string& value);
+  static bool validateIntrinsic(const std::string& value);
+};
+
+}  // namespace YAML
diff --git a/RWR/src/ze_oss/ze_imu/package.xml b/RWR/src/ze_oss/ze_imu/package.xml
new file mode 100644
index 0000000000000000000000000000000000000000..e5cbad85e4c01534839f4d2e21dd7e05c1e60b1e
--- /dev/null
+++ b/RWR/src/ze_oss/ze_imu/package.xml
@@ -0,0 +1,26 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>ze_imu</name>
+  <version>0.1.4</version>
+  <description>
+    Common IMU tools for Zurich Eye.
+  </description>
+
+  <maintainer email="christian.forster@WyssZurich.ch">Christian Forster</maintainer>
+  <license>ZE</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <buildtool_depend>catkin_simple</buildtool_depend>
+
+  <depend>eigen_catkin</depend>
+  <depend>glog_catkin</depend>
+  <depend>minkindr</depend>
+  <depend>yaml_cpp_catkin</depend>
+  <depend>ze_cmake</depend>
+  <depend>ze_common</depend>
+  <depend>rospy</depend>
+
+  <test_depend>gtest</test_depend>
+
+  <export></export>
+</package>
\ No newline at end of file
diff --git a/RWR/src/ze_oss/ze_imu/py/ze_imu/__init__.py b/RWR/src/ze_oss/ze_imu/py/ze_imu/__init__.py
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/RWR/src/ze_oss/ze_imu/py/ze_imu/imu_integration.py b/RWR/src/ze_oss/ze_imu/py/ze_imu/imu_integration.py
new file mode 100644
index 0000000000000000000000000000000000000000..7e8f49f94d4381bd2e035885fef2bcf9dd620196
--- /dev/null
+++ b/RWR/src/ze_oss/ze_imu/py/ze_imu/imu_integration.py
@@ -0,0 +1,371 @@
+#!/usr/bin/python
+"""
+Zurich Eye
+"""
+
+import numpy as np
+import matplotlib.pyplot as plt
+import ze_py.plot_utils as plot_utils
+import ze_py.transformations as tf
+from pylab import setp
+import ze_imu.imu_trajectory_simulation as imu_sim
+from mpl_toolkits.mplot3d import Axes3D
+from matplotlib import rc
+
+# tell matplotlib to use latex font
+rc('font',**{'family':'serif','serif':['Cardo']})
+rc('text', usetex=True)
+
+gravity = 9.81
+g = np.array([0, 0, -gravity])
+
+def renormalize_quaternion_in_state(state):
+    state[:4] /= np.linalg.norm(state[:4])
+    return state
+       
+def integrate_expmap(t_W_B0, v_W_B0, R_W_B0, B_a_W_B, B_w_W_B, dt):
+    n = np.shape(B_a_W_B)[0]
+    t_W_B = np.zeros((n,3))
+    t_W_B[0,:] = t_W_B0
+    v_W_B = np.zeros((n,3))
+    v_W_B[0,:] = v_W_B0
+    R_W_B = np.zeros((n, 9))
+    R_W_B[0,:] = R_W_B0
+    for i in range(1,n):
+        R = np.reshape(R_W_B[i-1,:], (3,3))
+        a = B_a_W_B[i-1,:]
+        w = B_w_W_B[i-1,:]
+        t_W_B[i,:] = t_W_B[i-1,:] + v_W_B[i-1,:] * dt  + g * (dt**2) * 0.5  + np.dot(R, a * (dt**2) * 0.5)
+        v_W_B[i,:] = v_W_B[i-1,:] + g * dt + np.dot(R, a * dt)
+        R_W_B[i,:] = np.reshape(np.dot(R, tf.expmap_so3(w * dt)), (9,))
+      
+    return R_W_B, v_W_B, t_W_B
+     
+def integrate_quaternion_zero_order_hold(t_W_B0, v_W_B0, R_W_B0, B_a_W_B, B_w_W_B, dt):
+    n = np.shape(B_a_W_B)[0]
+    t_W_B = np.zeros((n,3))
+    t_W_B[0,:] = t_W_B0
+    v_W_B = np.zeros((n,3))
+    v_W_B[0,:] = v_W_B0
+    R_W_B = np.zeros((n, 9))
+    R_W_B[0,:] = R_W_B0
+    for i in range(1,n):
+        # Last state:
+        R_W_Bk = np.reshape(R_W_B[i-1,:], (3,3))
+        #q_Bk_W = tf.quaternion_from_matrix(tf.convert_3x3_to_4x4(np.transpose(R_W_Bk)))
+        q_Bk_W = tf.quaternion_from_matrix(tf.convert_3x3_to_4x4(R_W_Bk))
+        a = B_a_W_B[i-1,:]
+        w = B_w_W_B[i-1,:]
+ 
+        Theta = np.eye(4) + 0.5 * dt * tf.quat_Omega(w) # Eq. 124 Quat. Tutorial
+        q_Bkp1_W = np.dot(Theta, q_Bk_W)                # Eq. 111 Quat. Tutorial
+        #R_W_B[i,:] = np.reshape(np.transpose(tf.matrix_from_quaternion(q_Bkp1_W)[:3,:3]), (9,))
+        R_W_B[i,:] = np.reshape(tf.matrix_from_quaternion(q_Bkp1_W)[:3,:3], (9,))
+        v_W_B[i,:] = v_W_B[i-1,:] + g * dt + np.dot(R_W_Bk, a) * dt
+        t_W_B[i,:] = t_W_B[i-1,:] + v_W_B[i-1,:] * dt
+    
+    return R_W_B, v_W_B, t_W_B
+   
+def integrate_quaternion_runge_kutta_4(t_W_B0, v_W_B0, R_W_B0, B_a_W_B, B_w_W_B, dt):
+    n = np.shape(B_a_W_B)[0]
+    t_W_B = np.zeros((n,3))from pylab import setp
+    t_W_B[0,:] = t_W_B0
+    v_W_B = np.zeros((n,3))
+    v_W_B[0,:] = v_W_B0
+    R_W_B = np.zeros((n, 9))
+    R_W_B[0,:] = R_W_B0
+    
+    def state_derivative(imu_acc, imu_gyr, state, dt):
+        q_W_B = state[:4]
+        v     = state[4:7]
+        R_W_B = tf.matrix_from_quaternion(q_W_B)[:3,:3]
+        Omega = tf.quat_Omega(imu_gyr)
+        
+        # Quaternion derivative according to MARS-Lab Quaternion Tutorial.
+        q_dot = 0.5 * np.dot(Omega, q_W_B)
+        v_dot = np.array([0.0, 0.0, -gravity]) + np.dot(R_W_B, imu_acc)
+        p_dot = v # + dt * np.array([0.0, 0.0, -gravity]) + dt * np.dot(R_W_B, imu_acc)
+        state_dot = np.concatenate((q_dot, v_dot, p_dot))
+        return state_dot
+    
+    for i in range(1,n):
+        # Last state:
+        R = np.reshape(R_W_B[i-1,:], (3,3))
+        q_W_B = tf.quaternion_from_matrix(tf.convert_3x3_to_4x4(R))
+        v = v_W_B[i-1,:]
+        p = t_W_B[i-1,:]
+      
+        # Get imu measurements from last, current timestamp and interpolate in between.
+        imu_acc_1 = B_a_W_B[i-1,:]
+        imu_acc_3 = B_a_W_B[i,:]
+        imu_acc_2 = 0.5 * (imu_acc_1 + imu_acc_3)
+        imu_gyr_1 = B_w_W_B[i-1,:]
+        imu_gyr_3 = B_w_W_B[i,:]
+        imu_gyr_2 = 0.5 * (imu_gyr_1 + imu_gyr_3)
+      
+        # Runge-Kutta Integration:
+        state_1 = np.concatenate((q_W_B, v, p))
+        state_1_dot = state_derivative(imu_acc_1, imu_gyr_1, state_1, 0.0)
+        
+        state_2 = state_1 + 0.5 * dt * state_1_dot
+        state_2 = renormalize_quaternion_in_state(state_2)
+        state_2_dot = state_derivative(imu_acc_2, imu_gyr_2, state_2, 0.5 * dt)
+        
+        state_3 = state_1 + 0.5 * dt * state_2_dot
+        state_3 = renormalize_quaternion_in_state(state_3)
+        state_3_dot = state_derivative(imu_acc_2, imu_gyr_2, state_3, 0.5 * dt)
+        
+        state_4 = state_1 + 1.0 * dt * state_3_dot
+        state_4 = renormalize_quaternion_in_state(state_4)
+        state_4_dot = state_derivative(imu_acc_3, imu_gyr_3, state_4, dt)
+        
+        integrated_state = \
+            state_1 + 1.0 / 6.0 * dt * (state_1_dot + 2.0 * state_2_dot + 2.0 * state_3_dot + state_4_dot) 
+        integrated_state = renormalize_quaternion_in_state(integrated_state)
+
+        # Save next state:
+        R = tf.matrix_from_quaternion(integrated_state[:4])[:3,:3]
+        R_W_B[i,:] = np.reshape(R, (9,))
+        v_W_B[i,:] = integrated_state[4:7]
+        t_W_B[i,:] = integrated_state[7:10]
+    
+    return R_W_B, v_W_B, t_W_B
+    
+def rotation_estimation_error(R_W_Bgt, R_W_Bes):
+    n = np.shape(R_W_Bgt)[0]
+    errors = np.zeros(n)
+    for i in range(0,n):
+        R_gt = np.reshape(R_W_Bgt[i,:], (3,3))
+        R_es = np.reshape(R_W_Bes[i,:], (3,3))
+        R_err = np.dot(np.transpose(R_gt), R_es)
+        errors[i] = np.linalg.norm(tf.logmap_so3(R_err))
+    errors = errors * 180.0 / np.pi
+    return errors
+    
+def translation_estimation_errors(t_W_Bgt, t_W_Bes):
+    errors = np.sqrt(np.sum((t_W_Bgt - t_W_Bes)**2, 1))
+    return errors
+    
+def experiment1():
+    
+    t_max = 0.25
+    dt = 1.0/800.0
+    n = int(t_max / dt)
+    
+    # -----------------------------------------------------------------------------
+    # Generate measurements and ground-truth data.
+    
+    t_W_B, v_W_B, R_W_B, B_a_W_B, B_w_W_B = imu_sim.get_simulated_imu_data(t_max, dt)
+    
+    ax = imu_sim.plot_trajectory(t_W_B, R_W_B)
+    
+    # -------------------------------------------------------------------------
+    # Verification: Repeat integration to see how big the integration error is
+    
+    R_expm, v_expm, t_expm = integrate_expmap(
+        t_W_B[0,:], v_W_B[0,:], R_W_B[0,:], B_a_W_B, B_w_W_B, dt)
+        
+    R_qrk4, v_qrk4, t_qrk4 = integrate_quaternion_runge_kutta_4(
+        t_W_B[0,:], v_W_B[0,:], R_W_B[0,:], B_a_W_B, B_w_W_B, dt)
+        
+    ax.plot(t_expm[:,0], t_expm[:,1], t_expm[:,2], '-r', label='Proposed')
+    ax.plot(t_qrk4[:,0], t_qrk4[:,1], t_qrk4[:,2], '-g', label='Runge-Kutta 4th order')
+    ax.legend()
+    plot_utils.axis_equal_3d(ax)
+    
+    
+    # Compute rotation error:
+    R_err_expm = rotation_estimation_error(R_W_B, R_expm)
+    R_err_qrk4 = rotation_estimation_error(R_W_B, R_qrk4)
+    t_err_expm = translation_estimation_errors(t_W_B, t_expm)
+    t_err_qrk4 = translation_estimation_errors(t_W_B, t_qrk4)
+    
+    fig = plt.figure(figsize=(8,4))
+    ax = fig.add_subplot(211, title='Rotation Error', ylabel='Rotation error [deg]')
+    times = np.arange(0, n*dt*1000, dt*1000)
+    ax.plot(times, R_err_expm, '-b', label='Proposed', lw=3)
+    ax.plot(times, R_err_qrk4, '-g', label='Runge-Kutta 4th order', lw=3)
+    
+    ax = fig.add_subplot(212, title='Translation Error', ylabel='Translation error [m]', xlabel='Time [ms]')
+    ax.plot(times, t_err_expm, '-b', lw=3)
+    ax.plot(times, t_err_qrk4, '-g', lw=3)
+    ax.legend()
+
+def experiment2():
+    
+    N = 100
+    t_interval = 0.25 
+    t_max = (N+1) * t_interval
+    dt = 1.0 / 200.0
+    n_per_interval = t_interval / dt
+    
+    # -----------------------------------------------------------------------------
+    # Generate measurements and ground-truth data.
+    
+    t_W_B, v_W_B, R_W_B, B_a_W_B, B_w_W_B = imu_sim.get_simulated_imu_data(t_max, dt)
+    
+    ax, fig = imu_sim.plot_trajectory(t_W_B, R_W_B, interval=10)
+    fig.savefig('imu_sim_trajectory.pdf')
+    
+    # -------------------------------------------------------------------------
+    # Verification: Repeat integration to see how big the integration error is
+    R_err_expm = np.zeros(N)
+    R_err_qrk4 = np.zeros(N)
+    t_err_expm = np.zeros(N)
+    t_err_qrk4 = np.zeros(N)
+    for i in np.arange(N):
+        s = i * n_per_interval
+        e = (i+1) * n_per_interval        
+
+        # Integrate Segment        
+        R_expm, v_expm, t_expm = integrate_expmap(
+            t_W_B[s,:], v_W_B[s,:], R_W_B[s,:], B_a_W_B[s:e,:], B_w_W_B[s:e,:], dt)
+            
+        R_qrk4, v_qrk4, t_qrk4 = integrate_quaternion_runge_kutta_4(
+            t_W_B[s,:], v_W_B[s,:], R_W_B[s,:], B_a_W_B[s:e,:], B_w_W_B[s:e,:], dt)
+        
+        
+        # Compute rotation error:
+        R_err_expm[i] = rotation_estimation_error(R_W_B[s:e,:], R_expm)[-1]
+        R_err_qrk4[i] = rotation_estimation_error(R_W_B[s:e,:], R_qrk4)[-1]
+        t_err_expm[i] = translation_estimation_errors(t_W_B[s:e,:], t_expm)[-1]
+        t_err_qrk4[i] = translation_estimation_errors(t_W_B[s:e,:], t_qrk4)[-1]
+    
+    fig = plt.figure(figsize=(8,8))
+    bins = np.linspace(0,np.max(R_err_expm),20)
+    ax = fig.add_subplot(211, xlabel='Rotation error [deg]')
+    ax.hist(R_err_expm, bins=bins, color='b', label='Proposed')
+    ax.hist(R_err_qrk4, bins=bins, color='g', label='Runge-Kutta 4th order')
+    ax.legend()
+    
+    bins = np.linspace(0,np.max(t_err_expm),20)
+    ax = fig.add_subplot(212, xlabel='Translation error [m]')
+    ax.hist(t_err_expm, bins=bins, color='b')
+    ax.hist(t_err_qrk4, bins=bins, color='g')
+    fig.savefig('imu_sim_error_hist.png')
+    
+    
+def experiment3():
+    
+    N = 50
+    t_interval = 0.25 
+    t_max = (N+1) * t_interval
+    hz_range = np.arange(100, 1000, 200) 
+    
+    R_err_expm_all = []
+    R_err_qrk4_all = []
+    t_err_expm_all = []
+    t_err_qrk4_all = []
+    
+    for hz in hz_range:
+        print('Computing errors for hz = ' + str(hz))
+        
+        dt = 1.0 / hz
+        n_per_interval = t_interval / dt
+        
+        # -----------------------------------------------------------------------------
+        # Generate measurements and ground-truth data.
+        
+        t_W_B, v_W_B, R_W_B, B_a_W_B, B_w_W_B = imu_sim.get_simulated_imu_data(t_max, dt)
+        
+        #ax, fig = imu_sim.plot_trajectory(t_W_B, R_W_B, interval=10)
+        #fig.savefig('imu_sim_trajectory.png')
+        
+        # -------------------------------------------------------------------------
+        # Verification: Repeat integration to see how big the integration error is
+        R_err_expm = np.zeros(N)
+        R_err_qrk4 = np.zeros(N)
+        t_err_expm = np.zeros(N)
+        t_err_qrk4 = np.zeros(N)
+        for i in np.arange(N):
+            s = i * n_per_interval
+            e = (i+1) * n_per_interval        
+    
+            # Integrate Segment        
+            R_expm, v_expm, t_expm = integrate_expmap(
+                t_W_B[s,:], v_W_B[s,:], R_W_B[s,:], B_a_W_B[s:e,:], B_w_W_B[s:e,:], dt)
+                
+            R_qrk4, v_qrk4, t_qrk4 = integrate_quaternion_runge_kutta_4(
+                t_W_B[s,:], v_W_B[s,:], R_W_B[s,:], B_a_W_B[s:e,:], B_w_W_B[s:e,:], dt)
+            
+            
+            # Compute rotation error:
+            R_err_expm[i] = rotation_estimation_error(R_W_B[s:e,:], R_expm)[-1]
+            R_err_qrk4[i] = rotation_estimation_error(R_W_B[s:e,:], R_qrk4)[-1]
+            t_err_expm[i] = translation_estimation_errors(t_W_B[s:e,:], t_expm)[-1]
+            t_err_qrk4[i] = translation_estimation_errors(t_W_B[s:e,:], t_qrk4)[-1]
+        
+        R_err_expm_all.append(R_err_expm)
+        R_err_qrk4_all.append(R_err_qrk4)
+        t_err_expm_all.append(t_err_expm)
+        t_err_qrk4_all.append(t_err_qrk4)
+        
+        
+        
+    def set_boxplot_colors(pb, color):
+        setp(pb['boxes'][0], color=color)
+        setp(pb['caps'][0], color=color)
+        setp(pb['caps'][1], color=color)
+        setp(pb['whiskers'][0], color=color)
+        setp(pb['whiskers'][1], color=color)
+        #setp(pb['fliers'][0], color=color)
+        #setp(pb['fliers'][1], color=color)
+        setp(pb['medians'][0], color=color)
+        
+        
+    n_exp = 2
+    n_dist = len(hz_range)
+    spacing = 2
+    pos = np.arange(0, n_dist*(n_exp+spacing), (n_exp+spacing))
+
+    fig = plt.figure(figsize=(6,5))
+    ax_rot = fig.add_subplot(211, ylabel='Rotation error [deg]')
+    ax_pos = fig.add_subplot(212, ylabel='Translation error [m]', xlabel='IMU Rate [Hz]')
+    
+    dummy_plots_rot = []
+    dummy_plot_rot = ax_rot.plot([1,1], '-', color='b', label="Proposed")
+    dummy_plots_rot.append(dummy_plot_rot[0])    
+    dummy_plot_rot = ax_rot.plot([1,1], '-', color='g', label="Runge-Kutta 4th order")
+    dummy_plots_rot.append(dummy_plot_rot[0])
+
+    for idx_hz, hz in enumerate(hz_range):     
+        pb = ax_rot.boxplot(R_err_expm_all[idx_hz], False, '', positions=[pos[idx_hz]+0], widths=0.8)
+        set_boxplot_colors(pb, 'b')
+    
+        pb = ax_pos.boxplot(t_err_expm_all[idx_hz], False, '', positions=[pos[idx_hz]+0], widths=0.8)
+        set_boxplot_colors(pb, 'b')
+        
+        pb = ax_rot.boxplot(R_err_qrk4_all[idx_hz], False, '', positions=[pos[idx_hz]+1], widths=0.8)
+        set_boxplot_colors(pb, 'g')
+    
+        pb = ax_pos.boxplot(t_err_qrk4_all[idx_hz], False, '', positions=[pos[idx_hz]+1], widths=0.8)
+        set_boxplot_colors(pb, 'g')
+           
+    
+        
+    ax_rot.set_xticks(pos+0.5*n_exp-0.5)
+    ax_rot.set_xticklabels(hz_range)
+    ax_rot.set_xlim(xmin=pos[0]-2, xmax=pos[-1]+5)
+    ax_rot.set_ylim([-0.001, np.max(R_err_expm_all[0])])
+    
+    ax_pos.set_xticks(pos+0.5*n_exp-0.5)
+    ax_pos.set_xticklabels(hz_range)
+    ax_pos.set_xlim(xmin=pos[0]-2, xmax=pos[-1]+5)
+    ax_pos.set_ylim([-0.00001, np.max(t_err_expm_all[0])])
+
+    
+    
+    # create legend
+    ax_rot.legend(dummy_plots_rot, ['Proposed','Runge-Kutta 4th order'], loc='upper right')
+    for p in dummy_plots_rot:
+        p.set_visible(False)
+        
+
+    fig.tight_layout()
+    fig.savefig('imu_sim_errors.pdf', bbox_inches="tight")
+    #
+    #fig_yaw.tight_layout()
+    #fig_yaw.savefig('vicon_yaw_error_comparison'+FORMAT, bbox_inches="tight")
+    #
+    #fig_g.tight_layout()
+    #fig_g.savefig('vicon_gravity_error_comparison'+FORMAT, bbox_inches="tight")
\ No newline at end of file
diff --git a/RWR/src/ze_oss/ze_imu/py/ze_imu/imu_trajectory_simulation.py b/RWR/src/ze_oss/ze_imu/py/ze_imu/imu_trajectory_simulation.py
new file mode 100644
index 0000000000000000000000000000000000000000..6b09b757030557b8e0f67798a0467134e860afc5
--- /dev/null
+++ b/RWR/src/ze_oss/ze_imu/py/ze_imu/imu_trajectory_simulation.py
@@ -0,0 +1,172 @@
+#!/usr/bin/python
+"""
+Zurich Eye
+
+This script requires the installation of Sympy:
+ > sudo apt-get install python3-pip
+ > sudo pip3 install sympy
+"""
+
+import numpy as np
+import sympy as sy
+import matplotlib.pyplot as plt
+import ze_py.plot_utils as plot_utils
+import ze_py.transformations as tf
+from mpl_toolkits.mplot3d import Axes3D
+from matplotlib import rc
+
+# tell matplotlib to use latex font
+rc('font',**{'family':'serif','serif':['Cardo']})
+rc('text', usetex=True)
+
+# Time: 
+t = sy.Symbol('t')
+gravity = 9.81
+
+def sy_unskew(R):
+    return sy.Matrix([R[2,1], R[0,2], R[1,0]])
+    
+def sy_rpy_matrix(theta1, theta2, theta3):
+    # Define the principal rotation matrices:
+    R1 = sy.Matrix([[1,               0,              0],
+                    [0,  sy.cos(theta1), sy.sin(theta1)],
+                    [0, -sy.sin(theta1), sy.cos(theta1)]])
+                    
+    R2 = sy.Matrix([[sy.cos(theta2), 0, -sy.sin(theta2)],
+                    [0,              1,               0],
+                    [sy.sin(theta2), 0,  sy.cos(theta2)]])
+    
+    R3 = sy.Matrix([[ sy.cos(theta3), sy.sin(theta3), 0],
+                    [-sy.sin(theta3), sy.cos(theta3), 0],
+                    [ 0,             0,               1]])
+    
+    # This sequence, which is very common in aerospace applications, is called
+    # the 1-2-3 attitude sequence or the ‘roll-pitch-yaw’ convention.
+    R_W_B = R3 * R2 * R1    
+    
+    # IMPORTANT: a singularity occurs when theta2 = pi/2, in this case theta1
+    #            and theta3 are associated to the same rotation.
+    return R_W_B
+   
+def simulate_rotation_matrix(w1=1, w2=1, w3=1, 
+                             a1=1, a2=1, a3=1,
+                             b1=0, b2=0, b3=0):
+    """The rotation matrix is computed using Euler angles. Each angle is
+    modeled as sinus with amplitude a, frequency w and phase b:
+    """
+    theta1 = a1 * sy.sin(b1 + w1 * t) # yaw
+    theta2 = a2 * sy.sin(b2 + w2 * t) # pitch
+    theta3 = a3 * sy.sin(b3 + w3 * t) # roll
+    R_W_B = sy_rpy_matrix(theta1, theta2, theta3)
+    return R_W_B
+    
+def rotation_rate_from_rotation_matrix(R_W_B):
+    """Computes rotation rate from the derivative of a rotation matrix:
+    body_w_world_body = skew(R_world_body^T * dot(R_world_body))
+    
+    """
+    R_W_B_dot = sy.diff(R_W_B, t)
+    W = R_W_B.T * R_W_B_dot
+    B_w_W_B = sy_unskew(W)
+    return B_w_W_B
+
+def simulate_pos_vel_acc(w1=1, w2=1, w3=1, 
+                         a1=1, a2=1, a3=1,
+                         b1=0, b2=0, b3=0):
+    """We model the position as sinus with amplitude a, frequency w and phase b.
+    
+    """
+    t_W_B = sy.Matrix([a1 * sy.sin(b1 + w1 * t),
+                       a2 * sy.sin(b2 + w2 * t),
+                       a3 * sy.sin(b3 + w3 * t)])
+    
+    # Velocity = dot(Position), assuming World is an inertial frame.
+    v_W_B = sy.diff(t_W_B, t)
+     
+    # Acceleration = dot(Velocity), assuming World is an inertial frame.        
+    a_W_B = sy.diff(v_W_B, t) + sy.Matrix([0, 0, gravity])
+    
+    return t_W_B, v_W_B, a_W_B
+    
+def evaluate_symbolic_trajectory(t_W_B, v_W_B, a_W_B, R_W_B, B_w_W_B, t_min, t_max, dt):
+    
+    t_values = np.arange(t_min, t_max, dt)
+    n = len(t_values)
+    
+    def copy_to_Nx3(a):
+        x = np.zeros((n,3))
+        x[:,0] = a[0][0,:]
+        x[:,1] = a[1][0,:]
+        x[:,2] = a[2][0,:]
+        return x
+        
+    def copy_to_Nx9(a):
+        x = np.zeros((n,9))
+        x[:,0] = a[0][0,:]
+        x[:,1] = a[0][1,:]
+        x[:,2] = a[0][2,:]
+        x[:,3] = a[1][0,:]
+        x[:,4] = a[1][1,:]
+        x[:,5] = a[1][2,:]
+        x[:,6] = a[2][0,:]
+        x[:,7] = a[2][1,:]
+        x[:,8] = a[2][2,:]
+        return x
+        
+    eval_t_W_B   = sy.utilities.lambdify(t, t_W_B, 'numpy')
+    eval_v_W_B   = sy.utilities.lambdify(t, v_W_B, 'numpy')
+    eval_a_W_B   = sy.utilities.lambdify(t, a_W_B, 'numpy')
+    eval_R_W_B   = sy.utilities.lambdify(t, R_W_B, 'numpy')
+    eval_B_w_W_B = sy.utilities.lambdify(t, B_w_W_B, 'numpy')
+    t_W_B_val   = copy_to_Nx3(eval_t_W_B(t_values))
+    v_W_B_val   = copy_to_Nx3(eval_v_W_B(t_values))
+    a_W_B_val   = copy_to_Nx3(eval_a_W_B(t_values))
+    R_W_B_val   = copy_to_Nx9(eval_R_W_B(t_values))
+    B_w_W_B_val = copy_to_Nx3(eval_B_w_W_B(t_values))
+    
+    return t_W_B_val, v_W_B_val, a_W_B_val, R_W_B_val, B_w_W_B_val
+    
+def plot_trajectory(t_W_B_val, R_W_B_val, interval = 10):
+    n = np.shape(t_W_B_val)[0]
+    fig = plt.figure(figsize=(8,5))
+    ax = Axes3D(fig, xlabel='x [m]', ylabel='y [m]', zlabel='z [m]')
+    ax.plot(t_W_B_val[:,0], t_W_B_val[:,1], t_W_B_val[:,2], 'b-')
+    for i in range(0, n, interval):
+        R = np.reshape(R_W_B_val[i,:], (3,3))
+        assert(np.abs(np.linalg.det(R) - 1.0) < 1e-5)
+        plot_utils.draw_coordinate_frame(ax, t_W_B_val[i,:], R, 1.0)
+    ax.legend()
+    return ax, fig
+      
+def get_simulated_imu_data(t_max, dt):
+    
+    t_W_B, v_W_B, a_W_B = simulate_pos_vel_acc(0.7, 1.0, .5,
+                                               2.0, 1.0, 2.0,
+                                               0.7, 1.0, 1.0)
+    
+    R_W_B  = simulate_rotation_matrix(0.5, 0.05, 0.2,
+                                      1.0, 2.0, 1.0,
+                                      0.0, 1.0, 4.0)
+    
+    B_w_W_B = rotation_rate_from_rotation_matrix(R_W_B)
+    
+    if False:
+        # Plot symbolic
+        sy.plot(B_w_W_B[0], B_w_W_B[1], B_w_W_B[2], (t, 0, 2.0))
+        sy.plotting.plot3d_parametric_line(t_W_B[0], t_W_B[1], t_W_B[2], (t, 0, 2.0))
+        sy.plot(t_W_B[0], v_W_B[0], a_W_B[0], (t, 0, 2.0))
+        sy.plot(t_W_B[1], v_W_B[1], a_W_B[1], (t, 0, 2.0))
+        sy.plot(t_W_B[2], v_W_B[2], a_W_B[2], (t, 0, 2.0))
+    
+    # Evaluate symbolic trajectory for specific time instances
+    t_W_B_val, v_W_B_val, a_W_B_val, R_W_B_val, B_w_W_B_val = \
+        evaluate_symbolic_trajectory(t_W_B, v_W_B, a_W_B, R_W_B, B_w_W_B, 0.0, t_max, dt)
+        
+    # Transform IMU Measurements to body frame, where they are measured.
+    n = np.shape(t_W_B_val)[0]
+    B_a_W_B = np.zeros((n,3), dtype=np.float32)
+    for i in range(n):
+        R_B_W = np.transpose(np.reshape(R_W_B_val[i,:], (3,3)))
+        B_a_W_B[i,:] = np.dot(R_B_W, a_W_B_val[i,:]) 
+
+    return t_W_B_val, v_W_B_val, R_W_B_val, B_a_W_B, B_w_W_B_val
diff --git a/RWR/src/ze_oss/ze_imu/setup.py b/RWR/src/ze_oss/ze_imu/setup.py
new file mode 100644
index 0000000000000000000000000000000000000000..f7cf283b5ee3aa305e505dd36f409a29e505fd62
--- /dev/null
+++ b/RWR/src/ze_oss/ze_imu/setup.py
@@ -0,0 +1,12 @@
+#!/usr/bin/env python
+
+from distutils.core import setup
+from catkin_pkg.python_setup import generate_distutils_setup
+
+d = generate_distutils_setup(
+    packages=['ze_imu'],
+    package_dir={'': 'py'},
+    install_requires=['rospy', 'yaml'],
+    )
+
+setup(**d)
\ No newline at end of file
diff --git a/RWR/src/ze_oss/ze_imu/src/accelerometer_model.cpp b/RWR/src/ze_oss/ze_imu/src/accelerometer_model.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..1519e6dee9d79d35df6fe49249c2ed3c26e3e115
--- /dev/null
+++ b/RWR/src/ze_oss/ze_imu/src/accelerometer_model.cpp
@@ -0,0 +1,51 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include <ze/imu/accelerometer_model.hpp>
+
+namespace ze {
+
+AccelerometerModel::AccelerometerModel(
+    const ImuIntrinsicModel::Ptr intrinsicModel, const ImuNoiseModel::Ptr noiseModel)
+  : intrinsicModel_(intrinsicModel)
+  , noiseModel_(noiseModel)
+{
+}
+
+Vector3 AccelerometerModel::distort(const Eigen::Ref<const measurement_t>& a,
+                                    const Eigen::Ref<const measurement_t>& w)
+const
+{
+  return intrinsicModel_->distort(a, w);
+}
+
+Vector3 AccelerometerModel::undistort(const Eigen::Ref<const measurement_t>& a,
+                                      const Eigen::Ref<const measurement_t>& w)
+const
+{
+  return intrinsicModel_->undistort(a, w);
+}
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_imu/src/gyroscope_model.cpp b/RWR/src/ze_oss/ze_imu/src/gyroscope_model.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..9edebd746b4325d2d517888d37dbca9272868b24
--- /dev/null
+++ b/RWR/src/ze_oss/ze_imu/src/gyroscope_model.cpp
@@ -0,0 +1,52 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include <ze/imu/gyroscope_model.hpp>
+
+namespace ze {
+
+//----------------------------
+// White brownian noise model
+GyroscopeModel::GyroscopeModel(
+    const ImuIntrinsicModel::Ptr intrinsicModel,
+    const ImuNoiseModel::Ptr noiseModel)
+  : intrinsicModel_(intrinsicModel)
+  , noiseModel_(noiseModel)
+{
+}
+
+Vector3 GyroscopeModel::distort(const Eigen::Ref<const measurement_t>& w,
+                                const Eigen::Ref<const measurement_t>& a) const
+{
+  return intrinsicModel_->distort(w, a);
+}
+
+Vector3 GyroscopeModel::undistort(const Eigen::Ref<const measurement_t>& w,
+                                  const Eigen::Ref<const measurement_t>& a) const
+{
+  return intrinsicModel_->undistort(w, a);
+}
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_imu/src/imu_buffer.cpp b/RWR/src/ze_oss/ze_imu/src/imu_buffer.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b31e07989aeebeb61c6c758cbd2368a8c67891dc
--- /dev/null
+++ b/RWR/src/ze_oss/ze_imu/src/imu_buffer.cpp
@@ -0,0 +1,229 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include <ze/imu/imu_buffer.hpp>
+
+namespace ze {
+
+template<int BufferSize, typename GyroInterp, typename AccelInterp>
+ImuBuffer<BufferSize, GyroInterp, AccelInterp>::ImuBuffer(ImuModel::Ptr imu_model)
+  : imu_model_(imu_model)
+  , gyro_delay_(secToNanosec(imu_model->gyroscopeModel()->intrinsicModel()->delay()))
+  , accel_delay_(secToNanosec(imu_model->accelerometerModel()->intrinsicModel()->delay()))
+{
+}
+
+template<int BufferSize, typename GyroInterp, typename AccelInterp>
+void ImuBuffer<BufferSize, GyroInterp, AccelInterp>::insertImuMeasurement(
+    int64_t time, const ImuAccGyr value)
+{
+  acc_buffer_.insert(correctStampAccel(time), value.head<3>(3));
+  gyr_buffer_.insert(correctStampGyro(time), value.tail<3>(3));
+}
+
+
+template<int BufferSize, typename GyroInterp, typename AccelInterp>
+void ImuBuffer<BufferSize, GyroInterp, AccelInterp>::insertGyroscopeMeasurement(
+    int64_t time, const Vector3 value)
+{
+  gyr_buffer_.insert(correctStampGyro(time), value);
+}
+
+template<int BufferSize, typename GyroInterp, typename AccelInterp>
+void ImuBuffer<BufferSize, GyroInterp, AccelInterp>::insertAccelerometerMeasurement(
+    int64_t time, const Vector3 value)
+{
+  acc_buffer_.insert(correctStampAccel(time), value);
+}
+
+template<int BufferSize, typename GyroInterp, typename AccelInterp>
+bool ImuBuffer<BufferSize, GyroInterp, AccelInterp>::get(int64_t time,
+                                              Eigen::Ref<ImuAccGyr> out)
+{
+  std::lock_guard<std::mutex> gyr_lock(gyr_buffer_.mutex());
+  std::lock_guard<std::mutex> acc_lock(acc_buffer_.mutex());
+
+  if (time > gyr_buffer_.times().back()
+      || time > acc_buffer_.times().back())
+  {
+    return false;
+  }
+
+  const auto gyro_before = gyr_buffer_.iterator_equal_or_before(time);
+  const auto acc_before = acc_buffer_.iterator_equal_or_before(time);
+
+  if (gyro_before == gyr_buffer_.times().end()
+      || acc_before == acc_buffer_.times().end()) {
+    return false;
+  }
+
+  VectorX w = GyroInterp::interpolate(&gyr_buffer_, time, gyro_before);
+  VectorX a = AccelInterp::interpolate(&acc_buffer_, time, acc_before);
+
+  out = imu_model_->undistort(a, w);
+  return true;
+}
+
+template<int BufferSize, typename GyroInterp, typename AccelInterp>
+bool ImuBuffer<BufferSize, GyroInterp, AccelInterp>::getAccelerometerDistorted(
+    int64_t time,
+    Eigen::Ref<Vector3> out)
+{
+  return acc_buffer_.getValueInterpolated(time, out);
+}
+
+template<int BufferSize, typename GyroInterp, typename AccelInterp>
+bool ImuBuffer<BufferSize, GyroInterp, AccelInterp>::getGyroscopeDistorted(
+    int64_t time,
+    Eigen::Ref<Vector3> out)
+{
+  return gyr_buffer_.getValueInterpolated(time, out);
+}
+
+template<int BufferSize, typename GyroInterp, typename AccelInterp>
+std::pair<ImuStamps, ImuAccGyrContainer>
+ImuBuffer<BufferSize, GyroInterp, AccelInterp>::getBetweenValuesInterpolated(
+    int64_t stamp_from, int64_t stamp_to)
+{
+  //Takes gyroscope timestamps and interpolates accelerometer measurements at
+  // same times. Rectifies all measurements.
+  CHECK_GE(stamp_from, 0u);
+  CHECK_LT(stamp_from, stamp_to);
+  ImuAccGyrContainer rectified_measurements;
+  ImuStamps stamps;
+
+  std::lock_guard<std::mutex> gyr_lock(gyr_buffer_.mutex());
+  std::lock_guard<std::mutex> acc_lock(acc_buffer_.mutex());
+
+  if(gyr_buffer_.times().size() < 2)
+  {
+    LOG(WARNING) << "Buffer has less than 2 entries.";
+    // return empty means unsuccessful.
+    return std::make_pair(stamps, rectified_measurements);
+  }
+
+  const time_t oldest_stamp = gyr_buffer_.times().front();
+  const time_t newest_stamp = gyr_buffer_.times().back();
+  if (stamp_from < oldest_stamp)
+  {
+    LOG(WARNING) << "Requests older timestamp than in buffer.";
+    // return empty means unsuccessful.
+    return std::make_pair(stamps, rectified_measurements);
+  }
+  if (stamp_to > newest_stamp)
+  {
+    LOG(WARNING) << "Requests newer timestamp than in buffer.";
+    // return empty means unsuccessful.
+    return std::make_pair(stamps, rectified_measurements);
+  }
+
+  const auto it_from_before = gyr_buffer_.iterator_equal_or_before(stamp_from);
+  const auto it_to_after = gyr_buffer_.iterator_equal_or_after(stamp_to);
+  CHECK(it_from_before != gyr_buffer_.times().end());
+  CHECK(it_to_after != gyr_buffer_.times().end());
+  const auto it_from_after = it_from_before + 1;
+  const auto it_to_before = it_to_after - 1;
+  if (it_from_after == it_to_before)
+  {
+    LOG(WARNING) << "Not enough data for interpolation";
+    // return empty means unsuccessful.
+    return std::make_pair(stamps, rectified_measurements);
+  }
+
+  // resize containers
+  const size_t range = it_to_before.index() - it_from_after.index() + 3;
+  rectified_measurements.resize(Eigen::NoChange, range);
+  stamps.resize(range);
+
+  // first element
+  VectorX w = GyroInterp::interpolate(&gyr_buffer_, stamp_from, it_from_before);
+  VectorX a = AccelInterp::interpolate(&acc_buffer_, stamp_from);
+  stamps(0) = stamp_from;
+  rectified_measurements.col(0) = imu_model_->undistort(a, w);
+
+  // this is a real edge case where we hit the two consecutive timestamps
+  //  with from and to.
+  size_t col = 1;
+  if (range > 2)
+  {
+    for (auto it=it_from_before+1; it!=it_to_after; ++it) {
+      w = GyroInterp::interpolate(&gyr_buffer_, (*it), it);
+      a = AccelInterp::interpolate(&acc_buffer_, (*it));
+      stamps(col) = (*it);
+      rectified_measurements.col(col) = imu_model_->undistort(a, w);
+      ++col;
+    }
+  }
+
+  // last element
+  w = GyroInterp::interpolate(&gyr_buffer_, stamp_to, it_to_before);
+  a = AccelInterp::interpolate(&acc_buffer_, stamp_to);
+  stamps(range - 1) = stamp_to;
+  rectified_measurements.col(range - 1) = imu_model_->undistort(a, w);
+
+  return std::make_pair(stamps, rectified_measurements);
+}
+
+template<int BufferSize, typename GyroInterp, typename AccelInterp>
+std::tuple<int64_t, int64_t, bool>
+ImuBuffer<BufferSize, GyroInterp, AccelInterp>::getOldestAndNewestStamp() const
+{
+  std::tuple<int64_t, int64_t, bool> accel =
+      acc_buffer_.getOldestAndNewestStamp();
+  std::tuple<int64_t, int64_t, bool> gyro =
+      gyr_buffer_.getOldestAndNewestStamp();
+
+  if (!std::get<2>(accel) || !std::get<2>(gyro))
+  {
+    return std::make_tuple(-1, -1, false);
+  }
+
+  int64_t oldest = std::get<0>(accel) < std::get<0>(gyro) ?
+                     std::get<0>(gyro) : std::get<0>(accel);
+
+  int64_t newest = std::get<1>(accel) < std::get<1>(gyro) ?
+                     std::get<1>(accel) : std::get<1>(gyro);
+
+  // This is an extreme edge case where the accel and gyro measurements
+  // do not overlap at all.
+  if (oldest > newest)
+  {
+    return std::make_tuple(-1, -1, false);
+  }
+
+  return std::make_tuple(oldest, newest, true);
+}
+
+// A set of explicit declarations
+template class ImuBuffer<2000, InterpolatorLinear>;
+template class ImuBuffer<5000, InterpolatorLinear>;
+template class ImuBuffer<2000, InterpolatorNearest>;
+template class ImuBuffer<5000, InterpolatorNearest>;
+template class ImuBuffer<2000, InterpolatorDifferentiatorLinear,
+InterpolatorLinear>;
+template class ImuBuffer<5000, InterpolatorDifferentiatorLinear,
+InterpolatorLinear>;
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_imu/src/imu_intrinsic_model.cpp b/RWR/src/ze_oss/ze_imu/src/imu_intrinsic_model.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..2e43652178c3e91b728b7723085db1812b1585d7
--- /dev/null
+++ b/RWR/src/ze_oss/ze_imu/src/imu_intrinsic_model.cpp
@@ -0,0 +1,207 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include <ze/imu/imu_intrinsic_model.hpp>
+
+namespace ze {
+
+// The constexpr needs a definition to make it passable by reference.
+constexpr real_t ImuIntrinsicModel::UndefinedRange;
+
+//------------------------------------------------------------------------------
+// Intrinsics Base Class
+ImuIntrinsicModel::ImuIntrinsicModel(ImuIntrinsicType type)
+  : ImuIntrinsicModel(type, 0.0, UndefinedRange)
+{
+}
+
+ImuIntrinsicModel::ImuIntrinsicModel(ImuIntrinsicType type, real_t delay, real_t range)
+ : type_(type), delay_(delay), range_(range)
+{
+  CHECK(range == UndefinedRange || range > 0) << "Range must either be of constant UndefinedRange or be > 0";
+}
+
+std::string ImuIntrinsicModel::typeAsString() const
+{
+  switch (type())
+  {
+    case ImuIntrinsicType::Calibrated: return "Calibrated";
+    case ImuIntrinsicType::ScaleMisalignment: return "Scale Misalignment";
+    case ImuIntrinsicType::ScaleMisalignmentGSensitivity: return "Scale Misalignment g-Sensitivity";
+    case ImuIntrinsicType::ScaleMisalignmentSizeEffect: return "Scale Misalignment Size Effect";
+    default:
+      LOG(FATAL) << "Unknown intrinsics model";
+  }
+
+  return "";
+}
+
+//------------------------------------------------------------------------------
+// Calibrated
+ImuIntrinsicModelCalibrated::ImuIntrinsicModelCalibrated()
+  : ImuIntrinsicModel(Type, 0.0, ImuIntrinsicModel::UndefinedRange)
+{
+}
+
+ImuIntrinsicModelCalibrated::ImuIntrinsicModelCalibrated(real_t delay, real_t range)
+  : ImuIntrinsicModel(Type, delay, range)
+{
+}
+
+Vector3 ImuIntrinsicModelCalibrated::undistort(
+    const Eigen::Ref<const primary_measurement_t>& primary,
+    const Eigen::Ref<const secondary_measurement_t>& secondary) const
+{
+  //! The calibrated model assumes that all relevant deterministic effects have
+  //! been taken care of by the manufacturer. Hence, the mapping is an identity.
+  CHECK_GE(primary.rows(), 3) << "Primary model input has incorrect size.";
+  return primary.head<3>();
+}
+
+Vector3 ImuIntrinsicModelCalibrated::distort(
+    const Eigen::Ref<const primary_measurement_t>& primary,
+    const Eigen::Ref<const secondary_measurement_t>& secondary) const
+{
+  //! The calibrated model assumes that all relevant deterministic effects have
+  //! been taken care of by the manufacturer. Hence, the mapping is an identity.
+  CHECK_GE(primary.rows(), 3) << "Primary model input has incorrect size.";
+  return primary.head<3>();
+}
+
+//------------------------------------------------------------------------------
+// Intrinsic Model Scale Misalignment
+ImuIntrinsicModelScaleMisalignment::ImuIntrinsicModelScaleMisalignment(
+    real_t delay,
+    real_t range,
+    const Vector3& b,
+    const Matrix3& M)
+  : ImuIntrinsicModel(Type, delay, range)
+  , b_(b)
+  , M_(M)
+  , M_inverse_(M.inverse())
+{
+  CHECK(std::fabs(M_.determinant()) > 1.e-10)
+    << "M must be invertible. Its determinant evaluates to " << M_.determinant();
+}
+
+Vector3 ImuIntrinsicModelScaleMisalignment::undistort(
+    const Eigen::Ref<const primary_measurement_t>& primary,
+    const Eigen::Ref<const secondary_measurement_t>& secondary) const
+{
+  CHECK_GE(primary.rows(), 3) << "Primary model input has incorrect size.";
+  return M_inverse_ * (primary.head<3>() - b_);
+}
+
+Vector3 ImuIntrinsicModelScaleMisalignment::distort(
+    const Eigen::Ref<const primary_measurement_t>& primary,
+    const Eigen::Ref<const secondary_measurement_t>& secondary) const
+{
+  CHECK_GE(primary.rows(), 3) << "Primary model input has incorrect size.";
+  return M_ * primary.head<3>() + b_;
+}
+
+//------------------------------------------------------------------------------
+// Intrinsic Model Scale Misalignment g-Sensitivity
+ImuIntrinsicModelScaleMisalignmentGSensitivity::ImuIntrinsicModelScaleMisalignmentGSensitivity(
+    real_t delay,
+    real_t range,
+    const Vector3& b,
+    const Matrix3& M,
+    const Matrix3& Ma)
+  : ImuIntrinsicModel(Type, delay, range)
+  , b_(b)
+  , M_(M)
+  , M_inverse_(M.inverse())
+  , Ma_(Ma)
+{
+  CHECK(std::fabs(M_.determinant()) > 1.e-10)
+    << "M must be invertible. Its determinant evaluates to " << M_.determinant();
+}
+
+Vector3 ImuIntrinsicModelScaleMisalignmentGSensitivity::undistort(
+    const Eigen::Ref<const primary_measurement_t>& primary,
+    const Eigen::Ref<const secondary_measurement_t>& secondary) const
+{
+  CHECK_GE(primary.rows(), 3) << "Primary model input has incorrect size.";
+  CHECK_GE(secondary.rows(), 3) << "Secondary model input has incorrect size.";
+  Vector3 a = secondary.head<3>();
+  Vector3 w = primary.head<3>();
+  return M_inverse_ * (w - Ma_ * a - b_);
+}
+
+Vector3 ImuIntrinsicModelScaleMisalignmentGSensitivity::distort(
+    const Eigen::Ref<const primary_measurement_t>& primary,
+    const Eigen::Ref<const secondary_measurement_t>& secondary) const
+{
+  CHECK_GE(primary.rows(), 3) << "Primary model input has incorrect size.";
+  CHECK_GE(secondary.rows(), 3) << "Secondary model input has incorrect size.";
+  Vector3 a = secondary.head<3>();
+  Vector3 w = primary.head<3>();
+  return M_ * w + Ma_ * a + b_;
+}
+
+//------------------------------------------------------------------------------
+// Intrinsic Model Scale Misalignment Size Effect
+ImuIntrinsicModelScaleMisalignmentSizeEffect::ImuIntrinsicModelScaleMisalignmentSizeEffect(
+    real_t delay,
+    real_t range,
+    const Vector3& b,
+    const Matrix3& M,
+    const Matrix3& R)
+  : ImuIntrinsicModel(Type, delay, range)
+  , b_(b)
+  , M_(M)
+  , M_inverse_(M.inverse())
+  , R_(R)
+{
+  CHECK(std::fabs(M_.determinant()) > 1.e-10)
+    << "M must be invertible. Its determinant evaluates to " << M_.determinant();
+}
+
+Vector3 ImuIntrinsicModelScaleMisalignmentSizeEffect::undistort(
+    const Eigen::Ref<const primary_measurement_t>& primary,
+    const Eigen::Ref<const secondary_measurement_t>& secondary) const
+{
+  CHECK_GE(primary.rows(), 3) << "Primary model input has incorrect size.";
+  CHECK_GE(secondary.rows(), 6) << "Secondary model input has incorrect size.";
+  Vector3 a = primary.head<3>();
+  Vector3 w = secondary.head<3>();
+  Vector3 w_dot = secondary.segment<3>(3);
+  return M_inverse_ * (a - b_) - (skewSymmetric(w_dot) * R_ + skewSymmetric(w) * skewSymmetric(w) * R_).diagonal();
+}
+
+Vector3 ImuIntrinsicModelScaleMisalignmentSizeEffect::distort(
+    const Eigen::Ref<const primary_measurement_t>& primary,
+    const Eigen::Ref<const secondary_measurement_t>& secondary) const
+{
+  CHECK_GE(primary.rows(), 3) << "Primary model input has incorrect size.";
+  CHECK_GE(secondary.rows(), 6) << "Secondary model input has incorrect size.";
+  Vector3 a = primary.head<3>();
+  Vector3 w = secondary.head<3>();
+  Vector3 w_dot = secondary.segment<3>(3);
+  return M_ * (a + (skewSymmetric(w_dot) * R_ + skewSymmetric(w) * skewSymmetric(w) * R_).diagonal()) + b_;
+}
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_imu/src/imu_model.cpp b/RWR/src/ze_oss/ze_imu/src/imu_model.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ae1c9bca97bead92d62b4bfd896bba25ddeae714
--- /dev/null
+++ b/RWR/src/ze_oss/ze_imu/src/imu_model.cpp
@@ -0,0 +1,71 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include <ze/imu/imu_model.hpp>
+
+namespace ze {
+
+ImuModel::ImuModel(
+    const AccelerometerModel::Ptr accelerometerModel,
+    const GyroscopeModel::Ptr gyroscopeModel)
+  : accelerometerModel_(accelerometerModel)
+  , gyroscopeModel_(gyroscopeModel)
+{
+}
+
+ImuModel::Ptr ImuModel::loadFromYaml(const std::string& path)
+{
+  try
+  {
+    YAML::Node doc = YAML::LoadFile(path.c_str());
+    return doc.as<ImuModel::Ptr>();
+  }
+  catch (const std::exception& ex)
+  {
+    LOG(ERROR) << "Failed to load IMU from file " << path << " with the error: \n"
+               << ex.what();
+  }
+  return ImuModel::Ptr();
+}
+
+Vector6 ImuModel::distort(const Eigen::Ref<const measurement_t>& primary,
+                          const Eigen::Ref<const measurement_t>& secondary) const
+{
+  Vector6 out;
+  out.head<3>() = accelerometerModel_->distort(primary, secondary);
+  out.tail<3>() = gyroscopeModel_->distort(secondary, primary);
+  return out;
+}
+
+Vector6 ImuModel::undistort(const Eigen::Ref<const measurement_t>& primary,
+                            const Eigen::Ref<const measurement_t>& secondary) const
+{
+  Vector6 out;
+  out.head<3>() = accelerometerModel_->undistort(primary, secondary);
+  out.tail<3>() = gyroscopeModel_->undistort(secondary, primary);
+  return out;
+}
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_imu/src/imu_noise_model.cpp b/RWR/src/ze_oss/ze_imu/src/imu_noise_model.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..11049e0af3da0cf835c7d6e5eb3ec89cc751319d
--- /dev/null
+++ b/RWR/src/ze_oss/ze_imu/src/imu_noise_model.cpp
@@ -0,0 +1,68 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include <ze/imu/imu_noise_model.hpp>
+
+namespace ze {
+
+//------------------------------------------------------------------------------
+// Noise model base class
+
+ImuNoiseModel::ImuNoiseModel(ImuNoiseType type)
+  : type_(type)
+{
+}
+
+std::string ImuNoiseModel::typeAsString() const
+{
+  switch (type())
+  {
+    case ImuNoiseType::WhiteBrownian: return "White Brownian";
+    case ImuNoiseType::None: return "No Noise";
+    default:
+      LOG(FATAL) << "Unknown noise model";
+  }
+}
+
+//------------------------------------------------------------------------------
+// No Noise model
+ImuNoiseNone::ImuNoiseNone(): ImuNoiseModel(Type)
+{
+}
+
+//------------------------------------------------------------------------------
+// White brownian noise model
+ImuNoiseWhiteBrownian::ImuNoiseWhiteBrownian(real_t noise_density,
+           real_t bandwidth,
+           real_t bias_noise_density)
+  : ImuNoiseModel(Type)
+  , noise_density_(noise_density)
+  , bandwidth_(bandwidth)
+  , bias_noise_density_(bias_noise_density)
+{
+  CHECK(bandwidth > 0) << "Bandwidth must be >0'";
+}
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_imu/src/imu_rig.cpp b/RWR/src/ze_oss/ze_imu/src/imu_rig.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..da1f712341fe427a6c7d5355bce01c78e1ec757e
--- /dev/null
+++ b/RWR/src/ze_oss/ze_imu/src/imu_rig.cpp
@@ -0,0 +1,66 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include <ze/imu/imu_rig.hpp>
+#include <ze/imu/imu_yaml_serialization.hpp>
+
+namespace ze {
+
+ImuRig::ImuRig(
+    const TransformationVector& T_B_S,
+    const ImuVector& imus,
+    const std::string& label)
+  : T_B_S_(T_B_S)
+  , imus_(imus)
+  , label_(label)
+{
+  CHECK_EQ(T_B_S_.size(), imus_.size());
+  for(size_t i = 0; i < size(); ++i)
+  {
+    CHECK_NOTNULL(imus_[i].get());
+  }
+
+  // set inverse transformations
+  for(size_t i = 0; i < size(); ++i)
+  {
+    T_S_B_.push_back(T_B_S_[i].inverse());
+  }
+}
+
+ImuRig::Ptr ImuRig::loadFromYaml(const std::string& yaml_file)
+{
+  try
+  {
+    YAML::Node doc = YAML::LoadFile(yaml_file.c_str());
+    return doc.as<ImuRig::Ptr>();
+  }
+  catch (const std::exception& ex)
+  {
+    LOG(ERROR) << "Cannot load ImuRig from file:" << yaml_file << "\n"
+               << ex.what();
+  }
+  return nullptr;
+}
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_imu/src/imu_yaml_serialization.cpp b/RWR/src/ze_oss/ze_imu/src/imu_yaml_serialization.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b86f43c25b3a158d265b05e84f5c57af025c7a08
--- /dev/null
+++ b/RWR/src/ze_oss/ze_imu/src/imu_yaml_serialization.cpp
@@ -0,0 +1,286 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include <ze/imu/accelerometer_model.hpp>
+#include <ze/imu/gyroscope_model.hpp>
+#include <ze/imu/imu_intrinsic_model.hpp>
+#include <ze/imu/imu_model.hpp>
+#include <ze/imu/imu_noise_model.hpp>
+#include <ze/imu/imu_rig.hpp>
+#include <ze/imu/imu_yaml_serialization.hpp>
+#include <ze/common/types.hpp>
+#include <ze/common/yaml_serialization.hpp>
+
+using ze::Matrix3;
+using ze::Vector3;
+using ze::real_t;
+
+namespace YAML {
+
+//------------------------------------------------------------------------------
+// IMU loading.
+bool convert<std::shared_ptr<ze::ImuModel>>::decode(
+    const Node& node, std::shared_ptr<ze::ImuModel>& imu)
+{
+  imu.reset();
+  try {
+    if(!node.IsMap())
+    {
+      LOG(ERROR) << "Unable to get parse the imu because the node is not a map.";
+      return false;
+    }
+
+    const YAML::Node gyroscopes = node["gyroscopes"];
+    const YAML::Node accelerometers = node["accelerometers"];
+
+    if (!gyroscopes || !accelerometers)
+    {
+      throw std::runtime_error("Missing Gyroscopes / Accelerometer keys.");
+    }
+
+    // get the types
+    const YAML::Node g_noise_node = gyroscopes["noise_model"];
+    const YAML::Node g_intrinsic_node = gyroscopes["intrinsic_model"];
+    const YAML::Node a_noise_node = accelerometers["noise_model"];
+    const YAML::Node a_intrinsic_node = accelerometers["intrinsic_model"];
+
+    std::string g_noise_type = extractChild<std::string>(g_noise_node, "type");
+    std::string g_intrinsic_type = extractChild<std::string>(g_intrinsic_node, "type");
+    std::string a_noise_type = extractChild<std::string>(a_noise_node, "type");
+    std::string a_intrinsic_type = extractChild<std::string>(a_intrinsic_node, "type");
+
+    if (!internal::validateNoise(g_noise_type) ||
+        !internal::validateNoise(a_noise_type) ||
+        !internal::validateIntrinsic(g_intrinsic_type) ||
+        !internal::validateIntrinsic(a_intrinsic_type))
+    {
+      throw std::runtime_error("Invalid Intrinsic or Noise type.");
+    }
+
+    ze::ImuNoiseModel::Ptr g_noise = internal::decodeNoise(g_noise_node);
+    ze::ImuNoiseModel::Ptr a_noise = internal::decodeNoise(a_noise_node);
+
+    ze::ImuIntrinsicModel::Ptr g_intrinsic_model =
+        internal::decodeIntrinsics(g_intrinsic_node);
+    ze::ImuIntrinsicModel::Ptr a_intrinsic_model =
+        internal::decodeIntrinsics(a_intrinsic_node);
+
+    ze::GyroscopeModel::Ptr gyro =
+        std::make_shared<ze::GyroscopeModel>(g_intrinsic_model, g_noise);
+    ze::AccelerometerModel::Ptr accel =
+        std::make_shared<ze::AccelerometerModel>(a_intrinsic_model, a_noise);
+
+    imu = std::make_shared<ze::ImuModel>(accel, gyro);
+
+    if(node["label"])
+    {
+      imu->setLabel(node["label"].as<std::string>());
+    }
+    if(node["id"])
+    {
+      imu->setId(node["id"].as<std::string>());
+    }
+  }
+  catch(const std::exception& e)
+  {
+    LOG(ERROR) << "YAML exception during parsing: " << e.what();
+    imu.reset();
+    return false;
+  }
+  return true;
+}
+
+//------------------------------------------------------------------------------
+// Validation of types
+
+bool internal::validateNoise(const std::string& value)
+{
+  if ("white-brownian" == value)
+  {
+    return true;
+  }
+
+  return false;
+}
+
+bool internal::validateIntrinsic(const std::string& value)
+{
+  if ("calibrated" == value ||
+      "scale-misalignment" == value ||
+      "scale-misalignment-gsensitivity" == value ||
+      "scale-misalignment-size-effect" == value)
+  {
+    return true;
+  }
+
+  return false;
+}
+
+//------------------------------------------------------------------------------
+// Noise Model loading.
+std::shared_ptr<ze::ImuNoiseModel> internal::decodeNoise(const Node& node)
+{
+  if(!node.IsMap())
+  {
+    LOG(ERROR) << "Unable to get parse the imu because the node is not a map.";
+    return false;
+  }
+
+  // gyroscopes:
+  std::string noise_type = extractChild<std::string>(node, "type");
+  if (noise_type == "white-brownian")
+  {
+    real_t noise_density = extractChild<real_t>(node, "noise_density");
+    real_t bandwidth = extractChild<real_t>(node, "bandwidth");
+    real_t bias_noise_density = extractChild<real_t>(node, "bias_noise_density");
+    return std::make_shared<ze::ImuNoiseWhiteBrownian>(
+          noise_density, bandwidth, bias_noise_density);
+  }
+  else
+  {
+    throw std::runtime_error("Unsupported Noise Model.");
+  }
+}
+
+//------------------------------------------------------------------------------
+// Intrinsic Model loading.
+typename std::shared_ptr<ze::ImuIntrinsicModel> internal::decodeIntrinsics(
+    const Node& node)
+{
+  if(!node.IsMap())
+  {
+    LOG(ERROR) << "Unable to get parse intrinsic model because the node is not a map.";
+    return false;
+  }
+
+  // gyroscopes:
+  std::string type = extractChild<std::string>(node, "type");
+
+  if (type == "calibrated")
+  {
+    return std::make_shared<ze::ImuIntrinsicModelCalibrated>();
+  }
+  else if (type == "scale-misalignment" ||
+           type == "scale-misalignment-gsensitivity" ||
+           type == "scale-misalignment-size-effect")
+  {
+    real_t delay = extractChild<real_t>(node, "delay");
+    real_t range = extractChild<real_t>(node, "range");
+    Vector3 b = extractChild<Vector3>(node, "b");
+    Matrix3 M = extractChild<Matrix3>(node, "M");
+
+    if ("scale-misalignment" == type)
+    {
+      return std::make_shared<ze::ImuIntrinsicModelScaleMisalignment>(
+                         delay, range, b, M);
+    }
+    else if ("scale-misalignment-gsensitivity" == type)
+    {
+      Matrix3 Ma = extractChild<Matrix3>(node, "Ma");
+      return std::make_shared<
+                       ze::ImuIntrinsicModelScaleMisalignmentGSensitivity>(
+                         delay, range, b, M, Ma);
+    }
+    else if ("scale-misalignment-size-effect" == type)
+    {
+      Matrix3 R = extractChild<Matrix3>(node, "R");
+      return std::make_shared<
+                       ze::ImuIntrinsicModelScaleMisalignmentSizeEffect>(
+                         delay, range, b, M, R);
+    }
+  }
+  else
+  {
+    throw std::runtime_error("Unsupported Intrinsic Model.");
+  }
+
+  return nullptr;
+}
+
+//------------------------------------------------------------------------------
+// ImuRig loading.
+bool convert<std::shared_ptr<ze::ImuRig>>::decode(
+    const Node& node, std::shared_ptr<ze::ImuRig>& imu_rig)
+{
+  imu_rig.reset();
+  try {
+    if (!node.IsMap())
+    {
+      LOG(ERROR) << "Parsing ImuRig failed because node is not a map.";
+      return false;
+    }
+
+    std::string label = extractChild<std::string>(node, "label");
+
+    const Node& imus_node = node["imus"];
+    if (!imus_node.IsSequence())
+    {
+      LOG(ERROR) << "Parsing ImuRig failed because 'imus' is not a sequence.";
+      return false;
+    }
+
+    size_t num_imus = imus_node.size();
+    if (num_imus == 0)
+    {
+      LOG(ERROR) << "Parsing ImuRig failed. Number of imus is 0.";
+      return false;
+    }
+
+    ze::TransformationVector T_B_Si;
+    ze::ImuVector imus;
+    for (size_t i = 0; i < num_imus; ++i)
+    {
+      const Node& imu_node = imus_node[i];
+      if (!imu_node)
+      {
+        LOG(ERROR) << "Unable to get imu node for imu " << i;
+        return false;
+      }
+      if (!imu_node.IsMap())
+      {
+        LOG(ERROR) << "Imu node for imu " << i << " is not a map.";
+        return false;
+      }
+
+      ze::ImuModel::Ptr imu = extractChild<ze::ImuModel::Ptr>(imu_node, "imu");
+
+      ze::Matrix4 T_B_S = extractChild<ze::Matrix4>(imu_node, "T_B_S");
+
+      imus.push_back(imu);
+      T_B_Si.push_back(ze::Transformation(T_B_S).inverse());
+    }
+
+    imu_rig.reset(new ze::ImuRig(T_B_Si, imus, label));
+  }
+  catch (const std::exception& ex)
+  {
+    LOG(ERROR) << "YAML exception during parsing: " << ex.what();
+    imu_rig.reset();
+    return false;
+  }
+  return true;
+}
+
+} // namespace YAML
diff --git a/RWR/src/ze_oss/ze_imu/test/test_accelerometer_model.cpp b/RWR/src/ze_oss/ze_imu/test/test_accelerometer_model.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..7f344f23a29a28809476d1b9b45e84670fee0d38
--- /dev/null
+++ b/RWR/src/ze_oss/ze_imu/test/test_accelerometer_model.cpp
@@ -0,0 +1,45 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include <ze/common/test_entrypoint.hpp>
+
+#include <ze/imu/accelerometer_model.hpp>
+#include <ze/imu/imu_intrinsic_model.hpp>
+#include <ze/imu/imu_noise_model.hpp>
+
+TEST(AccelerometerModelTest, testAccelerometer)
+{
+  using namespace ze;
+  std::shared_ptr<ImuIntrinsicModelCalibrated> intrinsics =
+      std::make_shared<ImuIntrinsicModelCalibrated>();
+  std::shared_ptr<ImuNoiseNone> noise = std::make_shared<ImuNoiseNone>();
+
+  AccelerometerModel model(intrinsics, noise);
+
+  EXPECT_EQ(intrinsics, model.intrinsicModel());
+  EXPECT_EQ(noise, model.noiseModel());
+}
+
+ZE_UNITTEST_ENTRYPOINT
diff --git a/RWR/src/ze_oss/ze_imu/test/test_gyroscope_model.cpp b/RWR/src/ze_oss/ze_imu/test/test_gyroscope_model.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..354db169032be8cd22532453354186fd2313d57c
--- /dev/null
+++ b/RWR/src/ze_oss/ze_imu/test/test_gyroscope_model.cpp
@@ -0,0 +1,45 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include <ze/common/test_entrypoint.hpp>
+
+#include <ze/imu/gyroscope_model.hpp>
+#include <ze/imu/imu_intrinsic_model.hpp>
+#include <ze/imu/imu_noise_model.hpp>
+
+TEST(GyroscopeModelTest, testGyroscope)
+{
+  using namespace ze;
+  std::shared_ptr<ImuIntrinsicModelCalibrated> intrinsics =
+      std::make_shared<ImuIntrinsicModelCalibrated>();
+  std::shared_ptr<ImuNoiseNone> noise = std::make_shared<ImuNoiseNone>();
+
+  GyroscopeModel model(intrinsics, noise);
+
+  EXPECT_EQ(intrinsics, model.intrinsicModel());
+  EXPECT_EQ(noise, model.noiseModel());
+}
+
+ZE_UNITTEST_ENTRYPOINT
diff --git a/RWR/src/ze_oss/ze_imu/test/test_imu_buffer.cpp b/RWR/src/ze_oss/ze_imu/test/test_imu_buffer.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..00a53f6afc150aea16604e3e39663e77f949fc1e
--- /dev/null
+++ b/RWR/src/ze_oss/ze_imu/test/test_imu_buffer.cpp
@@ -0,0 +1,170 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include <ze/common/test_entrypoint.hpp>
+
+#include <ze/imu/imu_buffer.hpp>
+
+TEST(ImuBufferTest, testBuffer)
+{
+  using namespace ze;
+  std::shared_ptr<ImuIntrinsicModelCalibrated> intrinsics =
+      std::make_shared<ImuIntrinsicModelCalibrated>();
+  std::shared_ptr<ImuNoiseNone> noise = std::make_shared<ImuNoiseNone>();
+
+  AccelerometerModel::Ptr a_model =
+      std::make_shared<AccelerometerModel>(intrinsics, noise);
+  GyroscopeModel::Ptr g_model =
+      std::make_shared<GyroscopeModel>(intrinsics, noise);
+
+  ImuModel::Ptr model(std::make_shared<ImuModel>(a_model, g_model));
+
+  ImuBufferLinear5000 buffer(model);
+  Vector3 a1 = Vector3::Random();
+  Vector3 a2 = Vector3::Random();
+  Vector3 a3 = Vector3::Random();
+  buffer.insertAccelerometerMeasurement(10, a1);
+  buffer.insertAccelerometerMeasurement(20, a2);
+  buffer.insertAccelerometerMeasurement(30, a3);
+
+  Vector3 w1 = Vector3::Random();
+  Vector3 w2 = Vector3::Random();
+  Vector3 w3 = Vector3::Random();
+  buffer.insertGyroscopeMeasurement(11, w1);
+  buffer.insertGyroscopeMeasurement(21, w2);
+  buffer.insertGyroscopeMeasurement(33, w3);
+
+  ImuAccGyr i1 = ImuAccGyr::Random();
+  buffer.insertImuMeasurement(40, i1);
+
+  ImuAccGyr out1;
+  EXPECT_TRUE(buffer.get(15, out1));
+  EXPECT_TRUE(EIGEN_MATRIX_NEAR((a1 + a2)/2, out1.head<3>(3), 1e-6));
+  EXPECT_TRUE(buffer.get(16, out1));
+  EXPECT_TRUE(EIGEN_MATRIX_NEAR((w1 + w2)/2, out1.tail<3>(3), 1e-6));
+
+  // Between interpolation:
+  ImuAccGyrContainer values;
+  ImuStamps stamps;
+  std::tie(stamps, values) = buffer.getBetweenValuesInterpolated(15, 35);
+
+  ImuStamps ref_stamps(4); ref_stamps << 15, 21, 33, 35;
+  EXPECT_TRUE(EIGEN_MATRIX_NEAR(ref_stamps, stamps, 1e-8));
+
+  // Check interpolation of accelerometer
+  buffer.get(stamps(0), out1);
+  EXPECT_TRUE(EIGEN_MATRIX_NEAR(values.col(0), out1, 1e-6));
+  buffer.get(stamps(1), out1);
+  EXPECT_TRUE(EIGEN_MATRIX_NEAR(values.col(1), out1, 1e-6));
+  buffer.get(stamps(2), out1);
+  EXPECT_TRUE(EIGEN_MATRIX_NEAR(values.col(2), out1, 1e-6));
+  buffer.get(stamps(3), out1);
+  EXPECT_TRUE(EIGEN_MATRIX_NEAR(values.col(3), out1, 1e-6));
+
+  // Test Interpolated bounds:
+  // lower out of bound:
+  std::tie(stamps, values) = buffer.getBetweenValuesInterpolated(0, 35);
+  EXPECT_EQ(0, stamps.size());
+  EXPECT_EQ(0, values.cols());
+
+  std::tie(stamps,values) = buffer.getBetweenValuesInterpolated(15, 50);
+  EXPECT_EQ(0, stamps.size());
+  EXPECT_EQ(0, values.cols());
+
+  std::tie(stamps, values) = buffer.getBetweenValuesInterpolated(0, 50);
+  EXPECT_EQ(0, stamps.size());
+  EXPECT_EQ(0, values.cols());
+
+  // Test Bounds Get:
+  EXPECT_FALSE(buffer.get(0, out1));
+  EXPECT_FALSE(buffer.get(50, out1));
+}
+
+TEST(ImuBufferTest, testOldestAndNewestTimestamp)
+{
+  using namespace ze;
+  std::shared_ptr<ImuIntrinsicModelCalibrated> intrinsics =
+      std::make_shared<ImuIntrinsicModelCalibrated>();
+  std::shared_ptr<ImuNoiseNone> noise = std::make_shared<ImuNoiseNone>();
+
+  AccelerometerModel::Ptr a_model =
+      std::make_shared<AccelerometerModel>(intrinsics, noise);
+  GyroscopeModel::Ptr g_model =
+      std::make_shared<GyroscopeModel>(intrinsics, noise);
+
+  ImuModel::Ptr model(std::make_shared<ImuModel>(a_model, g_model));
+
+  ImuBufferLinear5000 buffer(model);
+
+  int64_t oldest;
+  int64_t newest;
+  bool success;
+  std::tie(oldest, newest, success) = buffer.getOldestAndNewestStamp();
+  EXPECT_FALSE(success);
+
+  buffer.insertImuMeasurement(1, ImuAccGyr::Random());
+
+  std::tie(oldest, newest, success) = buffer.getOldestAndNewestStamp();
+  EXPECT_TRUE(success);
+  EXPECT_EQ(1, oldest);
+  EXPECT_EQ(1, newest);
+
+  buffer.insertImuMeasurement(2, ImuAccGyr::Random());
+
+  std::tie(oldest, newest, success) = buffer.getOldestAndNewestStamp();
+  EXPECT_TRUE(success);
+  EXPECT_EQ(1, oldest);
+  EXPECT_EQ(2, newest);
+
+}
+
+TEST(ImuBufferTest, testDifferentiation)
+{
+
+  using namespace ze;
+  typedef Ringbuffer<real_t, 3, 100> RingBuffer_t;
+  VectorX m;
+  Vector6 ref;
+
+  RingBuffer_t buffer;
+  buffer.insert(10., Vector3::Zero());
+  buffer.insert(20., 5. * Vector3::Ones());
+  buffer.insert(30., 15. * Vector3::Ones());
+
+  std::lock_guard<std::mutex> lock(buffer.mutex());
+
+  //test differentiation between support points.
+  m = InterpolatorDifferentiatorLinear::interpolate<RingBuffer_t>(&buffer, 15.);  ref << 2.5 * Vector3::Ones(), .5 * Vector3::Ones();
+  ref << 2.5 * Vector3::Ones(), .5 * Vector3::Ones();
+  EXPECT_TRUE(EIGEN_MATRIX_NEAR(ref, m, 1e-10));
+
+  //test differentiation at support point.
+  //the expected derivative is the one of the segment starting with this point.
+  m = InterpolatorDifferentiatorLinear::interpolate<RingBuffer_t>(&buffer, 20.);
+  ref << 5. * Vector3::Ones(), 1. * Vector3::Ones();
+  EXPECT_TRUE(EIGEN_MATRIX_NEAR(ref, m, 1e-10));
+}
+
+ZE_UNITTEST_ENTRYPOINT
diff --git a/RWR/src/ze_oss/ze_imu/test/test_imu_model.cpp b/RWR/src/ze_oss/ze_imu/test/test_imu_model.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..539e8de3d8543dba6aed15c621c6011e2e9dacf3
--- /dev/null
+++ b/RWR/src/ze_oss/ze_imu/test/test_imu_model.cpp
@@ -0,0 +1,80 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include <ze/common/test_entrypoint.hpp>
+
+#include <ze/imu/imu_model.hpp>
+
+TEST(ImuModelTest, testImu)
+{
+  using namespace ze;
+  std::shared_ptr<ImuIntrinsicModelCalibrated> intrinsics =
+      std::make_shared<ImuIntrinsicModelCalibrated>();
+  std::shared_ptr<ImuNoiseNone> noise = std::make_shared<ImuNoiseNone>();
+
+  AccelerometerModel::Ptr a_model =
+      std::make_shared<AccelerometerModel>(intrinsics, noise);
+  GyroscopeModel::Ptr g_model =
+      std::make_shared<GyroscopeModel>(intrinsics, noise);
+
+  ImuModel model(a_model, g_model);
+
+  EXPECT_EQ(a_model, model.accelerometerModel());
+  EXPECT_EQ(g_model, model.gyroscopeModel());
+}
+
+TEST(ImuModelTest, testUndistortion)
+{
+  using namespace ze;
+  Matrix3 M;
+  M << 0., 0., 1.,
+       0., 1., 0.,
+       1., 0., 0.;
+  std::shared_ptr<ImuIntrinsicModelScaleMisalignment> intrinsics =
+      std::make_shared<ImuIntrinsicModelScaleMisalignment>(
+        0.0,
+        ImuIntrinsicModel::UndefinedRange,
+        Vector3::Zero(),
+        M);
+  std::shared_ptr<ImuNoiseNone> noise = std::make_shared<ImuNoiseNone>();
+
+  AccelerometerModel::Ptr a_model =
+      std::make_shared<AccelerometerModel>(intrinsics, noise);
+  GyroscopeModel::Ptr g_model =
+      std::make_shared<GyroscopeModel>(intrinsics, noise);
+
+  ImuModel model(a_model, g_model);
+
+  Vector3 gyro_measurement;
+  Vector3 accel_measurement;
+  accel_measurement << 1., 2., 3.;
+  gyro_measurement << 4., 5., 6.;
+  Vector6 undistorted_measurement = model.undistort(accel_measurement, gyro_measurement);
+  EXPECT_EQ(undistorted_measurement, (Vector6() << 3., 2., 1., 6., 5., 4.).finished());
+
+  EXPECT_EQ(a_model, model.accelerometerModel());
+  EXPECT_EQ(g_model, model.gyroscopeModel());
+}
+ZE_UNITTEST_ENTRYPOINT
diff --git a/RWR/src/ze_oss/ze_imu/test/test_imu_types.cpp b/RWR/src/ze_oss/ze_imu/test/test_imu_types.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..94ed2df0a5e694602f59e09fd3e4da8dc9980008
--- /dev/null
+++ b/RWR/src/ze_oss/ze_imu/test/test_imu_types.cpp
@@ -0,0 +1,47 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include <ze/common/test_entrypoint.hpp>
+
+#include <ze/imu/imu_types.hpp>
+
+TEST(ImuModelTest, testImuGetAccGetGyr)
+{
+  using namespace ze;
+
+  ImuAccGyr m = ImuAccGyr::Random();
+
+  // Mutable:
+  getAcc(m) = Vector3(1, 2, 3);
+  EXPECT_EQ(Vector3(1, 2, 3).eval(), m.head<3>());
+  getGyr(m) = Vector3(4, 5, 6);
+  EXPECT_EQ(Vector3(4, 5, 6).eval(), m.tail<3>());
+
+  const ImuAccGyr m_const = m;
+  EXPECT_EQ(getAcc(m), getAcc(m_const));
+  EXPECT_EQ(getGyr(m), getGyr(m_const));
+}
+
+ZE_UNITTEST_ENTRYPOINT
diff --git a/RWR/src/ze_oss/ze_imu/test/test_intrinsic_model.cpp b/RWR/src/ze_oss/ze_imu/test/test_intrinsic_model.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..078058312b5dfbd43577ff80afd91619c9544cd0
--- /dev/null
+++ b/RWR/src/ze_oss/ze_imu/test/test_intrinsic_model.cpp
@@ -0,0 +1,120 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include <ze/common/test_entrypoint.hpp>
+
+#include <ze/imu/imu_intrinsic_model.hpp>
+
+TEST(IntrinsicModelTests, testIntrinsicModelCalibrated)
+{
+  using namespace ze;
+  ImuIntrinsicModelCalibrated::Ptr model =
+      std::make_shared<ImuIntrinsicModelCalibrated>();
+
+  ASSERT_TRUE(ImuIntrinsicType::Calibrated == model->type());
+}
+
+TEST(IntrinsicModelTests, testIntrinsicModelScaleMisalignment)
+{
+  using namespace ze;
+  Vector3 b; b << 1.0, 2.0, 3.0;
+  Matrix3 M; M << 1.0, 0.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0;
+  ImuIntrinsicModelScaleMisalignment::Ptr model =
+      std::make_shared<ImuIntrinsicModelScaleMisalignment>(0.1, 10, b, M);
+
+  EXPECT_TRUE(ImuIntrinsicType::ScaleMisalignment == model->type());
+  EXPECT_DOUBLE_EQ(0.1, model->delay());
+  EXPECT_EQ(10, model->range());
+  EXPECT_TRUE(EIGEN_MATRIX_EQUAL_DOUBLE(b, model->b()));
+  EXPECT_TRUE(EIGEN_MATRIX_EQUAL_DOUBLE(M, model->M()));
+
+  Vector3 primary = Vector3::Random();
+  Vector3 secondary = Vector3::Random();
+  EXPECT_TRUE(EIGEN_MATRIX_EQUAL_DOUBLE(primary, model->undistort(
+      model->distort(primary, secondary), secondary)));
+
+  //make sure that inputs actually get altered.
+  EXPECT_FALSE(EIGEN_MATRIX_EQUAL_DOUBLE(model->distort(
+      primary, secondary), model->undistort(primary, secondary)));
+}
+
+TEST(IntrinsicModelTests, testIntrinsicModelScaleMisalignmentGSensitivity)
+{
+  using namespace ze;
+  Vector3 b; b << 1.0, 2.0, 3.0;
+  Matrix3 M; M << 1.0, 0.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0;
+  Matrix3 Ma; Ma << 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0;
+
+  ImuIntrinsicModelScaleMisalignmentGSensitivity::Ptr model =
+      std::make_shared<ImuIntrinsicModelScaleMisalignmentGSensitivity>(
+        0.1, 10, b, M, Ma);
+
+  ASSERT_TRUE(ImuIntrinsicType::ScaleMisalignmentGSensitivity == model->type());
+
+  EXPECT_DOUBLE_EQ(0.1, model->delay());
+  EXPECT_EQ(10, model->range());
+  EXPECT_TRUE(EIGEN_MATRIX_EQUAL_DOUBLE(b, model->b()));
+  EXPECT_TRUE(EIGEN_MATRIX_EQUAL_DOUBLE(M, model->M()));
+  EXPECT_TRUE(EIGEN_MATRIX_EQUAL_DOUBLE(Ma, model->Ma()));
+
+  Vector3 primary = Vector3::Random();
+  Vector3 secondary = Vector3::Random();
+  EXPECT_TRUE(EIGEN_MATRIX_EQUAL_DOUBLE(primary, model->undistort(
+      model->distort(primary, secondary), secondary)));
+
+  //make sure that inputs actually get altered.
+  EXPECT_FALSE(EIGEN_MATRIX_EQUAL_DOUBLE(model->distort(
+      primary, secondary), model->undistort(primary, secondary)));
+}
+
+TEST(IntrinsicModelTests, testIntrinsicModelScaleMisalignmentSizeEffect)
+{
+  using namespace ze;
+  Vector3 b; b << 1.0, 2.0, 3.0;
+  Matrix3 M; M << 1.0, 0.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0;
+  Matrix3 R; R << 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0;
+  ImuIntrinsicModelScaleMisalignmentSizeEffect::Ptr model =
+      std::make_shared<ImuIntrinsicModelScaleMisalignmentSizeEffect>(
+        0.1, 10, b, M, R);
+
+  ASSERT_TRUE(ImuIntrinsicType::ScaleMisalignmentSizeEffect == model->type());
+
+  EXPECT_DOUBLE_EQ(0.1, model->delay());
+  EXPECT_EQ(10, model->range());
+  EXPECT_TRUE(EIGEN_MATRIX_EQUAL_DOUBLE(b, model->b()));
+  EXPECT_TRUE(EIGEN_MATRIX_EQUAL_DOUBLE(M, model->M()));
+  EXPECT_TRUE(EIGEN_MATRIX_EQUAL_DOUBLE(R, model->R()));
+
+  Vector3 primary = Vector3::Random();
+  Vector6 secondary = Vector6::Random();
+  EXPECT_TRUE(EIGEN_MATRIX_EQUAL_DOUBLE(primary, model->undistort(
+      model->distort(primary, secondary), secondary)));
+
+  //make sure that inputs actually get altered.
+  EXPECT_FALSE(EIGEN_MATRIX_EQUAL_DOUBLE(model->distort(
+      primary, secondary), model->undistort(primary, secondary)));
+}
+
+ZE_UNITTEST_ENTRYPOINT
diff --git a/RWR/src/ze_oss/ze_imu/test/test_noise_model.cpp b/RWR/src/ze_oss/ze_imu/test/test_noise_model.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..8859e887ea16687b15cbb13325dbe57f6a4e7478
--- /dev/null
+++ b/RWR/src/ze_oss/ze_imu/test/test_noise_model.cpp
@@ -0,0 +1,42 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include <ze/common/test_entrypoint.hpp>
+
+#include <ze/imu/imu_noise_model.hpp>
+
+TEST(NoiseModelTests, testWhiteBrownian)
+{
+  using namespace ze;
+  ImuNoiseWhiteBrownian::Ptr noise = std::make_shared<ImuNoiseWhiteBrownian>(
+                                    0.1, 2u, 0.2);
+
+  ASSERT_DOUBLE_EQ(0.1, noise->noiseDensity());
+  ASSERT_EQ(2, noise->bandwidth());
+  ASSERT_DOUBLE_EQ(0.2, noise->biasNoiseDensity());
+  ASSERT_TRUE(ImuNoiseType::WhiteBrownian == noise->type());
+}
+
+ZE_UNITTEST_ENTRYPOINT
diff --git a/RWR/src/ze_oss/ze_imu/test/test_yaml_serialization.cpp b/RWR/src/ze_oss/ze_imu/test/test_yaml_serialization.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..79d5fc680caeff30c597ce8863017ad3025dd43a
--- /dev/null
+++ b/RWR/src/ze_oss/ze_imu/test/test_yaml_serialization.cpp
@@ -0,0 +1,65 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include <string>
+
+#include <ze/common/test_entrypoint.hpp>
+#include <ze/common/test_utils.hpp>
+#include <ze/common/path_utils.hpp>
+
+#include <ze/imu/imu_model.hpp>
+#include <ze/imu/imu_rig.hpp>
+#include <ze/imu/imu_yaml_serialization.hpp>
+
+TEST(ImuYamlSerialization, testYamlLoading)
+{
+  std::string data_dir = ze::getTestDataDir("imu_models");
+  std::string yaml_file = ze::joinPath(data_dir, "/zurich_eye_one.yaml");
+  ASSERT_TRUE(ze::fileExists(yaml_file));
+
+  ze::ImuRig::Ptr rig = ze::ImuRig::loadFromYaml(yaml_file);
+
+  EXPECT_EQ(rig->size(), 3);
+  EXPECT_STREQ(rig->label().c_str(), "zuricheyeonetest");
+  EXPECT_STREQ(rig->at(0).label().c_str(), "bmx0");
+  EXPECT_EQ(ze::ImuIntrinsicType::ScaleMisalignment,
+            rig->at(0).accelerometerModel()->intrinsicModel()->type());
+  EXPECT_EQ(ze::ImuIntrinsicType::ScaleMisalignment,
+            rig->at(0).gyroscopeModel()->intrinsicModel()->type());
+
+  EXPECT_STREQ(rig->at(1).label().c_str(), "bmx1");
+  EXPECT_EQ(ze::ImuIntrinsicType::ScaleMisalignmentSizeEffect,
+            rig->at(1).accelerometerModel()->intrinsicModel()->type());
+  EXPECT_EQ(ze::ImuIntrinsicType::ScaleMisalignmentGSensitivity,
+            rig->at(1).gyroscopeModel()->intrinsicModel()->type());
+
+  EXPECT_STREQ(rig->at(2).label().c_str(), "bmx2");
+  EXPECT_EQ(ze::ImuIntrinsicType::Calibrated,
+            rig->at(2).accelerometerModel()->intrinsicModel()->type());
+  EXPECT_EQ(ze::ImuIntrinsicType::Calibrated,
+            rig->at(2).gyroscopeModel()->intrinsicModel()->type());
+}
+
+ZE_UNITTEST_ENTRYPOINT
diff --git a/RWR/src/ze_oss/ze_matplotlib/CMakeLists.txt b/RWR/src/ze_oss/ze_matplotlib/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..8a628cdc1227cc1bcdc3491c3148c81d7c96044a
--- /dev/null
+++ b/RWR/src/ze_oss/ze_matplotlib/CMakeLists.txt
@@ -0,0 +1,38 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(ze_matplotlib)
+
+find_package(catkin_simple REQUIRED)
+catkin_simple()
+
+include(ze_setup)
+
+# The matplotlib-cpp headers are linked against
+find_package(PythonLibs)
+include_directories(${PYTHON_INCLUDE_DIRS})
+
+#############
+# LIBRARIES #
+#############
+set(HEADERS
+    include/ze/matplotlib/matplotlibcpp.hpp
+  )
+
+set(SOURCES
+    src/matplotlibcpp.cpp
+  )
+
+cs_add_library(${PROJECT_NAME} ${SOURCES} ${HEADERS})
+target_link_libraries(${PROJECT_NAME} ${PYTHON_LIBRARY})
+
+###############
+# EXECUTABLES #
+###############
+
+cs_add_executable(mpl_example_node src/mpl_example_node.cpp)
+target_link_libraries(mpl_example_node ${PROJECT_NAME})
+
+##########
+# EXPORT #
+##########
+cs_install()
+cs_export()
diff --git a/RWR/src/ze_oss/ze_matplotlib/LICENSE b/RWR/src/ze_oss/ze_matplotlib/LICENSE
new file mode 100644
index 0000000000000000000000000000000000000000..997235abf2a895ace61486041086dc772de3098e
--- /dev/null
+++ b/RWR/src/ze_oss/ze_matplotlib/LICENSE
@@ -0,0 +1,48 @@
+Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+Derived from work of Benno Evers licensed:
+
+The MIT License (MIT)
+
+Copyright (c) 2014 Benno Evers
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.
\ No newline at end of file
diff --git a/RWR/src/ze_oss/ze_matplotlib/README.md b/RWR/src/ze_oss/ze_matplotlib/README.md
new file mode 100644
index 0000000000000000000000000000000000000000..709a608fc287b22f609b4db000e0e3a8e8bb71df
--- /dev/null
+++ b/RWR/src/ze_oss/ze_matplotlib/README.md
@@ -0,0 +1,97 @@
+# C++ Interface to Matplotlib
+
+## Principles
+
+- `plt::plot(...)` automatically creates a new figure
+- `plt::show()` shows the results of the plot
+- calls to `plt::plot(...)` go to the same plot until `plt::show()` is called
+- by default, calls to `plt::show()` are blocking and execution continues only after the plot window is closed
+
+## Simple plot of a vector:
+```
+// Std Vector:
+std::vector<double> v({1, 2, 3, 4});
+plt::plot(v);
+plt::show();
+
+// Eigen Types:
+VectorX w(100);
+w.setRandom();
+plt::plot(w)
+plt::show();
+```
+
+## Plot x and y
+```
+VectorX t(100);
+t.setLinSpaced(100, 0, 20);
+VectorX v(100);
+v.setRandom();
+plt::plot(t, v);
+plt::show();
+```
+
+## Subplots
+```
+plt::subplot(3, 1, 1);
+plt::plot(v1);
+plt::subplot(3, 1, 2);
+plt::plot(v2);
+plt::subplot(3, 1, 3);
+plt::plot(v2);
+plt::plot();
+```
+
+## Histogram
+
+The histogram function only exposes the `bins`, `data` and `type` parameters of matplotlibs histogram function.
+```
+plt::hist(v, 10, "bar");
+plt::show();
+```
+
+## Formatting
+
+All functions accept optional format arguments (`plt::plot(x, y, format)` or `plt::plot(y, format)`). The
+expected format strings are equivalent to the matplotlib format strings:
+```
+plt::plot(t, v, "rx"); // red x'es
+plt::show();
+```
+
+## Multiple open Plots
+
+`plt::show(false)` opens a non-blocking window. You have to open a new figure to get a second plot ( `plt::figure()`).
+The final figure should be opened in blocking mode to keep all windows open.
+
+```
+plt::plot(v1);
+plt::show(false); // show non-blocking
+
+...
+
+plt::figure(); // new figure
+plt::plot(v2)
+plt::show(false); // open non-blocking
+
+...
+
+plt::figure();
+plt::plot(v3);
+plt::show(); // blocking to keep figures 1-3 open.
+
+
+```
+
+`plt::figure()` accepts a string argument that labels / identifies a specific plot. 
+
+## Style and Labels
+
+```
+plt::plot(t, x);
+plt::xlabel("time [s]");
+plt::ylabel("Position [m]");
+plt::title("Position over time");
+plt::xlim(t_start, t_end);
+plt::ylim(0, x_max);
+```
diff --git a/RWR/src/ze_oss/ze_matplotlib/include/ze/matplotlib/matplotlibcpp.hpp b/RWR/src/ze_oss/ze_matplotlib/include/ze/matplotlib/matplotlibcpp.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..0a5daf57228ccf78576f88463dd55f938d1ea153
--- /dev/null
+++ b/RWR/src/ze_oss/ze_matplotlib/include/ze/matplotlib/matplotlibcpp.hpp
@@ -0,0 +1,180 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Derived from work of Benno Evers licensed:
+//
+// The MIT License (MIT)
+//
+// Copyright (c) 2014 Benno Evers
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in all
+// copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+
+#pragma once
+
+#include <vector>
+#include <map>
+#include <numeric>
+#include <stdexcept>
+#include <iostream>
+#include <ze/common/types.hpp>
+#include <initializer_list>
+
+
+namespace ze {
+namespace plt {
+
+//! Enable interactive mode.
+bool ion();
+
+//! Create a new figure.
+bool figure(std::string i = "");
+
+//! Histogram.
+bool hist(
+    const Eigen::Ref<const VectorX>& x,
+    const double bins = 10,
+    const std::string histtype = "bar");
+
+//! Every row of X is the data for a box.
+bool boxplot(
+    const Eigen::Ref<const MatrixX>& x,
+    const std::vector<std::string>& labels);
+bool boxplot(
+    const Eigen::Ref<const MatrixX>& x,
+    std::initializer_list<const std::string> labels);
+bool boxplot(
+    const Eigen::Ref<const MatrixX>& x);
+
+//! Create a subplot.
+bool subplot(const size_t nrows, const size_t ncols, const size_t plot_number);
+
+//! Create an x/y plot with properties as map.
+bool plot(
+    const Eigen::Ref<const MatrixX>& x,
+    const Eigen::Ref<const MatrixX>& y,
+    const std::map<std::string, std::string>& keywords);
+
+//! Create an x/y plot with properties in string.
+bool plot(
+    const Eigen::Ref<const MatrixX>& x,
+    const Eigen::Ref<const MatrixX>& y,
+    const std::string& s = "");
+
+//! Create an x/y plot with name as label.
+bool labelPlot(
+    const std::string& name,
+    const Eigen::Ref<const MatrixX>& x,
+    const Eigen::Ref<const MatrixX>& y,
+    const std::string& format = "");
+bool labelPlot(
+    const std::string& name,
+    const Eigen::Ref<const MatrixX>& y,
+    const std::string& format = "");
+
+bool plot(
+    const Eigen::Ref<const MatrixX>& x,
+    const std::string& format = "");
+
+// -----------------------------------------------------------------------------
+//! @name std::vector wrappers.
+//! @{
+bool plot(
+    const std::vector<real_t>& y,
+    const std::string& format = "");
+
+bool plot(
+    const std::vector<real_t>& x,
+    const std::vector<real_t>& y,
+    const std::map<std::string, std::string>& keywords);
+
+bool plot(
+    const std::vector<real_t>& x,
+    const Eigen::Ref<const MatrixX>& y,
+    const std::map<std::string, std::string>& keywords);
+
+bool plot(
+    const std::vector<real_t>& x,
+    const std::vector<real_t>& y,
+    const std::string& s = "");
+bool plot(
+    const std::vector<real_t>& x,
+    const Eigen::Ref<const MatrixX>& y,
+    const std::string& s = "");
+
+bool labelPlot(
+    const std::string& name,
+    const std::vector<real_t>& x,
+    const std::vector<real_t>& y,
+    const std::string& format = "");
+
+bool labelPlot(
+    const std::string& name,
+    const std::vector<real_t>& x,
+    const Eigen::Ref<const MatrixX>& y,
+    const std::string& format = "");
+//! @}
+
+// -----------------------------------------------------------------------------
+//! @name Plot settings.
+//! @{
+void legend();
+
+void ylim(real_t min, real_t max);
+
+void xlim(real_t xmin, real_t xmax);
+
+void title(const std::string &titlestr);
+
+void axis(const std::string &axisstr);
+
+void xlabel(const std::string &str);
+
+void ylabel(const std::string &str);
+
+void grid(bool flag);
+
+void show(bool block = true);
+
+void save(const std::string& filename);
+//! @}
+
+} // namespace plt
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_matplotlib/package.xml b/RWR/src/ze_oss/ze_matplotlib/package.xml
new file mode 100644
index 0000000000000000000000000000000000000000..a8e79018fae4006408978e20b1ae7d427f1baa01
--- /dev/null
+++ b/RWR/src/ze_oss/ze_matplotlib/package.xml
@@ -0,0 +1,19 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>ze_matplotlib</name>
+  <version>0.1.4</version>
+  <description>The ze_matplotlib package</description>
+
+  <maintainer email="luc.oth@WyssZurich.ch">Luc Oth</maintainer>
+  <license>ZE</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <buildtool_depend>catkin_simple</buildtool_depend>
+
+  <depend>eigen_catkin</depend>
+  <depend>ze_cmake</depend>
+  <depend>ze_common</depend>
+  <depend>PythonLibs</depend>
+
+<test_depend>gtest</test_depend>
+</package>
diff --git a/RWR/src/ze_oss/ze_matplotlib/src/matplotlibcpp.cpp b/RWR/src/ze_oss/ze_matplotlib/src/matplotlibcpp.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..29409851b15c801e39153b5d7d285b9d26abb454
--- /dev/null
+++ b/RWR/src/ze_oss/ze_matplotlib/src/matplotlibcpp.cpp
@@ -0,0 +1,851 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Derived from work of Benno Evers licensed:
+//
+// The MIT License (MIT)
+//
+// Copyright (c) 2014 Benno Evers
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in all
+// copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+
+#include <ze/matplotlib/matplotlibcpp.hpp>
+
+#include <ze/common/logging.hpp>
+
+#include <Python.h>
+
+namespace ze {
+namespace plt {
+
+namespace detail {
+
+// -----------------------------------------------------------------------------
+struct _interpreter
+{
+  PyObject *s_python_function_show;
+  PyObject *s_python_function_save;
+  PyObject *s_python_function_figure;
+  PyObject *s_python_function_plot;
+  PyObject *s_python_function_subplot;
+  PyObject *s_python_function_hist;
+  PyObject *s_python_function_boxplot;
+  PyObject *s_python_function_legend;
+  PyObject *s_python_function_xlim;
+  PyObject *s_python_function_ylim;
+  PyObject *s_python_function_title;
+  PyObject *s_python_function_axis;
+  PyObject *s_python_function_xlabel;
+  PyObject *s_python_function_ylabel;
+  PyObject *s_python_function_ion;
+  PyObject *s_python_function_grid;
+  PyObject *s_python_empty_tuple;
+
+  /** For now, _interpreter is implemented as a singleton since its currently
+   * not possible to have multiple independent embedded python interpreters
+   * without patching the python source code or starting a seperate process
+   * for each.
+   * http://bytes.com/topic/python/answers/793370-multiple-independent-python-interpreters-c-c-program
+   */
+
+  static _interpreter& get()
+  {
+    static _interpreter ctx;
+    return ctx;
+  }
+
+private:
+  _interpreter()
+  {
+    char name[] = "plotting"; // silence compiler warning about const strings
+    //Py_SetProgramName(name);  // optional but recommended
+    Py_Initialize();
+
+    PyObject* pyplotname = PyBytes_FromString("matplotlib.pyplot");
+    PyObject* pylabname  = PyBytes_FromString("pylab");
+    if (!pyplotname || !pylabname)
+    {
+      throw std::runtime_error("couldnt create string");
+    }
+
+    PyObject* pymod = PyImport_Import(pyplotname);
+    Py_DECREF(pyplotname);
+    if (!pymod) {
+      throw std::runtime_error("Error loading module matplotlib.pyplot!");
+    }
+
+    PyObject* pylabmod = PyImport_Import(pylabname);
+    Py_DECREF(pylabname);
+    if (!pymod)
+    {
+      throw std::runtime_error("Error loading module pylab!");
+    }
+
+    s_python_function_show = PyObject_GetAttrString(pymod, "show");
+    s_python_function_figure = PyObject_GetAttrString(pymod, "figure");
+    s_python_function_plot = PyObject_GetAttrString(pymod, "plot");
+    s_python_function_subplot = PyObject_GetAttrString(pymod, "subplot");
+    s_python_function_hist = PyObject_GetAttrString(pymod, "hist");
+    s_python_function_boxplot = PyObject_GetAttrString(pymod, "boxplot");
+    s_python_function_legend = PyObject_GetAttrString(pymod, "legend");
+    s_python_function_ylim = PyObject_GetAttrString(pymod, "ylim");
+    s_python_function_title = PyObject_GetAttrString(pymod, "title");
+    s_python_function_axis = PyObject_GetAttrString(pymod, "axis");
+    s_python_function_xlabel = PyObject_GetAttrString(pymod, "xlabel");
+    s_python_function_ylabel = PyObject_GetAttrString(pymod, "ylabel");
+    s_python_function_grid = PyObject_GetAttrString(pymod, "grid");
+    s_python_function_xlim = PyObject_GetAttrString(pymod, "xlim");
+    s_python_function_ion = PyObject_GetAttrString(pymod, "ion");
+
+    s_python_function_save = PyObject_GetAttrString(pylabmod, "savefig");
+
+    if(!s_python_function_show
+       || !s_python_function_save
+       || !s_python_function_figure
+       || !s_python_function_plot
+       || !s_python_function_subplot
+       || !s_python_function_hist
+       || !s_python_function_boxplot
+       || !s_python_function_legend
+       || !s_python_function_xlim
+       || !s_python_function_ylim
+       || !s_python_function_title
+       || !s_python_function_axis
+       || !s_python_function_xlabel
+       || !s_python_function_ylabel
+       || !s_python_function_ion
+       || !s_python_function_grid
+       )
+    {
+      throw std::runtime_error("Couldnt find required function!");
+    }
+
+    if(!PyFunction_Check(s_python_function_show)
+       || !PyFunction_Check(s_python_function_save)
+       || !PyFunction_Check(s_python_function_figure)
+       || !PyFunction_Check(s_python_function_plot)
+       || !PyFunction_Check(s_python_function_subplot)
+       || !PyFunction_Check(s_python_function_hist)
+       || !PyFunction_Check(s_python_function_boxplot)
+       || !PyFunction_Check(s_python_function_legend)
+       || !PyFunction_Check(s_python_function_xlim)
+       || !PyFunction_Check(s_python_function_ylim)
+       || !PyFunction_Check(s_python_function_title)
+       || !PyFunction_Check(s_python_function_axis)
+       || !PyFunction_Check(s_python_function_xlabel)
+       || !PyFunction_Check(s_python_function_ylabel)
+       || !PyFunction_Check(s_python_function_ion)
+       || !PyFunction_Check(s_python_function_grid)
+       )
+    {
+      throw std::runtime_error("Python object is unexpectedly not a PyFunction.");
+    }
+
+    s_python_empty_tuple = PyTuple_New(0);
+  }
+
+  ~_interpreter()
+  {
+    Py_Finalize();
+  }
+};
+} // namespace detail
+
+// -----------------------------------------------------------------------------
+bool ion()
+{
+
+  PyObject* args = PyTuple_New(0);
+  PyObject* res = PyObject_CallObject(
+                    detail::_interpreter::get().s_python_function_ion,
+                    args);
+
+  if (res)
+  {
+    Py_DECREF(res);
+  }
+
+  return res;
+}
+
+// -----------------------------------------------------------------------------
+//! Create a new figure
+bool figure(std::string i)
+{
+  PyObject* args;
+  if (i != "")
+  {
+    args = PyTuple_New(1);
+    PyObject* i_py = PyBytes_FromString(i.c_str());
+    PyTuple_SetItem(args, 0, i_py);
+  }
+  else
+  {
+    args = PyTuple_New(0);
+  }
+
+  PyObject* res = PyObject_CallObject(
+                    detail::_interpreter::get().s_python_function_figure,
+                    args);
+
+  if (res)
+  {
+    Py_DECREF(res);
+  }
+
+  return res;
+}
+
+// -----------------------------------------------------------------------------
+bool hist(
+    const Eigen::Ref<const VectorX>& x,
+    const double bins,
+    const std::string histtype)
+{
+  // using python lists
+  PyObject* xlist = PyList_New(x.size());
+
+  for (int i = 0; i < x.size(); ++i)
+  {
+    PyList_SetItem(xlist, i, PyFloat_FromDouble(x(i)));
+  }
+
+  PyObject* bins_py = PyFloat_FromDouble(bins);
+  PyObject* histtype_py = PyBytes_FromString(histtype.c_str());
+
+  // construct positional args
+  PyObject* args = PyTuple_New(8);
+  PyTuple_SetItem(args, 0, xlist);
+  PyTuple_SetItem(args, 1, bins_py);
+  PyTuple_SetItem(args, 2, Py_None);
+  PyTuple_SetItem(args, 3, Py_False);
+  PyTuple_SetItem(args, 4, Py_None);
+  PyTuple_SetItem(args, 5, Py_False);
+  PyTuple_SetItem(args, 6, Py_None);
+  PyTuple_SetItem(args, 7, histtype_py);
+
+  PyObject* res = PyObject_CallObject(
+                    detail::_interpreter::get().s_python_function_hist,
+                    args);
+
+  Py_DECREF(xlist);
+  Py_DECREF(bins_py);
+  Py_DECREF(histtype_py);
+  if (res)
+  {
+    Py_DECREF(res);
+  }
+
+  return res;
+}
+
+// -----------------------------------------------------------------------------
+bool boxplot(
+    const Eigen::Ref<const MatrixX>& x,
+    const std::vector<std::string>& labels)
+{
+  CHECK_EQ(x.rows(), static_cast<int>(labels.size()));
+
+  // using python lists
+  PyObject* data = PyList_New(x.rows());
+  PyObject* py_labels = PyList_New(x.rows());
+
+  for (int i = 0; i < x.rows(); ++i)
+  {
+    PyObject* row = PyList_New(x.cols());
+
+    PyList_SetItem(py_labels, i, PyBytes_FromString(labels[i].c_str()));
+
+    for (int j = 0; j < x.cols(); ++j)
+    {
+      PyList_SetItem(row, j, PyFloat_FromDouble(x(i, j)));
+    }
+    PyList_SetItem(data, i, row);
+  }
+
+  // construct positional args
+  PyObject* args = PyTuple_New(1);
+  PyTuple_SetItem(args, 0, data);
+
+  // construct keyword args
+  PyObject* kwargs = PyDict_New();
+  PyDict_SetItemString(kwargs,
+                       "labels",
+                       py_labels);
+
+  PyObject* res = PyObject_Call(
+                    detail::_interpreter::get().s_python_function_boxplot,
+                    args,
+                    kwargs);
+
+  Py_DECREF(data);
+  if (res)
+  {
+    Py_DECREF(res);
+  }
+
+  return res;
+}
+
+// -----------------------------------------------------------------------------
+bool boxplot(
+    const Eigen::Ref<const MatrixX>& x,
+    std::initializer_list<const std::string> labels)
+{
+  std::vector<std::string> labels_vector;
+  labels_vector.insert(labels_vector.end(), labels.begin(), labels.end());
+
+  return boxplot(x, labels_vector);
+}
+
+// -----------------------------------------------------------------------------
+bool boxplot(
+    const Eigen::Ref<const MatrixX>& x)
+{
+  std::vector<std::string> labels;
+  for (int i = 0; i < x.rows(); ++i)
+  {
+    labels.push_back(std::to_string(i));
+  }
+  return boxplot(x, labels);
+}
+
+
+// -----------------------------------------------------------------------------
+bool subplot(const size_t nrows, const size_t ncols, const size_t plot_number)
+{
+  PyObject* nrows_py = PyFloat_FromDouble(nrows);
+  PyObject* ncols_py = PyFloat_FromDouble(ncols);
+  PyObject* plot_number_py = PyFloat_FromDouble(plot_number);
+  PyObject* subplot_args = PyTuple_New(3);
+  PyTuple_SetItem(subplot_args, 0, nrows_py);
+  PyTuple_SetItem(subplot_args, 1, ncols_py);
+  PyTuple_SetItem(subplot_args, 2, plot_number_py);
+
+  PyObject* res = PyObject_CallObject(
+                    detail::_interpreter::get().s_python_function_subplot,
+                    subplot_args);
+
+  Py_DECREF(nrows_py);
+  Py_DECREF(ncols_py);
+  Py_DECREF(plot_number_py);
+  if (res)
+  {
+    Py_DECREF(res);
+  }
+
+  return res;
+}
+
+// -----------------------------------------------------------------------------
+bool plot(
+    const Eigen::Ref<const MatrixX>& x_raw,
+    const Eigen::Ref<const MatrixX>& y_raw,
+    const std::map<std::string, std::string>& keywords)
+{
+  CHECK_EQ(true, x_raw.cols() == 1 || x_raw.rows() == 1);
+  CHECK_EQ(true, y_raw.cols() == 1 || y_raw.rows() == 1);
+
+  Eigen::Map<const VectorX> x(x_raw.data(),
+                              x_raw.rows() == 1 ? x_raw.cols() : x_raw.rows());
+  Eigen::Map<const VectorX> y(y_raw.data(),
+                              y_raw.rows() == 1 ? y_raw.cols() : y_raw.rows());
+
+  CHECK_EQ(true, x.size() == y.size());
+
+  // using python lists
+  PyObject* xlist = PyList_New(x.size());
+  PyObject* ylist = PyList_New(y.size());
+
+  for (int i = 0; i < x.size(); ++i)
+  {
+    PyList_SetItem(xlist, i, PyFloat_FromDouble(x(i)));
+    PyList_SetItem(ylist, i, PyFloat_FromDouble(y(i)));
+  }
+
+  // construct positional args
+  PyObject* args = PyTuple_New(2);
+  PyTuple_SetItem(args, 0, xlist);
+  PyTuple_SetItem(args, 1, ylist);
+
+  Py_DECREF(xlist);
+  Py_DECREF(ylist);
+
+  // construct keyword args
+  PyObject* kwargs = PyDict_New();
+  for (std::map<std::string, std::string>::const_iterator it = keywords.begin();
+       it != keywords.end(); ++it)
+  {
+    PyDict_SetItemString(kwargs,
+                         it->first.c_str(),
+                         PyBytes_FromString(it->second.c_str()));
+  }
+
+  PyObject* res = PyObject_Call(
+                    detail::_interpreter::get().s_python_function_plot,
+                    args,
+                    kwargs);
+
+  Py_DECREF(args);
+  Py_DECREF(kwargs);
+  if (res)
+  {
+    Py_DECREF(res);
+  }
+
+  return res;
+}
+
+// -----------------------------------------------------------------------------
+bool plot(
+    const Eigen::Ref<const MatrixX>&  x_raw,
+    const Eigen::Ref<const MatrixX>&  y_raw,
+    const std::string& s)
+{
+  CHECK_EQ(true, x_raw.cols() == 1 || x_raw.rows() == 1);
+  CHECK_EQ(true, y_raw.cols() == 1 || y_raw.rows() == 1);
+
+  Eigen::Map<const VectorX> x(x_raw.data(),
+                              x_raw.rows() == 1 ? x_raw.cols() : x_raw.rows());
+  Eigen::Map<const VectorX> y(y_raw.data(),
+                              y_raw.rows() == 1 ? y_raw.cols() : y_raw.rows());
+
+  CHECK_EQ(true, x.size() == y.size());
+
+  PyObject* xlist = PyList_New(x.size());
+  PyObject* ylist = PyList_New(y.size());
+  PyObject* pystring = PyBytes_FromString(s.c_str());
+
+  for (int i = 0; i < x.size(); ++i)
+  {
+    PyList_SetItem(xlist, i, PyFloat_FromDouble(x(i)));
+    PyList_SetItem(ylist, i, PyFloat_FromDouble(y(i)));
+  }
+
+  PyObject* plot_args = PyTuple_New(3);
+  PyTuple_SetItem(plot_args, 0, xlist);
+  PyTuple_SetItem(plot_args, 1, ylist);
+  PyTuple_SetItem(plot_args, 2, pystring);
+
+  PyObject* res = PyObject_CallObject(
+                    detail::_interpreter::get().s_python_function_plot,
+                    plot_args);
+
+  Py_DECREF(xlist);
+  Py_DECREF(ylist);
+  Py_DECREF(plot_args);
+  if (res)
+  {
+    Py_DECREF(res);
+  }
+
+  return res;
+}
+
+// -----------------------------------------------------------------------------
+bool labelPlot(
+    const std::string& name,
+    const Eigen::Ref<const MatrixX>& x_raw,
+    const Eigen::Ref<const MatrixX>& y_raw,
+    const std::string& format)
+{
+  CHECK_EQ(true, x_raw.cols() == 1 || x_raw.rows() == 1);
+  CHECK_EQ(true, y_raw.cols() == 1 || y_raw.rows() == 1);
+
+  Eigen::Map<const VectorX> x(x_raw.data(),
+                              x_raw.rows() == 1 ? x_raw.cols() : x_raw.rows());
+  Eigen::Map<const VectorX> y(y_raw.data(),
+                              y_raw.rows() == 1 ? y_raw.cols() : y_raw.rows());
+
+  CHECK_EQ(true, x.size() == y.size());
+
+  PyObject* kwargs = PyDict_New();
+  PyDict_SetItemString(kwargs, "label", PyBytes_FromString(name.c_str()));
+
+  PyObject* xlist = PyList_New(x.size());
+  PyObject* ylist = PyList_New(y.size());
+  PyObject* pystring = PyBytes_FromString(format.c_str());
+
+  for (int i = 0; i < x.size(); ++i)
+  {
+    PyObject* f_xi = PyFloat_FromDouble(x(i));
+    PyObject* f_yi = PyFloat_FromDouble(y(i));
+    if (!f_xi || !f_yi)
+    {
+      VLOG(1) << "MPL: value could not be converted to PyFloat:"
+              << x(i) << ", " << y(i);
+      continue;
+    }
+    PyList_SetItem(xlist, i, PyFloat_FromDouble(x(i)));
+    PyList_SetItem(ylist, i, PyFloat_FromDouble(y(i)));
+  }
+
+  PyObject* plot_args = PyTuple_New(3);
+  PyTuple_SetItem(plot_args, 0, xlist);
+  PyTuple_SetItem(plot_args, 1, ylist);
+  PyTuple_SetItem(plot_args, 2, pystring);
+
+  PyObject* res = PyObject_Call(
+                    detail::_interpreter::get().s_python_function_plot,
+                    plot_args,
+                    kwargs);
+
+  Py_DECREF(kwargs);
+  Py_DECREF(xlist);
+  Py_DECREF(ylist);
+  Py_DECREF(plot_args);
+  if (res)
+  {
+    Py_DECREF(res);
+  }
+
+  return res;
+}
+
+// -----------------------------------------------------------------------------
+bool labelPlot(
+    const std::string& name,
+    const Eigen::Ref<const MatrixX>& y,
+    const std::string& format)
+{
+  Eigen::Matrix<real_t, Eigen::Dynamic, 1> x(y.size());
+  for (int i=0; i < x.size(); ++i)
+  {
+    x(i) = i;
+  }
+  return labelPlot(name, x, y, format);
+
+}
+
+// -----------------------------------------------------------------------------
+bool plot(
+    const Eigen::Ref<const MatrixX>& y,
+    const std::string& format)
+{
+  Eigen::Matrix<real_t, Eigen::Dynamic, 1> x(y.size());
+  for (int i=0; i < x.size(); ++i)
+  {
+    x(i) = i;
+  }
+  return plot(x,y,format);
+}
+
+// -----------------------------------------------------------------------------
+bool plot(
+    const std::vector<real_t>& y,
+    const std::string& format)
+{
+  Eigen::Map<const Eigen::Matrix<real_t, Eigen::Dynamic, 1>> y_e(y.data(),
+                                                                    y.size());
+  return plot(y_e, format);
+}
+
+// -----------------------------------------------------------------------------
+bool plot(
+    const std::vector<real_t>& x,
+    const std::vector<real_t>& y,
+    const std::map<std::string, std::string>& keywords)
+{
+  Eigen::Map<const Eigen::Matrix<real_t, Eigen::Dynamic, 1>> x_e(x.data(),
+                                                                    x.size());
+  Eigen::Map<const Eigen::Matrix<real_t, Eigen::Dynamic, 1>> y_e(y.data(),
+                                                                    y.size());
+  return plot(x_e, y_e, keywords);
+}
+
+// -----------------------------------------------------------------------------
+bool plot(
+    const std::vector<real_t>& x,
+    const Eigen::Ref<const MatrixX>& y,
+    const std::map<std::string, std::string>& keywords)
+{
+  Eigen::Map<const Eigen::Matrix<real_t, Eigen::Dynamic, 1>> x_e(x.data(),
+                                                                    x.size());
+  return plot(x_e, y, keywords);
+}
+
+// -----------------------------------------------------------------------------
+bool plot(
+    const std::vector<real_t>& x,
+    const std::vector<real_t>& y,
+    const std::string& s)
+{
+  Eigen::Map<const Eigen::Matrix<real_t, Eigen::Dynamic, 1>> x_e(x.data(),
+                                                                    x.size());
+  Eigen::Map<const Eigen::Matrix<real_t, Eigen::Dynamic, 1>> y_e(y.data(),
+                                                                    y.size());
+  return plot(x_e, y_e, s);
+}
+
+// -----------------------------------------------------------------------------
+bool plot(
+    const std::vector<real_t>& x,
+    const Eigen::Ref<const MatrixX>& y,
+    const std::string& s)
+{
+  Eigen::Map<const Eigen::Matrix<real_t, Eigen::Dynamic, 1>> x_e(x.data(),
+                                                                    x.size());
+  return plot(x_e, y, s);
+}
+
+// -----------------------------------------------------------------------------
+bool labelPlot(
+    const std::string& name,
+    const std::vector<real_t>& x,
+    const std::vector<real_t>& y,
+    const std::string& format)
+{
+  Eigen::Map<const Eigen::Matrix<real_t, Eigen::Dynamic, 1>> x_e(x.data(),
+                                                                    x.size());
+  Eigen::Map<const Eigen::Matrix<real_t, Eigen::Dynamic, 1>> y_e(y.data(),
+                                                                    y.size());
+  return labelPlot(name, x_e, y_e, format);
+}
+
+// -----------------------------------------------------------------------------
+bool labelPlot(
+    const std::string& name,
+    const std::vector<real_t>& x,
+    const Eigen::Ref<const MatrixX>& y,
+    const std::string& format)
+{
+  Eigen::Map<const Eigen::Matrix<real_t, Eigen::Dynamic, 1>> x_e(x.data(),
+                                                                    x.size());
+  return labelPlot(name, x_e, y, format);
+}
+
+// -----------------------------------------------------------------------------
+void legend()
+{
+  PyObject* res = PyObject_CallObject(
+                    detail::_interpreter::get().s_python_function_legend,
+                    detail::_interpreter::get().s_python_empty_tuple);
+
+  if (!res)
+  {
+    throw std::runtime_error("Call to legend() failed.");
+  }
+
+  Py_DECREF(res);
+}
+
+// -----------------------------------------------------------------------------
+void ylim(real_t ymin, real_t ymax)
+{
+  PyObject* list = PyList_New(2);
+  PyList_SetItem(list, 0, PyFloat_FromDouble(ymin));
+  PyList_SetItem(list, 1, PyFloat_FromDouble(ymax));
+
+  PyObject* args = PyTuple_New(1);
+  PyTuple_SetItem(args, 0, list);
+
+  PyObject* res = PyObject_CallObject(
+                    detail::_interpreter::get().s_python_function_ylim,
+                    args);
+  if (!res)
+  {
+    throw std::runtime_error("Call to ylim() failed.");
+  }
+
+  Py_DECREF(list);
+  Py_DECREF(args);
+  Py_DECREF(res);
+}
+
+// -----------------------------------------------------------------------------
+void xlim(real_t xmin, real_t xmax)
+{
+  PyObject* list = PyList_New(2);
+  PyList_SetItem(list, 0, PyFloat_FromDouble(xmin));
+  PyList_SetItem(list, 1, PyFloat_FromDouble(xmax));
+
+  PyObject* args = PyTuple_New(1);
+  PyTuple_SetItem(args, 0, list);
+
+  PyObject* res = PyObject_CallObject(
+                    detail::_interpreter::get().s_python_function_xlim,
+                    args);
+  if (!res)
+  {
+    throw std::runtime_error("Call to xlim() failed.");
+  }
+
+  Py_DECREF(list);
+  Py_DECREF(args);
+  Py_DECREF(res);
+}
+
+// -----------------------------------------------------------------------------
+void title(const std::string &titlestr)
+{
+  PyObject* pytitlestr = PyBytes_FromString(titlestr.c_str());
+  PyObject* args = PyTuple_New(1);
+  PyTuple_SetItem(args, 0, pytitlestr);
+
+  PyObject* res = PyObject_CallObject(
+                    detail::_interpreter::get().s_python_function_title,
+                    args);
+  if (!res)
+  {
+    throw std::runtime_error("Call to title() failed.");
+  }
+
+  // if PyDeCRFF, the function doesn't work on Mac OS
+}
+
+// -----------------------------------------------------------------------------
+void axis(const std::string &axisstr)
+{
+  PyObject* str = PyBytes_FromString(axisstr.c_str());
+  PyObject* args = PyTuple_New(1);
+  PyTuple_SetItem(args, 0, str);
+
+  PyObject* res = PyObject_CallObject(
+                    detail::_interpreter::get().s_python_function_axis,
+                    args);
+  if (!res)
+  {
+    throw std::runtime_error("Call to title() failed.");
+  }
+
+  // if PyDeCRFF, the function doesn't work on Mac OS
+}
+
+// -----------------------------------------------------------------------------
+void xlabel(const std::string &str)
+{
+  PyObject* pystr = PyBytes_FromString(str.c_str());
+  PyObject* args = PyTuple_New(1);
+  PyTuple_SetItem(args, 0, pystr);
+
+  PyObject* res = PyObject_CallObject(
+                    detail::_interpreter::get().s_python_function_xlabel,
+                    args);
+  if (!res)
+  {
+    throw std::runtime_error("Call to xlabel() failed.");
+  }
+
+  // if PyDeCRFF, the function doesn't work on Mac OS
+}
+
+// -----------------------------------------------------------------------------
+void ylabel(const std::string &str)
+{
+  PyObject* pystr = PyBytes_FromString(str.c_str());
+  PyObject* args = PyTuple_New(1);
+  PyTuple_SetItem(args, 0, pystr);
+
+  PyObject* res = PyObject_CallObject(
+                    detail::_interpreter::get().s_python_function_ylabel,
+                    args);
+  if (!res)
+  {
+    throw std::runtime_error("Call to ylabel() failed.");
+  }
+
+  // if PyDeCRFF, the function doesn't work on Mac OS
+}
+
+// -----------------------------------------------------------------------------
+void grid(bool flag)
+{
+  PyObject* pyflag = flag ? Py_True : Py_False;
+
+  PyObject* args = PyTuple_New(1);
+  PyTuple_SetItem(args, 0, pyflag);
+
+  PyObject* res = PyObject_CallObject(
+                    detail::_interpreter::get().s_python_function_grid,
+                    args);
+  if (!res)
+  {
+    throw std::runtime_error("Call to grid() failed.");
+  }
+
+  // if PyDeCRFF, the function doesn't work on Mac OS
+}
+
+// -----------------------------------------------------------------------------
+void show(bool block)
+{
+  PyObject* pyflag = block ? Py_True : Py_False;
+
+  PyObject* kwargs = PyDict_New();
+
+  PyDict_SetItemString(kwargs, "block", pyflag);
+
+  PyObject* res = PyObject_Call(
+                    detail::_interpreter::get().s_python_function_show,
+                    detail::_interpreter::get().s_python_empty_tuple,
+                    kwargs);
+  if (!res)
+  {
+    throw std::runtime_error("Call to show() failed.");
+  }
+
+  Py_DECREF(res);
+  Py_DECREF(kwargs);
+}
+
+// -----------------------------------------------------------------------------
+void save(const std::string& filename)
+{
+  PyObject* pyfilename = PyBytes_FromString(filename.c_str());
+
+  PyObject* args = PyTuple_New(1);
+  PyTuple_SetItem(args, 0, pyfilename);
+
+  PyObject* res = PyObject_CallObject(
+                    detail::_interpreter::get().s_python_function_save,
+                    args);
+  if (!res)
+  {
+    throw std::runtime_error("Call to save() failed.");
+  }
+
+  Py_DECREF(pyfilename);
+  Py_DECREF(args);
+  Py_DECREF(res);
+}
+
+} // namespace plt
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_matplotlib/src/mpl_example_node.cpp b/RWR/src/ze_oss/ze_matplotlib/src/mpl_example_node.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..189ba14a64ac8e03138d53b399c4a3807f68be41
--- /dev/null
+++ b/RWR/src/ze_oss/ze_matplotlib/src/mpl_example_node.cpp
@@ -0,0 +1,100 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include <ze/matplotlib/matplotlibcpp.hpp>
+
+int main()
+{
+  using namespace ze;
+
+  // Simple:
+  std::vector<real_t> v({1, 2, 3, 4});
+  plt::plot(v);
+  plt::show();
+
+  // Eigen Vector Types:
+  VectorX times(100);
+  times.setLinSpaced(100, 0, 20);
+  VectorX points(100);
+  points.setRandom();
+
+  plt::plot(times, points);
+  plt::show();
+
+  plt::labelPlot("A Name", times, points);
+  plt::show();
+
+  // enable interactive mode as of now (only tests if it doesn't crash)
+  plt::ion();
+
+  // subplots
+  plt::subplot(3, 1, 1);
+  plt::plot(v);
+  plt::subplot(3, 1, 2);
+  plt::plot(v);
+  plt::subplot(3, 1, 3);
+  plt::plot(v);
+  plt::show(false);
+
+  plt::figure();
+
+  // plot multiple curves in a single graph
+  std::vector<real_t> w({4, 3, 2, 1});
+  plt::plot(v, "x");
+  plt::plot(w, "o");
+  plt::show();
+
+  // Histogram
+  plt::hist(points, 3);
+  plt::show();
+
+  // Row vectors
+  MatrixX matrix(2, 100);
+  matrix.setRandom();
+  plt::plot(matrix.row(0), matrix.row(1));
+  plt::show();
+
+  // BoxPlot
+  MatrixX data(2, 100);
+  data.setRandom();
+  plt::figure();
+  std::vector<std::string> labels = {"A", "B"};
+  plt::boxplot(data, labels);
+  plt::show();
+
+
+  // BoxPlot
+  data.setRandom();
+  plt::figure();
+  plt::boxplot(data, {"A", "B"});
+  plt::show();
+
+  // Boxplot unlabelled
+  data.setRandom();
+  plt::figure();
+  plt::boxplot(data);
+  plt::show();
+
+}
diff --git a/RWR/src/ze_oss/ze_pangolin/CMakeLists.txt b/RWR/src/ze_oss/ze_pangolin/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..21ae8a2796e7df79d4ac4c6e4216797c554d5e9c
--- /dev/null
+++ b/RWR/src/ze_oss/ze_pangolin/CMakeLists.txt
@@ -0,0 +1,43 @@
+project(ze_pangolin)
+cmake_minimum_required(VERSION 2.8.3)
+
+find_package(catkin_simple REQUIRED)
+catkin_simple(ALL_DEPS_REQUIRED)
+
+include(ze_setup)
+
+#############
+# LIBRARIES #
+#############
+set(HEADERS
+  include/ze/pangolin/pangolin.hpp
+  include/ze/pangolin/type_watches.hpp
+  include/ze/pangolin/pangolin_insight.hpp
+  )
+
+set(SOURCES
+    src/pangolin.cpp
+  )
+
+cs_add_library(${PROJECT_NAME} ${SOURCES} ${HEADERS})
+
+find_library(RT_LIBRARY NAMES rt librt)
+target_link_libraries(${PROJECT_NAME} ${RT_LIBRARY})
+
+###############
+# EXECUTABLES #
+###############
+cs_add_executable(ze_pangolin_test_node src/pangolin_test_node.cpp)
+target_link_libraries(ze_pangolin_test_node ${PROJECT_NAME})
+
+##########
+# GTESTS #
+##########
+catkin_add_gtest(test_type_watches test/test_type_watches.cpp)
+target_link_libraries(test_type_watches ${PROJECT_NAME})
+
+##########
+# EXPORT #
+##########
+cs_install()
+cs_export()
diff --git a/RWR/src/ze_oss/ze_pangolin/README.md b/RWR/src/ze_oss/ze_pangolin/README.md
new file mode 100644
index 0000000000000000000000000000000000000000..386135f3adc30c44fecd0606d49a8a1c4eadcdb1
--- /dev/null
+++ b/RWR/src/ze_oss/ze_pangolin/README.md
@@ -0,0 +1,52 @@
+# Pangolin Wrappers for Debugging purposes
+
+**Warning:**
+Do not use the state watchers and pangolin singleton in a production build as it might result in unexpected behaviour.
+The tools are designed for the purpose of debugging private and internal states in a quick and easy way.
+The pangolin singleton is thread-safe but is untested in shared-library environments which might result in unexpected behaviour.
+
+## Sample Usage
+
+Add a (temporary) package dependency on `ze_pangolin` and include the pangolin header in the file where you want to watch
+an expression: `#include <ze/pangolin/pangolin_insight.hpp>`.
+
+### Watch a class member
+Given a simple class:
+```
+class Data
+{
+private:
+  int a;
+  double b;
+  uint c;
+}
+```
+
+We might be interested in the internal states `a`, `b` or `c` and how they evolve over the curse of time or iterations.
+To visualize the members in a plot every time the member is modified, simply wrap them in a `PANGOLIN_WATCH(TYPE, CLASS_MEMBER_NAME)` statement:
+```
+class Data
+{
+private:
+  PANGOLIN_WATCH(int, a);
+  PANGOLIN_WATCH(double, b);
+  PANGLIN_WATCH(uint, c);
+}
+```
+
+The current type watches support all common, numeric primitive types, but **no** Eigen-types.
+
+### Watch a temporary expression
+As opposed to a class member watch that actively tracks changes of the member one might want to explicitly push updates to the plots.
+The expression watch `PANGOLIN_WATCH_EXPR(VALUE, TYPE, PLOT_NAME)` supports all types that `PANGOLIN_WATCH` accepts and Eigen-vector-types.
+
+```
+Vector3 tmp = Vector3::Random();
+PANGOLIN_WATCH_EXPR(tmp, Vector3, The_Temporary);
+```
+
+The `PLOT_NAME` has to be a unique name used as identifier for a given plot. Any data provided with the same identifier will end up in the same plot.
+
+## Known Issues:
+
+* If a class member is constructor initialized, the plot-name will not be set properly but is attributed a default value. Tracking multiple class-members might currently result in mixed plots. A solution would be to attribute random plot-identifiers instead of a static value (currently `Default`).
diff --git a/RWR/src/ze_oss/ze_pangolin/include/ze/pangolin/pangolin.hpp b/RWR/src/ze_oss/ze_pangolin/include/ze/pangolin/pangolin.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..439b4c09ac7525f7ca0926e0c21deeb9dbbf2e12
--- /dev/null
+++ b/RWR/src/ze_oss/ze_pangolin/include/ze/pangolin/pangolin.hpp
@@ -0,0 +1,112 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#pragma once
+
+#include <memory>
+#include <atomic>
+#include <mutex>
+#include <thread>
+#include <ze/common/types.hpp>
+#include <ze/common/logging.hpp>
+#include <pangolin/pangolin.h>
+#include <ze/common/noncopyable.hpp>
+
+namespace ze {
+
+//! A class to continuously plot a series of measurements into a window.
+//! This implementation is not intended for production use but debugging only.
+//! The primary reason being that it might not be guaranteed a singleton in
+//! a shared-library setup. But it is a thread-safe implementation on multi-core
+//! systems.
+//! See http://preshing.com/20130930/double-checked-locking-is-fixed-in-cpp11/
+//! for details on thread-safe and lock-free singletons.
+class PangolinPlotter : Noncopyable
+{
+public:
+  ~PangolinPlotter();
+
+  //! A threaded visualization loop.
+  void loop();
+
+  template<typename Scalar>
+  void log(const std::string& identifier, Scalar value)
+  {
+    getLoggerOrCreate(identifier)->Log(value);
+  }
+
+  // Specializations for eigen vector types.
+  void log(const std::string& identifier, VectorX value)
+  {
+    std::vector<float> values;
+    values.resize(value.size());
+    for (int i = 0; i < value.size(); ++i)
+    {
+      values[i] = value[i];
+    }
+    getLoggerOrCreate(identifier)->Log(values);
+  }
+
+  //! Singleton accessor.
+  static PangolinPlotter& instance(
+      const std::string& window_title = "",
+      int width = 640,
+      int height = 480);
+
+private:
+  //! The window title, also used as window context.
+  const std::string window_title_;
+  const int width_;
+  const int height_;
+
+  //! Temporary information to notify the visualizing loop thread that
+  //! a new logger/plotter should be created.
+  std::string new_logger_identifier_;
+  bool add_logger_ = false;
+  std::mutex add_logger_mutex_;
+
+  //! Maps from identifiers to plotters and data-logs.
+  std::map<std::string, std::shared_ptr<pangolin::Plotter>> plotters_;
+  std::map<std::string, std::shared_ptr<pangolin::DataLog>> data_logs_;
+
+  std::unique_ptr<std::thread> thread_;
+  std::mutex stop_mutex_;
+  bool stop_requested_ = false;
+
+  void requestStop();
+  bool isStopRequested();
+  const uint thread_sleep_ms_ = 40;
+
+  //! Creates a new pangolin window
+  PangolinPlotter(const std::string& window_title = "",
+                  int width = 640,
+                  int height = 480);
+
+  //! Geta logger for a given identifier or create.
+  std::shared_ptr<pangolin::DataLog>& getLoggerOrCreate(
+      const std::string& new_logger_identifier_);
+};
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_pangolin/include/ze/pangolin/pangolin_insight.hpp b/RWR/src/ze_oss/ze_pangolin/include/ze/pangolin/pangolin_insight.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..ea39fe0f0ff5131901f6079ac5837bf0164cfefd
--- /dev/null
+++ b/RWR/src/ze_oss/ze_pangolin/include/ze/pangolin/pangolin_insight.hpp
@@ -0,0 +1,50 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#pragma once
+
+#include <ze/pangolin/pangolin.hpp>
+#include <ze/pangolin/type_watches.hpp>
+
+//! Wrapping the type watches in new names for potential later extension.
+//! Currently the type wrappers have the logging callbacks integrated to avoid
+//! duplicating all operators on the child type.
+
+namespace ze {
+namespace internal {
+
+template<typename Scalar>
+using PangolinInsightWatch = PrimitiveTypeWrapperImpl<Scalar>;
+
+}
+}
+
+//! Watch a class member variable.
+#define PANGOLIN_WATCH(TYPE, MEMBER_NAME)                             \
+  ze::internal::PangolinInsightWatch<TYPE> MEMBER_NAME =              \
+    ze::internal::PangolinInsightWatch<TYPE>(#MEMBER_NAME)
+
+#define PANGOLIN_WATCH_EXPR(VALUE, TYPE, NAME)                        \
+  ze::PangolinPlotter::instance().log<TYPE>(#NAME, VALUE)
diff --git a/RWR/src/ze_oss/ze_pangolin/include/ze/pangolin/type_watches.hpp b/RWR/src/ze_oss/ze_pangolin/include/ze/pangolin/type_watches.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..35d107c603e524d9515a1155c502840bc39f04b0
--- /dev/null
+++ b/RWR/src/ze_oss/ze_pangolin/include/ze/pangolin/type_watches.hpp
@@ -0,0 +1,182 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#pragma once
+
+#include <ze/pangolin/pangolin.hpp>
+
+namespace ze {
+
+//! A change event callback triggered, whenever the value of the wrapped scalar
+//! is changed.
+template<typename Scalar>
+using ChangeEventCallback = std::function<void(Scalar)>;
+
+//! A wrapper for primitive typically numeric types that calls a callback and
+//! notifies a pangolin plotter about changes.
+//! A pangolin context watching the primitive is only created if the watch
+//! is createad with a name or from a value.
+template<class T>
+class PrimitiveTypeWrapperImpl {
+public:
+  using value_type = T;
+
+  PrimitiveTypeWrapperImpl()
+    : member_name_("Default")
+  {
+  }
+
+  PrimitiveTypeWrapperImpl(T v)
+    : value_(v)
+    , member_name_("Default")
+  {
+    initialize();
+  }
+
+  PrimitiveTypeWrapperImpl(T v, bool watch)
+    : PrimitiveTypeWrapperImpl(v)
+  {
+    if (watch)
+    {
+      initialize();
+    }
+  }
+
+  PrimitiveTypeWrapperImpl(const PrimitiveTypeWrapperImpl<T>& rhs)
+    : PrimitiveTypeWrapperImpl(rhs.value_)
+  {
+  }
+
+  //! A string constructor to name the context / window of pangolin.
+  PrimitiveTypeWrapperImpl(const std::string& member_name)
+    : member_name_(member_name)
+  {
+    initialize();
+  }
+
+  operator T() const {return value_;}
+
+  void change_callback(T value)
+  {
+    ze::PangolinPlotter::instance().log(member_name_, value);
+  }
+
+  // Modifiers
+  PrimitiveTypeWrapperImpl& operator=(T v) { value_=v; notify(); return *this; }
+  PrimitiveTypeWrapperImpl& operator+=(T v) { value_+=v; notify(); return *this; }
+  PrimitiveTypeWrapperImpl& operator-=(T v) { value_-=v; notify(); return *this; }
+  PrimitiveTypeWrapperImpl& operator*=(T v) { value_*=v; notify(); return *this; }
+  PrimitiveTypeWrapperImpl& operator/=(T v) { value_/=v; notify(); return *this; }
+  PrimitiveTypeWrapperImpl& operator%=(T v) { value_%=v; notify(); return *this; }
+  PrimitiveTypeWrapperImpl& operator++() { ++value_; notify(); return *this; }
+  PrimitiveTypeWrapperImpl& operator--() { --value_; notify(); return *this; }
+  PrimitiveTypeWrapperImpl operator++(int) { notify(); return PrimitiveTypeWrapperImpl(value_++, false); }
+  PrimitiveTypeWrapperImpl operator--(int) { notify(); return PrimitiveTypeWrapperImpl(value_--, false); }
+  PrimitiveTypeWrapperImpl& operator&=(T v) { value_&=v; notify(); return *this; }
+  PrimitiveTypeWrapperImpl& operator|=(T v) { value_|=v; notify(); return *this; }
+  PrimitiveTypeWrapperImpl& operator^=(T v) { value_^=v; notify(); return *this; }
+  PrimitiveTypeWrapperImpl& operator<<=(T v) { value_<<=v; notify(); return *this; }
+  PrimitiveTypeWrapperImpl& operator>>=(T v) { value_>>=v; notify(); return *this; }
+  //accessors
+  PrimitiveTypeWrapperImpl operator+() const { return PrimitiveTypeWrapperImpl(+value_, false); }
+  PrimitiveTypeWrapperImpl operator-() const { return PrimitiveTypeWrapperImpl(-value_, false); }
+  PrimitiveTypeWrapperImpl operator!() const { return PrimitiveTypeWrapperImpl(!value_, false); }
+  PrimitiveTypeWrapperImpl operator~() const { return PrimitiveTypeWrapperImpl(~value_, false); }
+  //friends
+  friend PrimitiveTypeWrapperImpl operator+(PrimitiveTypeWrapperImpl iw, PrimitiveTypeWrapperImpl v) { return iw+=v; }
+  friend PrimitiveTypeWrapperImpl operator+(PrimitiveTypeWrapperImpl iw, T v) { return iw+=v; }
+  friend PrimitiveTypeWrapperImpl operator+(T v, PrimitiveTypeWrapperImpl iw) { return PrimitiveTypeWrapperImpl(v, false)+=iw; }
+  friend PrimitiveTypeWrapperImpl operator-(PrimitiveTypeWrapperImpl iw, PrimitiveTypeWrapperImpl v) { return iw-=v; }
+  friend PrimitiveTypeWrapperImpl operator-(PrimitiveTypeWrapperImpl iw, T v) { return iw-=v; }
+  friend PrimitiveTypeWrapperImpl operator-(T v, PrimitiveTypeWrapperImpl iw) { return PrimitiveTypeWrapperImpl(v, false)-=iw; }
+  friend PrimitiveTypeWrapperImpl operator*(PrimitiveTypeWrapperImpl iw, PrimitiveTypeWrapperImpl v) { return iw*=v; }
+  friend PrimitiveTypeWrapperImpl operator*(PrimitiveTypeWrapperImpl iw, T v) { return iw*=v; }
+  friend PrimitiveTypeWrapperImpl operator*(T v, PrimitiveTypeWrapperImpl iw) { return PrimitiveTypeWrapperImpl(v, false)*=iw; }
+  friend PrimitiveTypeWrapperImpl operator/(PrimitiveTypeWrapperImpl iw, PrimitiveTypeWrapperImpl v) { return iw/=v; }
+  friend PrimitiveTypeWrapperImpl operator/(PrimitiveTypeWrapperImpl iw, T v) { return iw/=v; }
+  friend PrimitiveTypeWrapperImpl operator/(T v, PrimitiveTypeWrapperImpl iw) { return PrimitiveTypeWrapperImpl(v, false)/=iw; }
+  friend PrimitiveTypeWrapperImpl operator%(PrimitiveTypeWrapperImpl iw, PrimitiveTypeWrapperImpl v) { return iw%=v; }
+  friend PrimitiveTypeWrapperImpl operator%(PrimitiveTypeWrapperImpl iw, T v) { return iw%=v; }
+  friend PrimitiveTypeWrapperImpl operator%(T v, PrimitiveTypeWrapperImpl iw) { return PrimitiveTypeWrapperImpl(v, false)%=iw; }
+  friend PrimitiveTypeWrapperImpl operator&(PrimitiveTypeWrapperImpl iw, PrimitiveTypeWrapperImpl v) { return iw&=v; }
+  friend PrimitiveTypeWrapperImpl operator&(PrimitiveTypeWrapperImpl iw, T v) { return iw&=v; }
+  friend PrimitiveTypeWrapperImpl operator&(T v, PrimitiveTypeWrapperImpl iw) { return PrimitiveTypeWrapperImpl(v, false)&=iw; }
+  friend PrimitiveTypeWrapperImpl operator|(PrimitiveTypeWrapperImpl iw, PrimitiveTypeWrapperImpl v) { return iw|=v; }
+  friend PrimitiveTypeWrapperImpl operator|(PrimitiveTypeWrapperImpl iw, T v) { return iw|=v; }
+  friend PrimitiveTypeWrapperImpl operator|(T v, PrimitiveTypeWrapperImpl iw) { return PrimitiveTypeWrapperImpl(v, false)|=iw; }
+  friend PrimitiveTypeWrapperImpl operator^(PrimitiveTypeWrapperImpl iw, PrimitiveTypeWrapperImpl v) { return iw^=v; }
+  friend PrimitiveTypeWrapperImpl operator^(PrimitiveTypeWrapperImpl iw, T v) { return iw^=v; }
+  friend PrimitiveTypeWrapperImpl operator^(T v, PrimitiveTypeWrapperImpl iw) { return PrimitiveTypeWrapperImpl(v, false)^=iw; }
+  friend PrimitiveTypeWrapperImpl operator<<(PrimitiveTypeWrapperImpl iw, PrimitiveTypeWrapperImpl v) { return iw<<=v; }
+  friend PrimitiveTypeWrapperImpl operator<<(PrimitiveTypeWrapperImpl iw, T v) { return iw<<=v; }
+  friend PrimitiveTypeWrapperImpl operator<<(T v, PrimitiveTypeWrapperImpl iw) { return PrimitiveTypeWrapperImpl(v, false)<<=iw;}
+  friend PrimitiveTypeWrapperImpl operator>>(PrimitiveTypeWrapperImpl iw, PrimitiveTypeWrapperImpl v) { return iw>>=v; }
+  friend PrimitiveTypeWrapperImpl operator>>(PrimitiveTypeWrapperImpl iw, T v) { return iw>>=v; }
+  friend PrimitiveTypeWrapperImpl operator>>(T v, PrimitiveTypeWrapperImpl iw) { return PrimitiveTypeWrapperImpl(v, false)>>=iw; }
+
+  //! Set the callback to trigger on change.
+  void setChangeCallback(ChangeEventCallback<T> change_cb)
+  {
+    change_cb_ = change_cb;
+  }
+
+private:
+  //! The value wrapped by the object.
+  T value_;
+  //! The identifier / variable name wrapped in the object and also used for
+  //! identification of the plot wrt. to pangolin.
+  const std::string member_name_;
+
+  //! A callback triggered when the primitive changes.
+  ChangeEventCallback<T> change_cb_;
+
+  //! Notifiy helper function that calls the callback if defined.
+  void notify()
+  {
+    if (change_cb_)
+    {
+      change_cb_(value_);
+    }
+  }
+
+  //! Initialize the pangolin logging callbacks on change.
+  void initialize()
+  {
+    this->setChangeCallback(std::bind(&PrimitiveTypeWrapperImpl<T>::change_callback,
+                                      this,
+                                      std::placeholders::_1));
+  }
+};
+
+typedef PrimitiveTypeWrapperImpl<int> intWrapper;
+typedef PrimitiveTypeWrapperImpl<unsigned> uintWrapper;
+typedef PrimitiveTypeWrapperImpl<short> shortWrapper;
+typedef PrimitiveTypeWrapperImpl<char> charWrapper;
+typedef PrimitiveTypeWrapperImpl<unsigned long long> ullWrapper;
+typedef PrimitiveTypeWrapperImpl<float> floatWrapper;
+typedef PrimitiveTypeWrapperImpl<double> doubleWrapper;
+typedef PrimitiveTypeWrapperImpl<long double> longDoubleWrapper;
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_pangolin/package.xml b/RWR/src/ze_oss/ze_pangolin/package.xml
new file mode 100644
index 0000000000000000000000000000000000000000..a4ed509cbdcf4950a5224fd2138b6ed2a478f76f
--- /dev/null
+++ b/RWR/src/ze_oss/ze_pangolin/package.xml
@@ -0,0 +1,21 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>ze_pangolin</name>
+  <version>0.0.1</version>
+  <description>
+    ze_pangolin
+  </description>
+  <maintainer email="luc.oth@zurich-eye.com">Luc Oth</maintainer>
+  <license>ZE</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <buildtool_depend>catkin_simple</buildtool_depend>
+
+  <depend>gflags_catkin</depend>
+  <depend>glog_catkin</depend>
+  <depend>ze_cmake</depend>
+  <depend>ze_common</depend>
+  <depend>pangolin_catkin</depend>
+
+  <test_depend>gtest</test_depend>
+</package>
diff --git a/RWR/src/ze_oss/ze_pangolin/src/pangolin.cpp b/RWR/src/ze_oss/ze_pangolin/src/pangolin.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..1d6ef21a021d31b2f9c4ee5f0f852b0a0784c3ea
--- /dev/null
+++ b/RWR/src/ze_oss/ze_pangolin/src/pangolin.cpp
@@ -0,0 +1,133 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include <ze/pangolin/pangolin.hpp>
+
+namespace ze {
+
+PangolinPlotter::PangolinPlotter(const std::string& window_title,
+                                 int width,
+                                 int height)
+  : window_title_(window_title)
+  , width_(width)
+  , height_(height)
+{
+  // Thread out a pangolin loop.
+  thread_.reset(new std::thread(&PangolinPlotter::loop, this));
+}
+
+PangolinPlotter::~PangolinPlotter()
+{
+  requestStop();
+  thread_->join();
+}
+
+PangolinPlotter& PangolinPlotter::instance(
+    const std::string& window_title,
+    int width,
+    int height)
+{
+  // Although this does not appear to be thread safe, it is as of cpp11.
+  static PangolinPlotter instance(window_title, width, height);
+  return instance;
+}
+
+void PangolinPlotter::loop()
+{
+  // Create OpenGL window and switch to context.
+  pangolin::CreateWindowAndBind(window_title_, width_, height_);
+
+  while(!isStopRequested())
+  {
+    // If the addition of a logger was requested, process the request.
+    if (add_logger_)
+    {
+      data_logs_[new_logger_identifier_] = std::make_shared<pangolin::DataLog>();
+
+      // Set the labels before adding the data_log to the plotter
+      std::vector<std::string> labels = {new_logger_identifier_};
+      data_logs_.at(new_logger_identifier_)->SetLabels(labels);
+
+      plotters_[new_logger_identifier_] = std::make_shared<pangolin::Plotter>(
+                                data_logs_.at(new_logger_identifier_).get());
+      // plotter.SetBounds(0.0, 1.0, 0.0, 1.0);
+      plotters_[new_logger_identifier_]->Track("$i");
+
+      // Add the new plotter to the pangolin window.
+      pangolin::Display("multi")
+          .SetLayout(pangolin::LayoutEqual)
+          .AddDisplay(*plotters_[new_logger_identifier_]);
+
+      VLOG(3) << "Add plotter display for: " << new_logger_identifier_;
+
+      add_logger_ = false;
+    }
+
+    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
+    // Swap frames and Process Events
+    pangolin::FinishFrame();
+
+    std::this_thread::sleep_for(std::chrono::milliseconds(thread_sleep_ms_));
+  }
+}
+
+std::shared_ptr<pangolin::DataLog>& PangolinPlotter::getLoggerOrCreate(
+    const std::string& identifier)
+{
+  // As the visualization runs in a separate thread, the gl context is not accessible
+  // from within this function. We request the thread to add a new plotter and
+  // wait for it to be ready.
+  if (data_logs_.find(identifier) == data_logs_.end())
+  {
+    std::lock_guard<std::mutex> lock(add_logger_mutex_);
+
+    add_logger_ = true;
+    new_logger_identifier_ = identifier;
+
+    while(add_logger_)
+    {
+      std::this_thread::sleep_for(std::chrono::milliseconds(thread_sleep_ms_));
+    }
+    CHECK(data_logs_.find(identifier) != data_logs_.end())
+        << "A pangolin plotter (" << identifier << ") that was requested to be added and confirmed"
+           "does actually not exist.";
+  }
+
+  return data_logs_.at(identifier);
+}
+
+void PangolinPlotter::requestStop()
+{
+  std::lock_guard<std::mutex> lock(stop_mutex_);
+  stop_requested_ = true;
+}
+
+bool PangolinPlotter::isStopRequested()
+{
+   std::lock_guard<std::mutex> lock(stop_mutex_);
+   return stop_requested_;
+}
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_pangolin/src/pangolin_test_node.cpp b/RWR/src/ze_oss/ze_pangolin/src/pangolin_test_node.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..1a07165e52375a7aa658ed4febdde100a8066d30
--- /dev/null
+++ b/RWR/src/ze_oss/ze_pangolin/src/pangolin_test_node.cpp
@@ -0,0 +1,69 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include <ze/pangolin/pangolin.hpp>
+#include <ze/common/types.hpp>
+#include <ze/pangolin/type_watches.hpp>
+#include <ze/pangolin/pangolin_insight.hpp>
+
+class Sample
+{
+public:
+  void increment() { ++value; --value_another; }
+private:
+  //int value;
+  PANGOLIN_WATCH(int, value);
+  PANGOLIN_WATCH(int, value_another);
+};
+
+int main( int /*argc*/, char* argv[] )
+{
+//  PANGOLIN_WATCH(int, test);
+
+//  int x = 2;
+
+//  test = x;
+
+//  while(true)
+//  {
+//    test += 0.3;
+//    sleep(1u);
+//  }
+
+  std::shared_ptr<Sample> sample(std::make_shared<Sample>());
+  ze::Vector3 vector; vector.setRandom();
+
+  int i = 15;
+  while(true)
+  {
+    sample->increment();
+    PANGOLIN_WATCH_EXPR(i++, double, T);
+    PANGOLIN_WATCH_EXPR(vector, ze::Vector3, vector);
+    vector.setRandom();
+    sleep(1u);
+  }
+
+  return 0;
+}
diff --git a/RWR/src/ze_oss/ze_pangolin/test/test_type_watches.cpp b/RWR/src/ze_oss/ze_pangolin/test/test_type_watches.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..629ca4c73d41a1dc9090ea4bacbee216a6e78f13
--- /dev/null
+++ b/RWR/src/ze_oss/ze_pangolin/test/test_type_watches.cpp
@@ -0,0 +1,77 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include <iostream>
+#include <string>
+
+#include <ze/common/test_entrypoint.hpp>
+#include <ze/pangolin/type_watches.hpp>
+
+#define ASSERT_OPERATOR(TYPE, LHS, RHS, OP)       \
+{                                                 \
+  PrimitiveTypeWrapperImpl<TYPE> wrappedLhs(LHS, false); \
+  PrimitiveTypeWrapperImpl<TYPE> wrappedRhs(RHS, false); \
+  TYPE plainLhs = LHS;                            \
+  TYPE plainRhs = RHS;                            \
+  TYPE result = plainLhs OP plainRhs;             \
+  EXPECT_EQ(wrappedLhs OP wrappedRhs, result);    \
+  EXPECT_EQ(wrappedLhs OP plainRhs, result);      \
+  EXPECT_EQ(plainLhs OP wrappedRhs, result);      \
+}
+
+#define ASSERT_OPERATOR_TYPE_INT(TYPE)      \
+  ASSERT_OPERATOR(TYPE, 10, 5, +);        \
+  ASSERT_OPERATOR(TYPE, 10, 5, -);        \
+  ASSERT_OPERATOR(TYPE, 10, 5, *);        \
+  ASSERT_OPERATOR(TYPE, 10, 5, /);        \
+  ASSERT_OPERATOR(TYPE, 10, 5, %);        \
+  ASSERT_OPERATOR(TYPE, 5, 10, %);        \
+  ASSERT_OPERATOR(TYPE, 10, 5, &);        \
+  ASSERT_OPERATOR(TYPE, 10, 5, |);        \
+  ASSERT_OPERATOR(TYPE, 10, 5, ^);        \
+  ASSERT_OPERATOR(TYPE, 10, 5, |);
+
+#define ASSERT_OPERATOR_TYPE_DOUBLE(TYPE)      \
+  ASSERT_OPERATOR(TYPE, 10, 5, +);           \
+  ASSERT_OPERATOR(TYPE, 10, 5, -);           \
+  ASSERT_OPERATOR(TYPE, 10, 5, *);           \
+  ASSERT_OPERATOR(TYPE, 10, 5, /);
+
+TEST(TypeWatchesTest, testAllTypes)
+{
+  using namespace ze;
+
+  ASSERT_OPERATOR_TYPE_DOUBLE(double);
+  ASSERT_OPERATOR_TYPE_DOUBLE(float);
+  ASSERT_OPERATOR_TYPE_DOUBLE(long double);
+
+  ASSERT_OPERATOR_TYPE_INT(int);
+  ASSERT_OPERATOR_TYPE_INT(unsigned);
+  ASSERT_OPERATOR_TYPE_INT(short);
+  ASSERT_OPERATOR_TYPE_INT(char);
+  ASSERT_OPERATOR_TYPE_INT(unsigned long long);
+}
+
+ZE_UNITTEST_ENTRYPOINT
diff --git a/RWR/src/ze_oss/ze_ros/CMakeLists.txt b/RWR/src/ze_oss/ze_ros/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..3d7421eef82dbbb6f12fd05d8e2be68cf66ca27f
--- /dev/null
+++ b/RWR/src/ze_oss/ze_ros/CMakeLists.txt
@@ -0,0 +1,36 @@
+project(ze_ros)
+cmake_minimum_required(VERSION 2.8.3)
+
+find_package(catkin_simple REQUIRED)
+catkin_simple(ALL_DEPS_REQUIRED)
+
+include(ze_setup)
+
+#############
+# LIBRARIES #
+#############
+set(HEADERS
+  include/ze/ros/rosbag_image_query.hpp
+  include/ze/ros/pose_msg_bridge.hpp
+  include/ze/ros/tf_bridge.hpp
+  )
+
+set(SOURCES
+  src/rosbag_image_query.cpp
+  src/pose_msg_bridge.cpp
+  src/tf_bridge.cpp
+  )
+
+cs_add_library(${PROJECT_NAME} ${SOURCES} ${HEADERS})
+
+##########
+# GTESTS #
+##########
+catkin_add_gtest(test_rosbag_image_query test/test_rosbag_image_query.cpp)
+target_link_libraries(test_rosbag_image_query ${PROJECT_NAME})
+
+##########
+# EXPORT #
+##########
+cs_install()
+cs_export()
diff --git a/RWR/src/ze_oss/ze_ros/include/ze/ros/pose_msg_bridge.hpp b/RWR/src/ze_oss/ze_ros/include/ze/ros/pose_msg_bridge.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..32084a95257e86a7b074236299c90e57128a32cc
--- /dev/null
+++ b/RWR/src/ze_oss/ze_ros/include/ze/ros/pose_msg_bridge.hpp
@@ -0,0 +1,40 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#pragma once
+
+#include <geometry_msgs/PoseStamped.h>
+#include <ze/common/transformation.hpp>
+
+namespace ze {
+
+Transformation poseMsgTotransformation(
+    const geometry_msgs::PoseStamped& pose_msg);
+
+geometry_msgs::PoseStamped transformationToPoseStampedMsg(
+    const Transformation& T, int64_t stamp);
+
+
+} // ze namespace
diff --git a/RWR/src/ze_oss/ze_ros/include/ze/ros/rosbag_image_query.hpp b/RWR/src/ze_oss/ze_ros/include/ze/ros/rosbag_image_query.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..7dfd02a4f7eed93703c5375436e2dedccfbb8f8c
--- /dev/null
+++ b/RWR/src/ze_oss/ze_ros/include/ze/ros/rosbag_image_query.hpp
@@ -0,0 +1,37 @@
+// RPG-BSD License, Titus Cieslewski
+// Heavily modified by ZE
+// Copyright (C) ETH Zurich, Wyss Zurich, Zurich Eye - All Rights Reserved
+
+#pragma once
+
+#include <string>
+#include <vector>
+#include <rosbag/bag.h>
+
+#include <imp/bridge/ros/ros_bridge.hpp>
+#include <imp/core/image.hpp>
+#include <ze/common/macros.hpp>
+
+namespace ze {
+
+class RosbagImageQuery
+{
+public:
+  ZE_POINTER_TYPEDEFS(RosbagImageQuery);
+
+  RosbagImageQuery() = default;
+  RosbagImageQuery(const std::string& bagfile_path);
+  ~RosbagImageQuery() = default;
+
+  bool loadRosbag(const std::string& bagfile_path);
+
+  StampedImage getStampedImageAtTime(
+      const std::string& img_topic,
+      const int64_t stamp_ns,
+      const real_t search_range_ms = 10.0);
+
+private:
+  rosbag::Bag bag_;
+};
+
+}  // namespace ze
diff --git a/RWR/src/ze_oss/ze_ros/include/ze/ros/tf_bridge.hpp b/RWR/src/ze_oss/ze_ros/include/ze/ros/tf_bridge.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..34f896b5ae3a5f0aee76528def0f33500697a25b
--- /dev/null
+++ b/RWR/src/ze_oss/ze_ros/include/ze/ros/tf_bridge.hpp
@@ -0,0 +1,35 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#pragma once
+
+#include <tf/tf.h>
+#include <ze/common/transformation.hpp>
+
+namespace ze {
+
+tf::Transform transformationToTF(const Transformation& ze_T);
+
+} // ze namespace
diff --git a/RWR/src/ze_oss/ze_ros/package.xml b/RWR/src/ze_oss/ze_ros/package.xml
new file mode 100644
index 0000000000000000000000000000000000000000..47597be584bdcdd2f29dba114dfce915e75c490b
--- /dev/null
+++ b/RWR/src/ze_oss/ze_ros/package.xml
@@ -0,0 +1,23 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>ze_ros</name>
+  <version>0.1.4</version>
+  <description>
+    ROS utilities.
+  </description>
+  <maintainer email="matia.pizzoli@WyssZurich.ch">Matia Pizzoli</maintainer>
+  <license>ZE</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <buildtool_depend>catkin_simple</buildtool_depend>
+
+  <depend>glog_catkin</depend>
+  <depend>imp_bridge_ros</depend>
+  <depend>imp_core</depend>
+  <depend>rosbag</depend>
+  <depend>roscpp</depend>
+  <depend>tf</depend>
+  <depend>ze_cmake</depend>
+  <depend>ze_common</depend>
+
+</package>
diff --git a/RWR/src/ze_oss/ze_ros/src/pose_msg_bridge.cpp b/RWR/src/ze_oss/ze_ros/src/pose_msg_bridge.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..8b2eb25c64d9a9af6d9084db2f5d559774a5a766
--- /dev/null
+++ b/RWR/src/ze_oss/ze_ros/src/pose_msg_bridge.cpp
@@ -0,0 +1,63 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include <ze/ros/pose_msg_bridge.hpp>
+
+namespace ze {
+
+Transformation poseMsgTotransformation(
+    const geometry_msgs::PoseStamped& pose_msg)
+{
+  Vector3 p(
+        pose_msg.pose.position.x,
+        pose_msg.pose.position.y,
+        pose_msg.pose.position.z);
+  Quaternion q(
+        pose_msg.pose.orientation.w,
+        pose_msg.pose.orientation.x,
+        pose_msg.pose.orientation.y,
+        pose_msg.pose.orientation.z);
+  return Transformation(p, q);
+}
+
+geometry_msgs::PoseStamped transformationToPoseStampedMsg(
+    const Transformation& T, int64_t stamp)
+{
+  geometry_msgs::PoseStamped m;
+  m.header.stamp = ros::Time().fromNSec(stamp);
+  Vector3 p = T.getPosition();
+  m.pose.position.x = p.x();
+  m.pose.position.y = p.y();
+  m.pose.position.z = p.z();
+  Quaternion q = T.getRotation();
+  m.pose.orientation.x = q.x();
+  m.pose.orientation.y = q.y();
+  m.pose.orientation.z = q.z();
+  m.pose.orientation.w = q.w();
+  return m;
+}
+
+
+} // ze namespace
diff --git a/RWR/src/ze_oss/ze_ros/src/rosbag_image_query.cpp b/RWR/src/ze_oss/ze_ros/src/rosbag_image_query.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..523c216c03f9e165c2088d7b57eaa30de3320e22
--- /dev/null
+++ b/RWR/src/ze_oss/ze_ros/src/rosbag_image_query.cpp
@@ -0,0 +1,83 @@
+// RPG-BSD License, Titus Cieslewski
+// Heavily modified by ZE
+// Copyright (C) ETH Zurich, Wyss Zurich, Zurich Eye - All Rights Reserved
+
+#include <ze/ros/rosbag_image_query.hpp>
+
+#include <glog/logging.h>
+#include <rosbag/view.h>
+#include <ze/common/file_utils.hpp>
+#include <ze/common/time_conversions.hpp>
+
+namespace ze {
+
+RosbagImageQuery::RosbagImageQuery(const std::string& bagfile_path)
+{
+  CHECK(loadRosbag(bagfile_path));
+}
+
+bool RosbagImageQuery::loadRosbag(const std::string& bagfile_path)
+{
+  CHECK(fileExists(bagfile_path)) << "File does not exist: " << bagfile_path;
+  try
+  {
+    VLOG(1) << "Opening rosbag " << bagfile_path << " ...";
+    bag_.open(bagfile_path, rosbag::BagMode::Read);
+  }
+  catch (rosbag::BagException& exception)
+  {
+    LOG(ERROR) << "Could not open rosbag: " << bagfile_path << ": " << exception.what();
+    return false;
+  }
+  return true;
+}
+
+StampedImage RosbagImageQuery::getStampedImageAtTime(
+    const std::string& img_topic,
+    const int64_t stamp_ns,
+    const real_t search_range_ms)
+{
+  // Considering rounding errors.
+  const int64_t search_range_ns = millisecToNanosec(search_range_ms);
+  ros::Time time_min, time_max;
+  time_min.fromNSec(stamp_ns - search_range_ns);
+  time_max.fromNSec(stamp_ns + search_range_ns);
+  rosbag::View view(bag_, rosbag::TopicQuery(img_topic), time_min, time_max);
+
+  VLOG(100) << "Found messages that fit = " << view.size();
+  int64_t best_time_diff = std::numeric_limits<int64_t>::max();
+  sensor_msgs::ImageConstPtr best_match_message;
+  for (const rosbag::MessageInstance& message : view)
+  {
+    const int64_t time_diff =
+        std::abs(static_cast<int64_t>(message.getTime().toNSec()) - stamp_ns);
+    if (time_diff < best_time_diff)
+    {
+      best_time_diff = time_diff;
+      best_match_message = message.instantiate<sensor_msgs::Image>();
+      CHECK(best_match_message);
+    }
+    else
+    {
+      break; // Already passed relevant time.
+    }
+  }
+
+  // Extract image
+  int64_t best_match_stamp = -1;
+  ImageBase::Ptr best_match_img;
+  if (best_match_message)
+  {
+    best_match_stamp = best_match_message->header.stamp.toNSec();
+    best_match_img = toImageCpu(*best_match_message);
+  }
+  else
+  {
+    LOG(WARNING) << "No image found in bag with this timestamp. If this "
+                 << "problem is persistent, you may need to re-index the bag: "
+                 << "rosrun ze_rosbag_tools bagrestamper.py -i dataset.bag -o dataset_new.bag";
+  }
+  return std::make_pair(best_match_stamp, best_match_img);
+}
+
+}  // namespace ze
diff --git a/RWR/src/ze_oss/ze_ros/src/tf_bridge.cpp b/RWR/src/ze_oss/ze_ros/src/tf_bridge.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..3099f8ae9a18b88ee5433b0fce5f2d44b4e20bcd
--- /dev/null
+++ b/RWR/src/ze_oss/ze_ros/src/tf_bridge.cpp
@@ -0,0 +1,47 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include <ze/ros/tf_bridge.hpp>
+
+namespace ze {
+
+tf::Transform transformationToTF(const Transformation& ze_T)
+{
+  tf::Transform tf_T;
+  tf_T.setOrigin(
+        tf::Vector3(
+          ze_T.getPosition().x(),
+          ze_T.getPosition().y(),
+          ze_T.getPosition().z()));
+  tf_T.setRotation(
+        tf::Quaternion(
+          ze_T.getRotation().x(),
+          ze_T.getRotation().y(),
+          ze_T.getRotation().z(),
+          ze_T.getRotation().w()));
+  return tf_T;
+}
+
+} // ze namespace
diff --git a/RWR/src/ze_oss/ze_ros/test/test_rosbag_image_query.cpp b/RWR/src/ze_oss/ze_ros/test/test_rosbag_image_query.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..42fa6b3c658360a7155735893ed4c8f2249181a8
--- /dev/null
+++ b/RWR/src/ze_oss/ze_ros/test/test_rosbag_image_query.cpp
@@ -0,0 +1,61 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include <string>
+#include <iostream>
+
+#include <ze/common/test_entrypoint.hpp>
+#include <ze/common/test_utils.hpp>
+#include <ze/common/time_conversions.hpp>
+#include <ze/common/path_utils.hpp>
+#include <ze/ros/rosbag_image_query.hpp>
+#include <imp/core/image_base.hpp>
+
+TEST(RosbagImageQueryTests, testImageQuery)
+{
+  using namespace ze;
+
+  std::string data_dir = getTestDataDir("rosbag_euroc_snippet");
+  std::string bag_filename = joinPath(data_dir, "dataset.bag");
+  RosbagImageQuery rosbag(bag_filename);
+
+  {
+    // stamp of the first image in the bag:
+    int64_t nsec = nanosecFromSecAndNanosec(1403636617, 863555500);
+    auto res = rosbag.getStampedImageAtTime("/cam0/image_raw", nsec);
+    EXPECT_EQ(res.first, nsec);
+    EXPECT_TRUE(res.second != nullptr);
+  }
+
+  {
+    // stamp of some image in the bag:
+    int64_t nsec = nanosecFromSecAndNanosec(1403636618, 463555500);
+    auto res = rosbag.getStampedImageAtTime("/cam0/image_raw", nsec);
+    EXPECT_EQ(res.first, nsec);
+    EXPECT_TRUE(res.second != nullptr);
+  }
+}
+
+ZE_UNITTEST_ENTRYPOINT
diff --git a/RWR/src/ze_oss/ze_splines/CMakeLists.txt b/RWR/src/ze_oss/ze_splines/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..47383ff8ab967969c30fbbe1c3f47c1c0a928716
--- /dev/null
+++ b/RWR/src/ze_oss/ze_splines/CMakeLists.txt
@@ -0,0 +1,53 @@
+project(ze_splines)
+cmake_minimum_required(VERSION 2.8.3)
+
+find_package(catkin_simple REQUIRED)
+find_package(OpenCV REQUIRED)
+catkin_simple(ALL_DEPS_REQUIRED)
+
+include(ze_setup)
+
+# The matplotlib-cpp headers are linked against
+find_package(PythonLibs)
+include_directories(${PYTHON_INCLUDE_DIRS})
+
+#############
+# LIBRARIES #
+#############
+set(HEADERS
+    include/ze/splines/bspline.hpp
+    include/ze/splines/bspline_pose_minimal.hpp
+    include/ze/splines/operators.hpp
+    include/ze/splines/rotation_vector.hpp
+    include/ze/splines/viz_splines.hpp
+    )
+
+set(SOURCES
+    src/bspline.cpp
+    src/bspline_pose_minimal.cpp
+    src/viz_splines.cpp
+    )
+
+cs_add_library(${PROJECT_NAME} ${SOURCES} ${HEADERS})
+target_link_libraries(${PROJECT_NAME} ${PYTHON_LIBRARIES} ${OpenCV_LIBRARIES})
+
+###############
+# EXECUTABLES #
+###############
+cs_add_executable(viz_splines_test src/viz_splines_test.cpp)
+target_link_libraries(viz_splines_test ${PROJECT_NAME} ${PYTHON_LIBRARIES})
+
+##########
+# GTESTS #
+##########
+catkin_add_gtest(test_bspline test/test_bspline.cpp)
+target_link_libraries(test_bspline ${PROJECT_NAME} ${OpenCV_LIBRARIES})
+
+catkin_add_gtest(test_bspline_pose_minimal test/test_bspline_pose_minimal.cpp)
+target_link_libraries(test_bspline_pose_minimal ${PROJECT_NAME} ${PYTHON_LIBRARIES} ${OpenCV_LIBRARIES})
+
+##########
+# EXPORT #
+##########
+cs_install()
+cs_export()
diff --git a/RWR/src/ze_oss/ze_splines/LICENSE b/RWR/src/ze_oss/ze_splines/LICENSE
new file mode 100644
index 0000000000000000000000000000000000000000..0f7377879a2fca2207bf7b28b577a8b77c83689f
--- /dev/null
+++ b/RWR/src/ze_oss/ze_splines/LICENSE
@@ -0,0 +1,72 @@
+Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+Partially derived from Kalibr:
+
+Copyright (c) 2014, Paul Furgale, Jérôme Maye and Jörn Rehder, Autonomous Systems Lab, ETH Zurich, Switzerland
+Copyright (c) 2014, Thomas Schneider, Skybotix AG, Switzerland
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
+
+Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
+
+Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution.
+
+All advertising materials mentioning features or use of this software must display the following acknowledgement: This product includes software developed by the Autonomous Systems Lab and Skybotix AG.
+
+Neither the name of the Autonomous Systems Lab and Skybotix AG nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE AUTONOMOUS SYSTEMS LAB AND SKYBOTIX AG ''AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL the AUTONOMOUS SYSTEMS LAB OR SKYBOTIX AG BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+Partially derived from Schweizer-Messer:
+
+Copyright (c) 2011-2013, Paul Furgale and others.
+All rights reserved.
+
+Unlike otherwise stated in source files, the code in this repository is
+published under the Revised BSD (New BSD) license.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the <organization> nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
\ No newline at end of file
diff --git a/RWR/src/ze_oss/ze_splines/doc/State Estimation for Robotics.pdf b/RWR/src/ze_oss/ze_splines/doc/State Estimation for Robotics.pdf
new file mode 100644
index 0000000000000000000000000000000000000000..23661480573612acaa2b81cfc0af188381c48545
Binary files /dev/null and b/RWR/src/ze_oss/ze_splines/doc/State Estimation for Robotics.pdf differ
diff --git a/RWR/src/ze_oss/ze_splines/include/ze/splines/bspline.hpp b/RWR/src/ze_oss/ze_splines/include/ze/splines/bspline.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..b63ea341d51fabeece33bfd5d24ab4427ee37bb6
--- /dev/null
+++ b/RWR/src/ze_oss/ze_splines/include/ze/splines/bspline.hpp
@@ -0,0 +1,764 @@
+// Copyright (c) 2014, Paul Furgale, Jérôme Maye and Jörn Rehder, Autonomous Systems Lab, ETH Zurich, Switzerland
+// Copyright (c) 2014, Thomas Schneider, Skybotix AG, Switzerland
+// Copyright (c) 2016, Luc Oth
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without modification, are permitted
+// provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice, this list of
+// conditions and the following disclaimer.
+//
+// * Redistributions in binary form must reproduce the above copyright notice, this list of
+// conditions and the following disclaimer in the documentation and/or other materials
+// provided with the distribution.
+//
+// * All advertising materials mentioning features or use of this software must display the
+// following acknowledgement: This product includes software developed by the
+// Autonomous Systems Lab and Skybotix AG.
+//
+// * Neither the name of the Autonomous Systems Lab and Skybotix AG nor the names of its
+// contributors may be used to endorse or promote products derived from this software
+// without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE AUTONOMOUS SYSTEMS LAB AND SKYBOTIX AG ''AS IS'' AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
+// OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL
+// the AUTONOMOUS SYSTEMS LAB OR SKYBOTIX AG BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
+// OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+// HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+// OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Derived from https://github.com/ethz-asl/kalibr/ (2016)
+//
+// Changes wrt. original:
+//  _ remove dependency on Schweizer-Messer toolbox
+//  _ remove dependency on sparse_block_matrix (drop support for sparse
+//    initialization)
+//
+// WARNING: Do NOT use as-is in production code.
+// @todo The code itself was initially designed purely for research and before
+//       actually using it in production code, we should properly verify its
+//       capabilities. One major part is the efficiency which isn't perfect at
+//       all (pure Eigen::Dynamic implementations, a bunch of duplicate
+//       evaluations for readability and some debuggng stuff)
+
+#pragma once
+
+#include <vector>
+#include <Eigen/Core>
+#include <ze/common/logging.hpp>
+#include <ze/common/types.hpp>
+
+namespace ze_splines {
+
+// Define a generic expression of a matrix where all coefficients
+// are defined by a functor.
+// A BiVector is a vector that holds the values of cumulative basis functions
+// and extends the definition to the outside of its definition:
+// if before the segment where it is defined: 0
+// after the segment: fixed to end_value
+class BiVector;
+}  // namespace ze_splines
+
+namespace Eigen {
+namespace internal {
+template<>
+
+struct functor_traits<ze_splines::BiVector>
+{
+  enum { Cost = 1, PacketAccess = false, IsRepeatable = true };
+};
+}  // namespace internal
+}  // namespace Eigen
+
+namespace ze_splines {
+
+class BiVector {
+  enum { Cost = 1, PacketAccess = false, IsRepeatable = true };
+private:
+  const int startIndex_;
+  const ze::real_t endValue_;
+  const ze::VectorX localBi_;
+
+public:
+  BiVector(int startIndex, const ze::VectorX& localBi, ze::real_t endValue)
+    : startIndex_(startIndex)
+    , endValue_(endValue)
+    , localBi_(localBi)
+  {};
+
+  ze::real_t operator() (int i, int j = 0) const {
+    i -= startIndex_;
+    if(i < 0)
+    {
+      return endValue_;
+    }
+    if(i >= (localBi_.rows()))
+    {
+      return 0;
+    }
+    return  i >= 0 ? localBi_(i): 0;
+  }
+};
+
+} // namespace ze_splines
+
+namespace ze {
+
+/**
+ * @class BSpline
+ *
+ * A class to facilitate state estimation for vechicles in 3D space using B-Splines
+ *
+ */
+class BSpline
+{
+public:
+
+  /**
+   * Create a spline of the specified order. The resulting B-spline will
+   * be a series of piecewise polynomials of degree splineOrder - 1.
+   *
+   * @param splineOrder The order of the spline.
+   */
+  BSpline(int spline_order);
+
+  /**
+   * A destructor.
+   *
+   */
+  ~BSpline();
+
+  /**
+   *
+   * @return The order of the spline
+   */
+  int spline_order() const;
+
+  /**
+   *
+   * @return The degree of polynomial used by the spline.
+   */
+  int polynomialDegree() const;
+
+  /**
+   *
+   * @return the minimum number of knots required to have at least one valid
+   * time segment.
+   */
+  int minimumKnotsRequired() const;
+
+  /**
+   *
+   * @return the dimension of the bspline (only valid after initialization)
+   */
+  int dimension() const;
+
+  /**
+   * @param numTimeSegments the number of time segments required
+   * @return the number of coefficients required for a specified number of
+   * valid time segments.
+   */
+  int numCoefficientsRequired(int num_time_segments) const;
+
+  /**
+   * @param numTimeSegments the number of time segments required
+   * @return the number of knots required for a specified number of valid
+   * time segments.
+   */
+  int numKnotsRequired(int num_time_segments) const;
+
+  /**
+   *
+   * @param numKnots the number of knots.
+   * @return the number of valid time segments for a given number of knots.
+   */
+  int numValidTimeSegments(int num_knots) const;
+
+  /**
+   *
+   * @return the number of valid time segments for a given for the current
+   * knot sequence.
+   */
+  int numValidTimeSegments() const;
+
+  /**
+   * Return the basis matrix active on the \f$i^{\textup{th}}\f$ time segment.
+   *
+   * @param i The index of the time segment.
+   *
+   * @return The basis matrix active on the time segment.
+   */
+  const MatrixX& basisMatrix(int i) const;
+
+  /**
+   *
+   * @return the time interval that the spline is well-defined on
+   * [t_min(), t_max()]
+   */
+  std::pair<real_t, real_t> timeInterval() const;
+
+  /**
+   * Return the time interval of a single spline segment.
+   *
+   * @param i The index of the time segment
+   *
+   * @return the time interval of the ith spline segment.
+   */
+  std::pair<real_t, real_t> timeInterval(int i) const;
+
+  /**
+   * Set the knots and coefficients of the spline. Each column of the
+   * coefficient matrix
+   * is interpreted as a single, vector-valued spline coefficient.
+   *
+   * @param knots        A non-decreasing knot sequence.
+   * @param coefficients A set of spline coefficients.
+   */
+  void setKnotsAndCoefficients(const std::vector<real_t>& knots,
+                               const MatrixX& coefficients);
+
+  /**
+   * Set the knots and coefficients of the spline. Each column of the
+   * coefficient matrix is interpreted as a single, vector-valued spline
+   * coefficient.
+   *
+   * @param knots        A non-decreasing knot sequence.
+   * @param coefficients A set of spline coefficients.
+   */
+  void setKnotVectorAndCoefficients(const VectorX& knots,
+                                    const MatrixX& coefficients);
+
+  /**
+   * Sets the coefficient matrix from the stacked vector of coefficients.
+   *
+   * @param coefficients The stacked vector of coefficients.
+   */
+  void setCoefficientVector(const VectorX& coefficients);
+
+  /**
+   *
+   * @return The stacked vector of coefficients.
+   */
+  VectorX coefficientVector();
+
+  /**
+   * Sets the matrix of coefficients.
+   *
+   * @param coefficients
+   */
+  void setCoefficientMatrix(const MatrixX& coefficients);
+
+  /**
+   * @return the current knot vector.
+   */
+  const std::vector<real_t> knots() const;
+
+  /**
+   * @return the current knot vector.
+   */
+  VectorX knotVector() const;
+
+  /**
+   * @return the current spline coefficient matrix. Each column of
+   * the coefficient matrix is interpreted as a single, vector-valued
+   * spline coefficient.
+   */
+  const MatrixX& coefficients() const;
+
+  /**
+   *
+   * @return The number of total coefficients the spline currently uses
+   */
+  int numCoefficients() const;
+
+  /**
+   * This is equivalent to spline.coefficients().cols()
+   *
+   * @return The number of vector-valued coefficient columns the spline
+   * currently uses
+   */
+  int numVvCoefficients() const;
+
+  /**
+   *
+   * @return The minimum time that the spline is well-defined on.
+   */
+  real_t t_min() const;
+
+  /**
+   *
+   * @return The maximum time that the spline is well-defined on. Because B-splines
+   *         are defined on half-open intervals, the spline curve is well defined up
+   *         to but not including this time.
+   */
+  real_t t_max() const;
+
+  /**
+   * Evaluate the spline curve at the time t.
+   *
+   * @param t The time to evaluate the spline curve
+   *
+   * @return The value of the spline curve at the time t.
+   */
+  VectorX eval(real_t t) const;
+
+  /**
+   * Evaluate the derivative of the spline curve at time t.
+   *
+   * @param t The time to evaluate the spline derivative.
+   * @param derivativeOrder The order of the derivative. This must be >= 0
+   *
+   * @return The value of the derivative of the spline curve evaluated at t.
+   */
+  VectorX evalD(real_t t, int derivative_order) const;
+
+  /**
+   * Evaluate the derivative of the spline curve at time t and retrieve the Jacobian
+   * of the value with respect to small changes in the paramter vector. The Jacobian
+   * only refers to the local parameter vector. The indices of the local parameters with
+   * respect to the full paramter vector can be retrieved using localCoefficientVectorIndices().
+   *
+   * @param t The time to evaluate the spline derivative.
+   * @param derivativeOrder The order of the derivative. This must be >= 0
+   *
+   * @return The value of the derivative of the spline curve evaluated at t and the Jacobian.
+   */
+  std::pair<VectorX, MatrixX> evalDAndJacobian(real_t t,
+                                               int derivative_order) const;
+
+  /**
+   * Evaluate the derivative of the spline curve at time t and retrieve the Jacobian
+   * of the value with respect to small changes in the paramter vector. The Jacobian
+   * only refers to the local parameter vector.
+   *
+   * @param t The time to evaluate the spline derivative.
+   * @param derivativeOrder The order of the derivative. This must be >= 0
+   * @param a pointer to the Jacobian matrix to fill in
+   * @param a pointer to an int vector that will be filled with the local coefficient indices
+   *
+   * @return The value of the derivative of the spline curve evaluated at t.
+   */
+  VectorX evalDAndJacobian(real_t t,
+                           int derivative_order,
+                           MatrixX * Jacobian,
+                           VectorXi * coefficient_indices) const;
+
+   /**
+   * Get the local basis matrix evaluated at the time \f$ t \f$.
+   * For vector-valued spline coefficients of dimension \f$ D \f$
+   * and a B-spline of order $S$, this matrix will be \f$ D \times SD \f$
+   *
+   * @param t The time to evaluate the local basis matrix.
+   * @param derivativeOrder The derivative order to return (0 is no derivative)
+   *
+   * @return The local basis matrix evaluated at time \f$ t \f$
+   */
+  MatrixX Phi(real_t t, int derivative_order = 0) const;
+
+  /**
+   * Get the local basis matrix evaluated at the time \f$ t \f$.
+   * For vector-valued spline coefficients of dimension \f$ D \f$
+   * and a B-spline of order $S$, this matrix will be \f$ D \times SD \f$
+   *
+   * @param t The time to evaluate the local basis matrix.
+   * @param derivativeOrder The derivative order to return (0 is no derivative)
+   *
+   * @return The local basis matrix evaluated at time \f$ t \f$
+   */
+  MatrixX localBasisMatrix(real_t t, int derivative_order = 0) const;
+
+  /**
+   * Get the local coefficient matrix evaluated at the time \f$ t \f$.
+   * For vector-valued spline coefficients of dimension \f$ D \f$
+   * and a B-spline of order $S$, this matrix will be \f$ D \times S \f$.
+   * Each column of the resulting matrix corresponds to a single vector-valued
+   * coefficient.
+   *
+   * @param t The time being queried
+   *
+   * @return The local coefficient matrix active at time \f$ t \f$
+   */
+  MatrixX localCoefficientMatrix(real_t t) const;
+
+  /**
+   * Return a map to a single coefficient column.
+   * This allows the user to pass around what is essentially a pointer
+   * to a single column in the coefficient matrix.
+   *
+   * @param i The column of the coefficient matrix to return. \f$ 0 \le i < \f$ coefficients().cols()
+   *
+   * @return A map to column i of the coefficient matrix.
+   */
+  Eigen::Map<VectorX> vvCoefficientVector(int i);
+
+  /**
+   * Return a map to a single coefficient column.
+   * This allows the user to pass around what is essentially a pointer
+   * to a single column in the coefficient matrix.
+   *
+   * @param i The column of the coefficient matrix to return. \f$ 0 \le i < \f$ coefficients().cols()
+   *
+   * @return A map to column i of the coefficient matrix.
+   */
+  Eigen::Map<const VectorX> vvCoefficientVector(int i) const;
+
+  /**
+   * Return a map to a single coefficient column.
+   * This allows the user to pass around what is essentially a pointer
+   * to a single column in the coefficient matrix.
+   *
+   * @param i The column of the coefficient matrix to return. \f$ 0 \le i < \f$ coefficients().cols()
+   *
+   * @return A map to column i of the coefficient matrix.
+   */
+  template<int D>
+  Eigen::Map<Eigen::Matrix<real_t, D, 1> > fixedSizeVvCoefficientVector(int i)
+  {
+    CHECK_EQ(D,coefficients_.rows())
+        << "Size mismatch between requested vector size and actual vector size";
+    CHECK_LE(0, coefficients_.cols()) << "Index out of range";
+    CHECK_LE(i, coefficients_.cols()) << "Index out of range";
+
+    return Eigen::Map<Eigen::Matrix<real_t, D, 1> >(&coefficients_(0,i),coefficients_.rows());
+  }
+
+  /**
+   * Return a map to a single coefficient column.
+   * This allows the user to pass around what is essentially a pointer
+   * to a single column in the coefficient matrix.
+   *
+   * @param i The column of the coefficient matrix to return. \f$ 0 \le i < \f$ coefficients().cols()
+   *
+   * @return A map to column i of the coefficient matrix.
+   */
+  template<int D>
+  Eigen::Map<const Eigen::Matrix<real_t, D, 1> > fixedSizeVvCoefficientVector(int i) const
+  {
+    CHECK_EQ(D,coefficients_.rows())
+        << "Size mismatch between requested vector size and actual vector size";
+    CHECK_LE(0, coefficients_.cols()) << "Index out of range";
+    CHECK_LE(i, coefficients_.cols()) << "Index out of range";
+
+    return Eigen::Map< const Eigen::Matrix<real_t, D, 1> >(
+          &coefficients_(0,i), coefficients_.rows());
+  }
+
+  /**
+   * Get the local coefficient vector evaluated at the time \f$ t \f$.
+   * For vector-valued spline coefficients of dimension \f$ D \f$
+   * and a B-spline of order $S$, this vector will be \f$ SD \times 1 \f$
+   * Evaluating the B-spline at time t, eval(t,O) is equivalent to evaluating
+   * Phi(t,O) * localCoefficientVector(t)
+   *
+   * @param t The time being queried
+   *
+   * @return The local coefficient vector active at time \f$ t \f$
+   */
+  VectorX localCoefficientVector(real_t t) const;
+
+  /**
+   * Update the local coefficient vector
+   *
+   * @param t The time used to select the local coefficients.
+   * @param c The local coefficient vector.
+   */
+  void setLocalCoefficientVector(real_t t, const VectorX& c);
+
+  /**
+   * Get the indices of the local coefficients active at time t.
+   *
+   * @param t The time being queried.
+   *
+   * @return The indices of the local coefficients active at time t.
+   */
+  VectorXi localCoefficientVectorIndices(real_t t) const;
+
+  /**
+   * Get the indices of the local vector-valued coefficients active at time t.
+   *
+   * @param t The time being queried.
+   *
+   * @return The indices of the local vector-valued coefficients active at time t.
+   */
+  VectorXi localVvCoefficientVectorIndices(real_t t) const;
+
+  int coefficientVectorLength() const;
+
+  /**
+   * Initialize a spline from two times and two positions. The spline will be initialized to
+   * have one valid time segment \f$[t_0, t_1)\f$ such that \f$\mathbf b(t_0) = \mathbf p_0\f$,
+   * \f$\mathbf b(t_1) = \mathbf p_1\f$,
+   * \f$\dot{\mathbf b}(t_0) = \frac{\mathbf{p_1} - \mathbf p_0}{t_1 - t_0}\f$, and
+   * \f$\dot{\mathbf b}(t_1) = \frac{\mathbf{p_1} - \mathbf p_0}{t_1 - t_0}\f$.
+   *
+   * @param t_0 The start of the time interval.
+   * @param t_1 The end of the time interval
+   * @param p_0 The position at the start of the time interval.
+   * @param p_1 The position at the end of the time interval.
+   */
+  void initSpline(real_t t_0,
+                  real_t t_1,
+                  const VectorX& p_0,
+                  const VectorX& p_1);
+
+  //! Spline initialization version 2.
+  void initSpline2(const VectorX& times,
+                   const MatrixX& interpolation_points,
+                   int num_segments,
+                   real_t lambda);
+
+  //! Spline initialization version 3.
+  void initSpline3(const VectorX& times,
+                   const MatrixX& interpolation_points,
+                   int num_segments,
+                   real_t lambda);
+
+  /**
+   * Add a curve segment that interpolates the point p, ending at time t.
+   *
+   * If the new time corresponds with the first knot past the end of the curve,
+   * the existing curve is perfectly preserved. Otherwise, the existing curve
+   * will interpolate its current position at the current endpoint and the new
+   * position at the new endpoint but will not necessarily match the last segment
+   * exactly.
+   *
+   * @param t The time of the point to interpolate. This must be greater than t_max()
+   * @param p The point to interpolate at time t.
+   */
+  void addCurveSegment(real_t t, const VectorX& p);
+
+  /**
+   * Add a curve segment that interpolates the point p, ending at time t.
+   *
+   * If the new time corresponds with the first knot past the end of the curve,
+   * the existing curve is perfectly preserved. Otherwise, the existing curve
+   * will interpolate its current position at the current endpoint and the new
+   * position at the new endpoint but will not necessarily match the last segment
+   * exactly.
+   *
+   * @param t The time of the point to interpolate. This must be greater than t_max()
+   * @param p The point to interpolate at time t.
+   * @param lambda a smoothness parameter. Higher for more smooth.
+   */
+  void addCurveSegment2(real_t t, const VectorX& p, real_t lambda);
+
+  /**
+   * Removes a curve segment from the left by removing one knot and one coefficient vector.
+   * After calling this function, the curve will have one fewer segment. The new minimum
+   * time will be timeInterval(0).first
+   *
+   */
+  void removeCurveSegment();
+
+  /**
+   * Get the \f$ \mathbf V_i \f$ matrix associated with the integral over the segment.
+   *
+   * @param segmentIndex
+   *
+   * @return the \f$ \mathbf V_i \f$ matrix
+   */
+  MatrixX Vi(int segmentIndex) const;
+
+  VectorX evalIntegral(real_t t1, real_t t2) const;
+  inline VectorX evalI(real_t t1, real_t t2) const
+  {
+    return evalIntegral(t1, t2);
+  }
+
+  MatrixX Mi(int segment_index) const;
+  MatrixX Bij(int segment_index, int column_index) const;
+  MatrixX U(real_t t, int derivative_order) const;
+  VectorX u(real_t t, int derivative_order) const;
+  int segmentIndex(real_t t) const;
+  MatrixX Dii(int segment_index) const;
+  MatrixX Di(int segment_index) const;
+
+  /**
+   * Get the b_i(t) for i in localVvCoefficientVectorIndices
+   * (@see #localVvCoefficientVectorIndices).
+   *
+   * @param t The time being queried.
+   *
+   * @return [b_i(t) for i in localVvCoefficientVectorIndices].
+   *
+   */
+  VectorX getLocalBiVector(real_t t, int derivative_order = 0) const;
+  void getLocalBiInto(real_t t, VectorX& ret, int derivative_order = 0) const;
+
+  /**
+   * Get the cumulative (tilde) b_i(t) for i in localVvCoefficientVectorIndices
+   * (@see #localVvCoefficientVectorIndices).
+   *
+   * @param t The time being queried.
+   *
+   * @return [tilde b_i(t) for i in localVvCoefficientVectorIndices].
+   *
+   */
+  VectorX getLocalCumulativeBiVector(real_t t, int derivative_order = 0) const;
+
+  Eigen::CwiseNullaryOp<ze_splines::BiVector, VectorX>
+  getBiVector(real_t t) const
+  {
+    return Eigen::CwiseNullaryOp<ze_splines::BiVector, VectorX>(
+          numValidTimeSegments(), 1,
+          ze_splines::BiVector(segmentIndex(t), getLocalBiVector(t), 0));
+  }
+  Eigen::CwiseNullaryOp<ze_splines::BiVector, VectorX>
+  getCumulativeBiVector(real_t t) const
+  {
+    return Eigen::CwiseNullaryOp<ze_splines::BiVector, VectorX>(
+          numValidTimeSegments(), 1,
+          ze_splines::BiVector(segmentIndex(t),getLocalCumulativeBiVector(t), 1));
+  }
+
+  MatrixX segmentQuadraticIntegral(const MatrixX& W,
+                                   int segment_index,
+                                   int derivative_order) const;
+  MatrixX segmentQuadraticIntegralDiag(const VectorX& Wdiag,
+                                       int segment_index,
+                                       int derivative_order) const;
+  MatrixX curveQuadraticIntegral(const MatrixX& W,int derivative_order) const;
+  MatrixX curveQuadraticIntegralDiag(const VectorX& Wdiag, int derivative_order) const;
+
+  void initConstantSpline(real_t t_min, real_t t_max,
+                          int num_segments, const VectorX& constant);
+
+protected:
+  /**
+   * An internal function to find the segment of the knot sequence
+   * that the time t falls in. The function returns the
+   * value \f$ u = \frac{t - t_i}{t_{i+1} - t_i} \f$ and the index \f$i\f$
+   *
+   * @param t The time being queried.
+   *
+   * @return A pair with the first value \f$ u = \frac{t - t_i}{t_{i+1} - t_i} \f$
+   * and the second value the index \f$i\f$
+   */
+  std::pair<real_t,int> computeUAndTIndex(real_t t) const;
+
+  /**
+   * An internal function to find the segment of the knot sequence
+   * that the time t falls in. The function returns the width of the
+   * knot segment \f$ \Delta t_i = t_{i+1} - t_i \f$ and the index \f$i\f$
+   *
+   * @param t The time being queried.
+   *
+   * @return A pair with the first value \f$ \Delta t_i = t_{i+1} - t_i \f$
+   * and the second value the index \f$i\f$
+   */
+  std::pair<real_t,int> computeTIndex(real_t t) const;
+
+  /**
+   * Compute the vector \f$ \mathbf u(t) \f$ for a spline of
+   * order \f$ S \f$, this is an \f$ S \times 1 \f$ vector.
+   *
+   * At derivative order 0 (no derivative), this vector is
+   * \f$ \mathbf u(t) = \left [ 1 \; u(t) \; u(t)^2 \; \dots \; u(t)^{S-1} \right ]^T \f$
+   *
+   * For higher derivative order \f$ M \f$, the vector returned is
+   * \f$ \mathbf u^{(M)}(t) = \frac{\partial^{(M)}\mathbf u(t)}{\partial t^{(M)}}
+   *
+   * @param uval the value \f$ u(t) \f$
+   * @param segmentIndex
+   * @param derivativeOrder
+   *
+   * @return
+   */
+  VectorX computeU(real_t uval, int segment_index, int derivative_order) const;
+
+  int basisMatrixIndexFromStartingKnotIndex(int starting_knot_index) const;
+  int startingKnotIndexFromBasisMatrixIndex(int basis_matrix_index) const;
+  const MatrixX& basisMatrixFromKnotIndex(int knot_index) const;
+
+  /**
+   * Throws an exception if the knot sequence is not non-decreasing.
+   *
+   * @param knots The knot sequence to verify.
+   */
+  void verifyKnotSequence(const std::vector<real_t>& knots);
+
+  /**
+   * Initialize the basis matrices based on the current knot sequence.
+   * There is one basis matrix for each valid time segment defined by the spline.
+   *
+   * Implemented using the recursive basis matrix algorithm from
+   * Qin, Kaihuai, General matrix representations for B-splines,
+   * The Visual Computer (2000) 16:177–186
+   *
+   */
+  void initializeBasisMatrices();
+
+  /**
+   * The recursive function used to implement the recursive basis matrix algorithm from
+   * Qin, Kaihuai, General matrix representations for B-splines,
+   * The Visual Computer (2000) 16:177–186
+   *
+   * @param k The order of the matrix requested.
+   * @param i The time segment of the basis matrix
+   *
+   * @return
+   */
+  MatrixX M(int k, int i);
+
+  /**
+   * A helper function for producing the M matrices. Defined in
+   * Qin, Kaihuai, General matrix representations for B-splines,
+   * The Visual Computer (2000) 16:177–186
+   *
+   */
+  real_t d_0(int k, int i, int j);
+
+  /**
+   * A helper function for producing the M matrices. Defined in
+   * Qin, Kaihuai, General matrix representations for B-splines,
+   * The Visual Computer (2000) 16:177–186
+   *
+   */
+  real_t d_1(int k, int i, int j);
+
+  /// The order of the spline.
+  int spline_order_;
+
+  /// The knot sequence used by the B-spline.
+  std::vector<real_t> knots_;
+
+  /// The coefficient matrix used by the B-Spline. Each column can be seen as a
+  /// single vector-valued spline coefficient.
+  /// This is stored explicityl in column major order to ensure that each column (i.e.
+  /// a single vector-valued spline coefficient) is stored in contiguous memory. This
+  /// allows one to, for example, map a single spline coefficient using the Eigen::Map type.
+  Eigen::Matrix<real_t, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor> coefficients_;
+
+  /// The basis matrices for each time segment the B-spline is defined over.
+  std::vector<MatrixX> basis_matrices_;
+};
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_splines/include/ze/splines/bspline_pose_minimal.hpp b/RWR/src/ze_oss/ze_splines/include/ze/splines/bspline_pose_minimal.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..391d5720c2b65dbdeb24ca4ae4f0e2d6d6ccff87
--- /dev/null
+++ b/RWR/src/ze_oss/ze_splines/include/ze/splines/bspline_pose_minimal.hpp
@@ -0,0 +1,215 @@
+// Copyright (c) 2014, Paul Furgale, Jérôme Maye and Jörn Rehder, Autonomous Systems Lab, ETH Zurich, Switzerland
+// Copyright (c) 2014, Thomas Schneider, Skybotix AG, Switzerland
+// Copyright (c) 2016, Luc Oth
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without modification, are permitted
+// provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice, this list of
+// conditions and the following disclaimer.
+//
+// * Redistributions in binary form must reproduce the above copyright notice, this list of
+// conditions and the following disclaimer in the documentation and/or other materials
+// provided with the distribution.
+//
+// * All advertising materials mentioning features or use of this software must display the
+// following acknowledgement: This product includes software developed by the
+// Autonomous Systems Lab and Skybotix AG.
+//
+// * Neither the name of the Autonomous Systems Lab and Skybotix AG nor the names of its
+// contributors may be used to endorse or promote products derived from this software
+// without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE AUTONOMOUS SYSTEMS LAB AND SKYBOTIX AG ''AS IS'' AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
+// OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL
+// the AUTONOMOUS SYSTEMS LAB OR SKYBOTIX AG BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
+// OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+// HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+// OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Derived from https://github.com/ethz-asl/kalibr/ (2016)
+//
+// Adopted from https://github.com/ethz-asl/kalibr/ (2016)
+// BSD Licensed
+//
+// Changes wrt. original:
+//  _ remove dependency on Schweizer-Messer toolbox
+//  _ remove dependency on sparse_block_matrix (drop support for sparse
+//    initialization)
+//
+// WARNING: Do NOT use as-is in production code.
+
+#pragma once
+
+#include <ze/splines/bspline.hpp>
+#include <ze/common/transformation.hpp>
+#include <ze/splines/rotation_vector.hpp>
+#include <ze/splines/operators.hpp>
+#include <ze/common/macros.hpp>
+
+namespace ze {
+
+/**
+ * @class BSpline
+ *
+ * A class to facilitate state estimation for vehicles in 3D space using B-Splines
+ * The spline represents a pose with respect to some navigation frame
+ * \f$ \mathbf F_n \f$.
+ * The pose is represented as a 6d spline of a translational component and
+ * a 3d minimal rotation parametrization.
+ *
+ */
+template<class ROTATION>
+class BSplinePoseMinimal : public BSpline
+{
+ public:
+    ZE_POINTER_TYPEDEFS(BSplinePoseMinimal);
+
+    /**
+     * Create a spline of the specified order. The resulting B-spline will
+     * be a series of piecewise polynomials of degree splineOrder - 1.
+     *
+     * @param splineOrder The order of the spline.
+     */
+    BSplinePoseMinimal(int spline_order);
+
+    ~BSplinePoseMinimal();
+
+    Matrix4 transformation(real_t tk) const;
+    Matrix4 transformationAndJacobian(
+        real_t tk,
+        MatrixX* J = NULL,
+        VectorXi* coefficient_indices = NULL) const;
+
+    Matrix4 inverseTransformationAndJacobian(
+        real_t tk,
+        MatrixX* J = NULL,
+        VectorXi* coefficient_indices = NULL) const;
+
+    Matrix4 inverseTransformation(real_t tk) const;
+
+
+    Vector4 transformVectorAndJacobian(
+        real_t tk,
+        const Vector4& v,
+        MatrixX* J = NULL,
+        VectorXi* coefficient_indices = NULL) const;
+
+    Vector3 position(real_t tk) const;
+
+    Matrix3 orientation(real_t tk) const;
+    Matrix3 orientationAndJacobian(
+        real_t tk,
+        MatrixX* J,
+        VectorXi* coefficient_indices) const;
+
+    Matrix3 inverseOrientation(real_t tk) const;
+    Matrix3 inverseOrientationAndJacobian(
+        real_t tk,
+        MatrixX* J,
+        VectorXi* coefficient_indices) const;
+
+    Vector3 linearVelocity(real_t tk) const;
+    Vector3 linearVelocityBodyFrame(real_t tk) const;
+
+    Vector3 linearAcceleration(real_t tk) const;
+    Vector3 linearAccelerationBodyFrame(real_t tk) const;
+    Vector3 linearAccelerationAndJacobian(
+        real_t tk,
+        MatrixX* J,
+        VectorXi* coefficient_indices) const;
+
+    Vector3 angularVelocity(real_t tk) const;
+    Vector3 angularVelocityBodyFrame(real_t tk) const;
+    Vector3 angularVelocityBodyFrameAndJacobian(
+        real_t tk,
+        MatrixX* J,
+        VectorXi* coefficient_indices) const;
+
+    Vector3 angularVelocityAndJacobian(
+        real_t tk,
+        MatrixX* J,
+        VectorXi* coefficient_indices) const;
+
+    //! takes the two transformation matrices at two points in time
+    //! to construct a pose spline
+    void initPoseSpline(
+        real_t t0,
+        real_t t1,
+        const Matrix4& T_n_t0,
+        const Matrix4& T_n_t);
+
+    //! take the pose in the minimal parametrization to initializ the spline
+    void initPoseSpline2(
+        const VectorX& times,
+        const Eigen::Matrix<real_t, 6, Eigen::Dynamic>& poses,
+        int num_segments,
+        real_t lambda);
+
+    void initPoseSpline3(
+        const VectorX& times,
+        const Eigen::Matrix<real_t, 6, Eigen::Dynamic>& poses,
+        int num_segments,
+        real_t lambda);
+
+    //! initialize a bspline given a vector of poses
+    void initPoseSplinePoses(
+        const VectorX& times,
+        const std::vector<Matrix4>& poses,
+        int num_segments,
+        real_t lambda);
+
+    void initPoseSplinePoses(
+        const StampedTransformationVector& poses,
+        int num_segments,
+        real_t lambda);
+
+    void addPoseSegment(
+        real_t tk,
+        const Matrix4& T_n_tk);
+
+    void addPoseSegment2(
+        real_t tk,
+        const Matrix4& T_n_tk,
+        real_t lambda);
+
+    Matrix4 curveValueToTransformation(const VectorX& c) const;
+    VectorX transformationToCurveValue(const Matrix4& T) const;
+
+    Matrix4 curveValueToTransformationAndJacobian(
+        const VectorX& c,
+        MatrixX * J) const;
+};
+
+typedef BSplinePoseMinimal<ze::sm::RotationVector> BSplinePoseMinimalRotationVector;
+
+}  // namespace ze
diff --git a/RWR/src/ze_oss/ze_splines/include/ze/splines/operators.hpp b/RWR/src/ze_oss/ze_splines/include/ze/splines/operators.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..cacd94a2d65af269664ee54040f8cd5ac2267787
--- /dev/null
+++ b/RWR/src/ze_oss/ze_splines/include/ze/splines/operators.hpp
@@ -0,0 +1,99 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Copyright (c) 2011-2013, Paul Furgale and others.
+// All rights reserved.
+//
+// Unlike otherwise stated in source files, the code in this repository is
+// published under the Revised BSD (New BSD) license.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the <organization> nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#pragma once
+
+#include <vector>
+#include <Eigen/Core>
+#include <ze/common/types.hpp>
+#include <ze/common/matrix.hpp>
+
+namespace ze {
+// imported from Schweizer-Messer Kinematics. Not intended to be used outside
+// of the spline package.
+namespace sm {
+
+inline Matrix4 boxPlus(const Vector6 & dt)
+{
+  Matrix4 Bp;
+  Bp <<   0.0,  -dt[5],   dt[4],  -dt[0],
+          dt[5],     0.0,  -dt[3],  -dt[1],
+         -dt[4],   dt[3],     0.0,  -dt[2],
+          0.0,     0.0,     0.0,     0.0;
+  return Bp;
+}
+
+inline Matrix46 boxMinus(const Vector4 & p)
+{
+  Matrix46 Bm;
+  Bm <<    p[3],     0.0,     0.0,     0.0,     -p[2],     p[1],
+           0.0,    p[3],     0.0,    p[2],       0.0,    -p[0],
+           0.0,     0.0,    p[3],   -p[1],      p[0],      0.0,
+           0.0,     0.0,     0.0,     0.0,       0.0,      0.0;
+  return Bm;
+}
+
+inline Matrix6 boxTimes(Matrix4 const & T_ba)
+{
+  Matrix6 Tbp = Matrix6::Zero();
+
+  Tbp.topLeftCorner<3, 3>() = T_ba.topLeftCorner<3, 3>();
+  Tbp.bottomRightCorner<3, 3>() = T_ba.topLeftCorner<3, 3>();
+  Tbp.topRightCorner<3, 3>() = -skewSymmetric(T_ba(0, 3), T_ba(1, 3), T_ba(2, 3))
+                               * T_ba.topLeftCorner<3, 3>();
+
+  return Tbp;
+}
+
+} // namespace kinematics
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_splines/include/ze/splines/rotation_vector.hpp b/RWR/src/ze_oss/ze_splines/include/ze/splines/rotation_vector.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..3e3cbfd8a9e78f07e5ebceba3a44f4c8300abec1
--- /dev/null
+++ b/RWR/src/ze_oss/ze_splines/include/ze/splines/rotation_vector.hpp
@@ -0,0 +1,261 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Copyright (c) 2011-2013, Paul Furgale and others.
+// All rights reserved.
+//
+// Unlike otherwise stated in source files, the code in this repository is
+// published under the Revised BSD (New BSD) license.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the <organization> nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#pragma once
+
+#include <vector>
+#include <Eigen/Core>
+#include <ze/common/types.hpp>
+#include <ze/common/matrix.hpp>
+
+namespace ze {
+// not intended for use in other packages but the ze_splines.
+namespace sm {
+
+class RotationVector {
+public:
+  RotationVector(const Vector3& v): v_(v) {}
+  RotationVector(const Matrix3& C): v_(getParametersFromMatrix(C))
+  {}
+
+  const Vector3 getParameters() const
+  {
+    return v_;
+  }
+
+  Matrix3 getRotationMatrix() const
+  {
+    Matrix3 C;
+
+    real_t angle = v_.norm();
+
+    if(angle < 1e-14)
+    {
+      // The angle goes to zero.
+      C = Matrix3::Identity();
+    }
+    else
+    {
+      Vector3 axis;
+      real_t recip_angle = 1.0/angle;
+      axis = v_ * recip_angle;
+
+      real_t ax = axis[0];
+      real_t ay = axis[1];
+      real_t az = axis[2];
+      real_t sa = sin(angle);
+      real_t ca = cos(angle);
+      real_t ax2 = ax*ax;
+      real_t ay2 = ay*ay;
+      real_t az2 = az*az;
+
+      C << ax2+ca*(1.0-ax2),     ax*ay-ca*ax*ay+sa*az, ax*az-ca*ax*az-sa*ay,
+           ax*ay-ca*ax*ay-sa*az, ay2+ca*(1.0-ay2),     ay*az-ca*ay*az+sa*ax,
+           ax*az-ca*ax*az+sa*ay, ay*az-ca*ay*az-sa*ax, az2+ca*(1.0-az2);
+    }
+
+    return C;
+  }
+
+  Matrix3 toSMatrix() const
+  {
+    Matrix3 S;
+    real_t angle = v_.norm();
+
+    if(angle < 1e-14)
+    {
+      S = Matrix3::Identity();
+    }
+    else
+    {
+      real_t recip_angle = 1.0/angle;
+      Vector3 axis = v_ * recip_angle;
+      real_t st2 = sin(angle * 0.5);
+      real_t st  = sin(angle);
+
+      real_t c1 = -2.0 * st2 * st2 * recip_angle;
+      real_t c2 = (angle - st) * recip_angle;
+      Matrix3 crossA = skewSymmetric(axis);
+
+      S = Matrix3::Identity() + (c1 * crossA) + (c2 * crossA * crossA);
+    }
+
+    return S;
+  }
+
+  Vector3 angularVelocityAndJacobian(const Vector3& pdot,
+                                     Matrix36* Jacobian) const
+  {
+    Vector3 omega;
+    Matrix3 S = toSMatrix();
+
+    omega = S * pdot;
+
+    if(Jacobian)
+    {
+      *Jacobian = Matrix36::Zero();
+
+      //Jacobian->block(0,0,3,3) = Matrix3::Zero();
+      Jacobian->block(0,3,3,3) = S;
+
+      // LAZY
+      // \todo...redo when there is time and not so lazy.
+      Matrix36 & J = *Jacobian;
+      real_t t1 = v_[0];
+      real_t t2 = v_[1];
+      real_t t3 = v_[2];
+      real_t dt1 = pdot[0];
+      real_t dt2 = pdot[1];
+      real_t dt3 = pdot[2];
+      real_t t5 = t1*t1;
+      real_t t6 = t2*t2;
+      real_t t7 = t3*t3;
+      real_t t8 = t5+t6+t7;
+      real_t t9 = pow(t8,3.0/2.0);
+      real_t t10 = sqrt(t8);
+      real_t t11 = sin(t10);
+      real_t t12 = cos(t10);
+      real_t t13 = t12*(1.0/2.0);
+      real_t t14 = t13-1.0/2.0;
+      real_t t15 = 1.0/pow(t8,5.0/2.0);
+      real_t t16 = dt3*t14*t9*2.0;
+      real_t t17 = dt3*t1*t11*t2*t3*3.0;
+      real_t t18 = dt3*t3*t9;
+      real_t t19 = dt1*t1*t10*t6*2.0;
+      real_t t20 = dt2*t11*t2*t5*3.0;
+      real_t t21 = dt1*t1*t10*t12*t6;
+      real_t t22 = dt2*t14*t9*2.0;
+      real_t t23 = dt2*t1*t10*t2*t3*2.0;
+      real_t t24 = dt2*t1*t10*t12*t2*t3;
+      real_t t25 = dt1*t14*t9*2.0;
+      real_t t26 = dt1*t1*t11*t2*t3*3.0;
+      real_t t27 = dt1*t1*t9;
+      real_t t28 = dt2*t2*t9;
+      real_t t29 = dt1*t1*t10*t7*2.0;
+      real_t t30 = dt2*t10*t2*t7*2.0;
+      real_t t31 = dt3*t11*t3*t5*3.0;
+      real_t t32 = dt3*t11*t3*t6*3.0;
+      real_t t33 = dt2*t1*t11*t3*t8;
+      real_t t34 = dt2*t1*t10*t14*t3*4.0;
+      real_t t35 = dt1*t1*t10*t12*t7;
+      real_t t36 = dt2*t10*t12*t2*t7;
+      real_t t0  = t15*(t18+t19+t20+t21+t28+t29+t31+t33+t34+t35-dt1*t1*t11*t6*3.0-dt2*t10*t2*t5*2.0-dt1*t1*t11*t7*3.0-dt3*t10*t3*t5*2.0-dt2*t11*t2*t8-dt3*t11*t3*t8-dt3*t1*t10*t14*t2*4.0-dt2*t10*t12*t2*t5-dt3*t10*t12*t3*t5-dt3*t1*t11*t2*t8);
+
+      J(0,0) = t0;
+      J(0,1) = t15*(t16+t17+dt2*t1*t9-dt1*t2*t9*2.0-dt2*t1*t10*t6*2.0+dt2*t1*t11*t6*3.0-dt3*t10*t14*t6*4.0+dt1*t10*t2*t6*2.0-dt1*t11*t2*t6*3.0+dt1*t10*t2*t7*2.0-dt1*t11*t2*t7*3.0-dt2*t1*t11*t8+dt1*t11*t2*t8*2.0-dt3*t11*t6*t8-dt3*t1*t10*t2*t3*2.0+dt2*t10*t14*t2*t3*4.0-dt2*t1*t10*t12*t6+dt1*t10*t12*t2*t6+dt1*t10*t12*t2*t7+dt2*t11*t2*t3*t8-dt3*t1*t10*t12*t2*t3);
+      J(0,2) = -t15*(t22+t23+t24-dt3*t1*t9+dt1*t3*t9*2.0+dt3*t1*t10*t7*2.0-dt3*t1*t11*t7*3.0-dt2*t10*t14*t7*4.0-dt1*t10*t3*t6*2.0+dt1*t11*t3*t6*3.0+dt3*t1*t11*t8-dt1*t10*t3*t7*2.0+dt1*t11*t3*t7*3.0-dt1*t11*t3*t8*2.0-dt2*t11*t7*t8-dt2*t1*t11*t2*t3*3.0+dt3*t10*t14*t2*t3*4.0+dt3*t1*t10*t12*t7-dt1*t10*t12*t3*t6-dt1*t10*t12*t3*t7+dt3*t11*t2*t3*t8);
+      J(1,0) = -t15*(t16-t17+dt2*t1*t9*2.0-dt1*t2*t9-dt2*t1*t10*t5*2.0+dt2*t1*t11*t5*3.0-dt3*t10*t14*t5*4.0+dt1*t10*t2*t5*2.0-dt1*t11*t2*t5*3.0-dt2*t1*t10*t7*2.0+dt2*t1*t11*t7*3.0-dt2*t1*t11*t8*2.0+dt1*t11*t2*t8-dt3*t11*t5*t8+dt1*t1*t10*t14*t3*4.0+dt3*t1*t10*t2*t3*2.0-dt2*t1*t10*t12*t5+dt1*t10*t12*t2*t5-dt2*t1*t10*t12*t7+dt1*t1*t11*t3*t8+dt3*t1*t10*t12*t2*t3);
+      J(1,1) = t15*(t18-t19-t20-t21+t27+t30+t32+t36+dt1*t1*t11*t6*3.0+dt2*t10*t2*t5*2.0-dt1*t1*t11*t8-dt2*t11*t2*t7*3.0-dt3*t10*t3*t6*2.0-dt3*t11*t3*t8+dt3*t1*t10*t14*t2*4.0-dt1*t10*t14*t2*t3*4.0+dt2*t10*t12*t2*t5-dt3*t10*t12*t3*t6+dt3*t1*t11*t2*t8-dt1*t11*t2*t3*t8);
+      J(1,2) = t15*(t25+t26+dt3*t2*t9-dt2*t3*t9*2.0+dt2*t10*t3*t5*2.0-dt2*t11*t3*t5*3.0-dt1*t10*t14*t7*4.0-dt3*t10*t2*t7*2.0+dt3*t11*t2*t7*3.0+dt2*t10*t3*t7*2.0-dt2*t11*t3*t7*3.0-dt3*t11*t2*t8+dt2*t11*t3*t8*2.0-dt1*t11*t7*t8+dt3*t1*t10*t14*t3*4.0-dt1*t1*t10*t2*t3*2.0+dt2*t10*t12*t3*t5-dt3*t10*t12*t2*t7+dt2*t10*t12*t3*t7+dt3*t1*t11*t3*t8-dt1*t1*t10*t12*t2*t3);
+      J(2,0) = t15*(t22-t23-t24-dt3*t1*t9*2.0+dt1*t3*t9+dt3*t1*t10*t5*2.0-dt3*t1*t11*t5*3.0-dt2*t10*t14*t5*4.0+dt3*t1*t10*t6*2.0-dt3*t1*t11*t6*3.0-dt1*t10*t3*t5*2.0+dt1*t11*t3*t5*3.0+dt3*t1*t11*t8*2.0-dt1*t11*t3*t8-dt2*t11*t5*t8+dt1*t1*t10*t14*t2*4.0+dt2*t1*t11*t2*t3*3.0+dt3*t1*t10*t12*t5+dt3*t1*t10*t12*t6-dt1*t10*t12*t3*t5+dt1*t1*t11*t2*t8);
+      J(2,1) = -t15*(t25-t26+dt3*t2*t9*2.0-dt2*t3*t9-dt3*t10*t2*t5*2.0+dt3*t11*t2*t5*3.0-dt1*t10*t14*t6*4.0-dt3*t10*t2*t6*2.0+dt3*t11*t2*t6*3.0+dt2*t10*t3*t6*2.0-dt2*t11*t3*t6*3.0-dt3*t11*t2*t8*2.0+dt2*t11*t3*t8-dt1*t11*t6*t8+dt2*t1*t10*t14*t2*4.0+dt1*t1*t10*t2*t3*2.0-dt3*t10*t12*t2*t5-dt3*t10*t12*t2*t6+dt2*t10*t12*t3*t6+dt2*t1*t11*t2*t8+dt1*t1*t10*t12*t2*t3);
+      J(2,2) = t15*(t27+t28-t29-t30-t31-t32-t33-t34-t35-t36+dt1*t1*t11*t7*3.0+dt3*t10*t3*t5*2.0-dt1*t1*t11*t8+dt2*t11*t2*t7*3.0+dt3*t10*t3*t6*2.0-dt2*t11*t2*t8+dt1*t10*t14*t2*t3*4.0+dt3*t10*t12*t3*t5+dt3*t10*t12*t3*t6+dt1*t11*t2*t3*t8);
+    }
+
+    return omega;
+  }
+
+  Matrix3 parametersToInverseSMatrix(const Vector3 & parameters) const
+  {
+    real_t phi = std::sqrt(parameters.transpose()*parameters);
+    Matrix3 invS;
+    if(phi == 0)
+    {
+      invS = Matrix3::Identity();
+    }
+    else
+    {
+      real_t cot = - std::sin(phi)/(std::cos(phi)-1);
+      real_t a1 = 1/(phi*phi) * (1- 0.5*phi*cot);
+      invS = Matrix3::Identity()
+             + 0.5 * skewSymmetric(parameters)
+             + a1 * skewSymmetric(parameters) * skewSymmetric(parameters);
+    }
+    return invS;
+  }
+
+private:
+  Vector3 v_;
+
+  Vector3 getParametersFromMatrix(const Matrix3& C) const
+  {
+    Vector3 p;
+    // Sometimes, because of roundoff error, the value of tr ends up outside
+    // the valid range of arccos. Truncate to the valid range.
+    real_t tr = std::max(-1.0, std::min(
+                           (C(0,0) + C(1,1) + C(2,2) - 1.0) * 0.5, 1.0));
+    real_t a = acos( tr ) ;
+
+    if(fabs(a) < 1e-14)
+    {
+      return Vector3::Zero();
+    }
+
+    p[0] = (C(2,1) - C(1,2));
+    p[1] = (C(0,2) - C(2,0));
+    p[2] = (C(1,0) - C(0,1));
+    real_t n2 = p.norm();
+    if(fabs(n2) < 1e-14)
+    {
+      return Vector3::Zero();
+    }
+
+    real_t scale = -a/n2;
+    p = scale * p;
+
+    return p;
+  }
+};
+
+} // namespace kinematics
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_splines/include/ze/splines/viz_splines.hpp b/RWR/src/ze_oss/ze_splines/include/ze/splines/viz_splines.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..730a189f2e6994a23529de91207159656c8af099
--- /dev/null
+++ b/RWR/src/ze_oss/ze_splines/include/ze/splines/viz_splines.hpp
@@ -0,0 +1,86 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#pragma once
+
+#include <memory>
+
+#include <ze/common/types.hpp>
+#include <ze/splines/bspline.hpp>
+#include <ze/splines/bspline_pose_minimal.hpp>
+#include <ze/visualization/viz_common.hpp>
+
+namespace ze {
+
+// fwd
+class Visualizer;
+
+class SplinesVisualizer
+{
+public:
+  SplinesVisualizer(const std::shared_ptr<Visualizer>& viz);
+
+  //! Plot a single dimension of a spline.
+  void plotSpline(
+      const BSpline& bs,
+      const size_t dimension,
+      real_t step_size = 0.05);
+
+  //! Display a spline (max 3 dimensions) as point curve in space.
+  void displaySplineTrajectory(
+      const BSpline& bs,
+      const std::string& topic,
+      const size_t id,
+      const Color& color,
+      real_t step_size = 0.05);
+
+  //! Display a spline (max 3 dimensions) as point curve specifying the dimensions
+  //! of the spline to plot.
+  void displaySplineTrajectory(
+      const BSpline& bs,
+      const std::string& topic,
+      const size_t id,
+      const Color& color,
+      const std::vector<size_t>& draw_dimensions,
+      real_t step_size = 0.05);
+
+  //! Display a pose spline as pose-curve.
+  void displaySplineTrajectory(
+      const BSplinePoseMinimalRotationVector& bs,
+      const std::string& topic,
+      const size_t id,
+      real_t step_size = 0.05);
+
+  //! Plot a pose spline as one graph for rotational and one graph
+  //! for translational components.
+  void plotSpline(
+      const BSplinePoseMinimalRotationVector& bs,
+      real_t step_size = 0.05);
+
+private:
+  std::shared_ptr<Visualizer> viz_;
+};
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_splines/package.xml b/RWR/src/ze_oss/ze_splines/package.xml
new file mode 100644
index 0000000000000000000000000000000000000000..a3af97148c9dee27290d083ca80bb6cbf68e6499
--- /dev/null
+++ b/RWR/src/ze_oss/ze_splines/package.xml
@@ -0,0 +1,26 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>ze_splines</name>
+  <version>0.1.4</version>
+  <description>
+    Basic spline package with Pose and N-Dimensional Spline
+  </description>
+  <maintainer email="luc.oth@wysszurich.ch">Luc Oth</maintainer>
+  <license>ZE</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <buildtool_depend>catkin_simple</buildtool_depend>
+
+  <depend>glog_catkin</depend>
+  <depend>gflags_catkin</depend>
+  <depend>eigen_catkin</depend>
+  <depend>eigen_checks</depend>
+  <depend>minkindr</depend>
+  <depend>ze_cmake</depend>
+  <depend>ze_common</depend>
+  <depend>ze_visualization</depend>
+  <depend>PythonLibs</depend>
+  <depend>ze_matplotlib</depend>
+
+  <test_depend>gtest</test_depend>
+</package>
diff --git a/RWR/src/ze_oss/ze_splines/src/bspline.cpp b/RWR/src/ze_oss/ze_splines/src/bspline.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..1b7bff9b9d624456a5667dd48b64b5f0246659ca
--- /dev/null
+++ b/RWR/src/ze_oss/ze_splines/src/bspline.cpp
@@ -0,0 +1,1505 @@
+// Copyright (c) 2014, Paul Furgale, Jérôme Maye and Jörn Rehder, Autonomous Systems Lab, ETH Zurich, Switzerland
+// Copyright (c) 2014, Thomas Schneider, Skybotix AG, Switzerland
+// Copyright (c) 2016, Luc Oth
+// Copyright (C) 2016 ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Derived from https://github.com/ethz-asl/kalibr/ (2016)
+
+#include <ze/splines/bspline.hpp>
+#include <Eigen/Cholesky>
+#include <Eigen/LU>
+#include <Eigen/QR>
+#include <boost/tuple/tuple.hpp>
+
+namespace ze {
+
+BSpline::BSpline(int spline_order)
+  : spline_order_(spline_order)
+{
+  CHECK_GE(spline_order_, 2)
+      << "The B-spline order must be greater than or equal to 2";
+}
+
+BSpline::~BSpline()
+{
+}
+
+int BSpline::spline_order() const
+{
+  return spline_order_;
+}
+
+int BSpline::dimension() const
+{
+  return coefficients_.rows();
+}
+
+int BSpline::polynomialDegree() const
+{
+  return spline_order_ - 1;
+}
+
+void BSpline::setKnotsAndCoefficients(const std::vector<real_t>& knots,
+                                      const MatrixX& coefficients)
+{
+  //std::cout << "setting " << knots.size() << " knots\n";
+  // This will throw an exception if it is an invalid knot sequence.
+  verifyKnotSequence(knots);
+
+  // Check if the number of coefficients matches the number of knots.
+  CHECK_EQ(numCoefficientsRequired(numValidTimeSegments(knots.size())), coefficients.cols())
+       <<  "A B-spline of order " << spline_order_ << " requires "
+       << numCoefficientsRequired(numValidTimeSegments(knots.size()))
+       << " coefficients for the " << numValidTimeSegments(knots.size())
+       << " time segments defined by " << knots.size() << " knots";
+
+  knots_ = knots;
+  coefficients_ = coefficients;
+
+  initializeBasisMatrices();
+}
+
+void BSpline::initializeBasisMatrices()
+{
+  basis_matrices_.resize(numValidTimeSegments());
+
+  for(unsigned i = 0; i < basis_matrices_.size(); i++)
+  {
+    basis_matrices_[i] = M(spline_order_,i + spline_order_ - 1);
+  }
+}
+
+MatrixX BSpline::M(int k, int i)
+{
+  CHECK_GE(k, 1) << "The parameter k must be greater than or equal to 1";
+  // \todo: redo these checks.
+  CHECK_GE(i, 0) << "The parameter i must be greater than or equal to 0";
+  CHECK_LT(i, (int)knots_.size())
+      << "The parameter i must be less than the number of time segments";
+  if(k == 1)
+  {
+    // The base-case for recursion.
+    MatrixX M(1,1);
+    M(0,0) = 1;
+    return M;
+  }
+  else
+  {
+    MatrixX M_km1 = M(k-1,i);
+    // The recursive equation for M
+    // M_k = [ M_km1 ] A  + [  0^T  ] B
+    //       [  0^T  ]      [ M_km1 ]
+    //        -------        -------
+    //         =: M1          =: M2
+    //
+    //     = M1 A + M2 B
+    MatrixX M1 = MatrixX::Zero(M_km1.rows() + 1, M_km1.cols());
+    MatrixX M2 = MatrixX::Zero(M_km1.rows() + 1, M_km1.cols());
+
+    M1.topRightCorner(M_km1.rows(),M_km1.cols()) = M_km1;
+    M2.bottomRightCorner(M_km1.rows(),M_km1.cols()) = M_km1;
+
+    MatrixX A = MatrixX::Zero(k-1, k);
+    for(int idx = 0; idx < A.rows(); idx++)
+    {
+      int j = i - k + 2 + idx;
+      real_t d0 = d_0(k, i, j);
+      A(idx, idx  ) = 1.0 - d0;
+      A(idx, idx+1) = d0;
+    }
+
+    MatrixX B = MatrixX::Zero(k-1, k);
+    for(int idx = 0; idx < B.rows(); idx++)
+    {
+      int j = i - k + 2 + idx;
+      real_t d1 = d_1(k, i, j);
+      B(idx, idx  ) = -d1;
+      B(idx, idx+1) = d1;
+    }
+
+    MatrixX M_k;
+
+    return M_k = M1 * A + M2 * B;
+  }
+}
+
+real_t BSpline::d_0(int k, int i, int j)
+{
+  CHECK_LE(j+k-1.0, (int)knots_.size()) <<  "Index out of range with k=" << k
+                                        << ", i=" << i << ", and j=" << j;
+  CHECK_LT(0, (int)knots_.size()) <<  "Index out of range with k=" << k
+                                  << ", i=" << i << ", and j=" << j;
+  CHECK_LE(j, (int)knots_.size()) <<  "Index out of range with k=" << k
+                                  << ", i=" << i << ", and j=" << j;
+  CHECK_LE(i, (int)knots_.size()) <<  "Index out of range with k=" << k
+                                  << ", i=" << i << ", and j=" << j;
+
+  real_t denom = knots_[j+k-1] - knots_[j];
+  if(denom <= 0.0)
+  {
+    return 0.0;
+  }
+
+  real_t numerator = knots_[i] - knots_[j];
+
+  return numerator/denom;
+}
+
+real_t BSpline::d_1(int k, int i, int j)
+{
+  CHECK_LE(j+k-1.0, (int)knots_.size()) <<  "Index out of range with k="
+                                        << k << ", i=" << i << ", and j=" << j;
+  CHECK_LT(0, (int)knots_.size()) <<  "Index out of range with k="
+                                  << k << ", i=" << i << ", and j=" << j;
+  CHECK_LE(j, (int)knots_.size()) <<  "Index out of range with k="
+                                  << k << ", i=" << i << ", and j=" << j;
+  CHECK_LE(i, (int)knots_.size()) <<  "Index out of range with k="
+                                  << k << ", i=" << i << ", and j=" << j;
+  real_t denom = knots_[j+k-1] - knots_[j];
+  if(denom <= 0.0)
+  {
+    return 0.0;
+  }
+
+  real_t numerator = knots_[i+1] - knots_[i];
+
+  return numerator/denom;
+}
+
+void BSpline::setKnotVectorAndCoefficients(const VectorX& knots,
+                                           const MatrixX& coefficients)
+{
+  std::vector<real_t> k(knots.size());
+  for(unsigned i = 0; i < k.size(); i++)
+  {
+    k[i] = knots(i);
+  }
+
+  setKnotsAndCoefficients(k, coefficients);
+}
+
+const std::vector<real_t> BSpline::knots() const
+{
+  return knots_;
+}
+
+VectorX BSpline::knotVector() const
+{
+  VectorX k(knots_.size());
+  for(unsigned i = 0; i < knots_.size(); i++)
+  {
+    k(i) = knots_[i];
+  }
+
+  return k;
+}
+
+const MatrixX& BSpline::coefficients() const
+{
+  return coefficients_;
+}
+
+void BSpline::verifyKnotSequence(const std::vector<real_t>& knots)
+{
+  CHECK_GE((int)knots.size(), minimumKnotsRequired())
+      << "The sequence does not contain enough knots to define an active time sequence "
+      << "for a B-spline of order " << spline_order_
+      << ". At least " << minimumKnotsRequired()
+      << " knots are required";
+
+  for(unsigned i = 1; i < knots_.size(); i++)
+  {
+    CHECK_LE(knots[i-1], knots[i])
+        << "The knot sequence must be nondecreasing. Knot " << i
+        << " was not greater than or equal to knot " << (i-1);
+  }
+}
+
+int BSpline::numValidTimeSegments(int numKnots) const
+{
+  int nv = numKnots - 2*spline_order_ + 1;
+  return std::max(nv,0);
+}
+
+int BSpline::numValidTimeSegments() const
+{
+  return numValidTimeSegments(knots_.size());
+}
+
+int BSpline::minimumKnotsRequired() const
+{
+  return numKnotsRequired(1);
+}
+
+int BSpline::numCoefficientsRequired(int num_time_segments) const
+{
+  return num_time_segments + spline_order_ - 1;
+}
+
+int BSpline::numKnotsRequired(int num_time_segments) const
+{
+  return numCoefficientsRequired(num_time_segments) + spline_order_;
+}
+
+real_t BSpline::t_min() const
+{
+  CHECK_GE((int)knots_.size(), minimumKnotsRequired())
+      << "The B-spline is not well initialized";
+  return knots_[spline_order_ - 1];
+}
+
+real_t BSpline::t_max() const
+{
+  CHECK_GE((int)knots_.size(), minimumKnotsRequired())
+      << "The B-spline is not well initialized";
+  return knots_[knots_.size() - spline_order_];
+}
+
+std::pair<real_t,int> BSpline::computeTIndex(real_t t) const
+{
+  CHECK_GE(t, t_min()) << "The time is out of range by " << (t - t_min());
+
+  //// HACK - avoids numerical problems on initialisation
+  if (std::abs(t_max() - t) < 1e-10)
+  {
+    t = t_max();
+  }
+  //// \HACK
+
+  CHECK_LE(t, t_max())
+      << "The time is out of range by " << (t_max() - t);
+  std::vector<real_t>::const_iterator i;
+  if(t == t_max())
+  {
+    // This is a special case to allow us to evaluate the spline at the boundary of the
+    // interval. This is not stricly correct but it will be useful when we start doing
+    // estimation and defining knots at our measurement times.
+    i = knots_.end() - spline_order_;
+  }
+  else
+  {
+    i = std::upper_bound(knots_.begin(), knots_.end(), t);
+  }
+  //CHECK_NE(i, knots_.end()) << "Something very bad has happened in computeTIndex(" << t << ")";
+
+  // Returns the index of the knot segment this time lies on and the width of this knot segment.
+  return std::make_pair(*i - *(i-1),(i - knots_.begin()) - 1);
+
+}
+
+std::pair<real_t,int> BSpline::computeUAndTIndex(real_t t) const
+{
+  std::pair<real_t,int> ui = computeTIndex(t);
+
+  int index = ui.second;
+  real_t denom = ui.first;
+
+  if(denom <= 0.0)
+  {
+    // The case of duplicate knots.
+    //std::cout << "Duplicate knots\n";
+    return std::make_pair(0, index);
+  }
+  else
+  {
+    real_t u = (t - knots_[index])/denom;
+
+    return std::make_pair(u, index);
+  }
+}
+
+int dmul(int i, int derivative_order)
+{
+  if(derivative_order == 0)
+  {
+    return 1;
+  }
+  else if(derivative_order == 1)
+  {
+    return i;
+  }
+  else
+  {
+    return i * dmul(i-1,derivative_order-1) ;
+  }
+}
+
+VectorX BSpline::computeU(real_t uval,
+                          int segmentIndex,
+                          int derivativeOrder) const
+{
+  VectorX u = VectorX::Zero(spline_order_);
+  real_t delta_t = knots_[segmentIndex+1] - knots_[segmentIndex];
+  real_t multiplier = 0.0;
+  if(delta_t > 0.0)
+  {
+    multiplier = 1.0/pow(delta_t, derivativeOrder);
+  }
+
+  real_t uu = 1.0;
+  for(int i = derivativeOrder; i < spline_order_; i++)
+  {
+    u(i) = multiplier * uu * dmul(i,derivativeOrder) ;
+    uu = uu * uval;
+  }
+
+  return u;
+}
+
+VectorX BSpline::eval(real_t t) const
+{
+  return evalD(t,0);
+}
+
+const MatrixX& BSpline::basisMatrixFromKnotIndex(int knot_index) const
+{
+  return basis_matrices_[basisMatrixIndexFromStartingKnotIndex(knot_index)];
+}
+
+VectorX BSpline::evalD(real_t t, int derivative_order) const
+{
+  CHECK_GE(derivative_order, 0) << "To integrate, use the integral function";
+  // Returns the normalized u value and the lower-bound time index.
+  std::pair<real_t,int> ui = computeUAndTIndex(t);
+  VectorX u = computeU(ui.first, ui.second, derivative_order);
+
+  int bidx = ui.second - spline_order_ + 1;
+
+  // Evaluate the spline (or derivative) in matrix form.
+  //
+  // [c_0 c_1 c_2 c_3] * B^T * u
+  // spline coefficients
+
+  VectorX rv = coefficients_.block(0,bidx,coefficients_.rows(),spline_order_)
+               * basis_matrices_[bidx].transpose() * u;
+
+  return rv;
+}
+
+VectorX BSpline::evalDAndJacobian(real_t t,
+                                  int derivative_order,
+                                  MatrixX* Jacobian,
+                                  VectorXi* coefficient_indices) const
+{
+  CHECK_GE(derivative_order, 0) << "To integrate, use the integral function";
+  // Returns the normalized u value and the lower-bound time index.
+  std::pair<real_t,int> ui = computeUAndTIndex(t);
+  VectorX u = computeU(ui.first, ui.second, derivative_order);
+
+  int bidx = ui.second - spline_order_ + 1;
+
+  // Evaluate the spline (or derivative) in matrix form.
+  //
+  // [c_0 c_1 c_2 c_3] * B^T * u
+  // spline coefficients
+
+  // The spline value
+  VectorX Bt_u = basis_matrices_[bidx].transpose() * u;
+  VectorX v = coefficients_.block(0,bidx,coefficients_.rows(),spline_order_) * Bt_u;
+
+  if(Jacobian)
+  {
+    // The Jacobian
+    Jacobian->resize(coefficients_.rows(), Bt_u.size() * coefficients_.rows());
+    MatrixX one = MatrixX::Identity(coefficients_.rows(), coefficients_.rows());
+    for(int i = 0; i < Bt_u.size(); i++)
+    {
+      Jacobian->block(0, i*coefficients_.rows(),
+                      coefficients_.rows(),
+                      coefficients_.rows()) = one * Bt_u[i];
+    }
+  }
+
+  if(coefficient_indices)
+  {
+    int D = coefficients_.rows();
+    *coefficient_indices = VectorXi::LinSpaced(spline_order_ * D,
+                                              bidx * D,
+                                              (bidx + spline_order_) * D - 1);
+  }
+
+  return v;
+}
+
+std::pair<VectorX, MatrixX> BSpline::evalDAndJacobian(real_t t,
+                                                      int derivative_order) const
+{
+  std::pair<VectorX, MatrixX> rv;
+
+  rv.first = evalDAndJacobian(t, derivative_order, &rv.second, NULL);
+
+  return rv;
+}
+
+MatrixX BSpline::localBasisMatrix(real_t t, int derivative_order) const
+{
+  return Phi(t,derivative_order);
+}
+
+MatrixX BSpline::localCoefficientMatrix(real_t t) const
+{
+  std::pair<real_t,int> ui = computeTIndex(t);
+  int bidx = ui.second - spline_order_ + 1;
+
+  return coefficients_.block(0,bidx,coefficients_.rows(),spline_order_);
+}
+
+VectorX BSpline::localCoefficientVector(real_t t) const
+{
+
+  std::pair<real_t,int> ui = computeTIndex(t);
+  int bidx = ui.second - spline_order_ + 1;
+  VectorX c(spline_order_ * coefficients_.rows());
+  for(int i = 0; i < spline_order_; i++)
+  {
+    c.segment(i*coefficients_.rows(), coefficients_.rows()) = coefficients_.col(i + bidx);
+  }
+
+  return c;
+}
+
+VectorXi BSpline::localCoefficientVectorIndices(real_t t) const
+{
+  std::pair<real_t,int> ui = computeTIndex(t);
+  int bidx = ui.second - spline_order_ + 1;
+  int D = coefficients_.rows();
+
+  return VectorXi::LinSpaced(spline_order_*D,bidx*D,(bidx + spline_order_)*D - 1);
+}
+
+VectorXi BSpline::localVvCoefficientVectorIndices(real_t t) const
+{
+  std::pair<real_t,int> ui = computeTIndex(t);
+  int bidx = ui.second - spline_order_ + 1;
+
+  return VectorXi::LinSpaced(spline_order_,bidx,(bidx + spline_order_) - 1);
+}
+
+MatrixX BSpline::Phi(real_t t, int derivative_order) const
+{
+  CHECK_GE(derivative_order, 0) << "To integrate, use the integral function";
+  std::pair<real_t,int> ui = computeUAndTIndex(t);
+
+  VectorX u = computeU(ui.first, ui.second, derivative_order);
+
+  int bidx = ui.second - spline_order_ + 1;
+
+  u = basis_matrices_[bidx].transpose() * u;
+
+  MatrixX Phi = MatrixX::Zero(coefficients_.rows(),
+                              spline_order_*coefficients_.rows());
+  MatrixX one = MatrixX::Identity(Phi.rows(), Phi.rows());
+  for(int i = 0; i < spline_order_; i++)
+  {
+    Phi.block(0,Phi.rows()*i,Phi.rows(),Phi.rows()) = one * u(i);
+  }
+
+  return Phi;
+}
+
+void BSpline::setCoefficientVector(const VectorX& c)
+{
+  CHECK_GE(c.size(), coefficients_.rows() * coefficients_.cols())
+      << "The coefficient vector is the wrong size. The vector must contain all vector-valued coefficients stacked up into one column.";
+  for(int i = 0; i < coefficients_.cols(); i++)
+  {
+    coefficients_.col(i) = c.segment(i * coefficients_.rows(),coefficients_.rows());
+  }
+}
+
+VectorX BSpline::coefficientVector()
+{
+  VectorX c(coefficients_.rows() * coefficients_.cols());
+  for(int i = 0; i < coefficients_.cols(); i++)
+  {
+    c.segment(i * coefficients_.rows(),coefficients_.rows()) = coefficients_.col(i);
+  }
+  return c;
+}
+
+void BSpline::setCoefficientMatrix(const MatrixX& coefficients)
+{
+  CHECK_EQ(coefficients_.rows(), coefficients.rows())
+      << "The new coefficient matrix must match the size of the existing coefficient matrix";
+  CHECK_EQ(coefficients_.cols(), coefficients.cols())
+      << "The new coefficient matrix must match the size of the existing coefficient matrix";
+  coefficients_ = coefficients;
+}
+
+const MatrixX& BSpline::basisMatrix(int i) const
+{
+  CHECK_LE(i, numValidTimeSegments()) << "index out of range";
+  CHECK_LE(0, numValidTimeSegments()) << "index out of range";
+  return basis_matrices_[i];
+}
+
+
+std::pair<real_t,real_t> BSpline::timeInterval() const
+{
+  return std::make_pair(t_min(), t_max());
+}
+
+std::pair<real_t,real_t> BSpline::timeInterval(int i) const
+{
+  CHECK_GE((int)knots_.size(), minimumKnotsRequired()) << "The B-spline is not well initialized";
+  CHECK_LE(i, numValidTimeSegments()) << "index out of range";
+  CHECK_LT(0, numValidTimeSegments()) << "index out of range";
+  return std::make_pair(knots_[spline_order_ + i - 1],knots_[spline_order_ + i]);
+}
+
+void BSpline::initSpline(real_t t_0, real_t t_1,
+                         const VectorX& p_0,
+                         const VectorX& p_1)
+{
+  CHECK_EQ(p_0.size(), p_1.size())
+      << "The coefficient vectors should be the same size";
+  CHECK_GT(t_1, t_0) << "Time must be increasing from t_0 to t_1";
+
+  // Initialize the spline so that it interpolates the two points
+  // and moves between them with a constant velocity.
+
+  // How many knots are required for one time segment?
+  int K = numKnotsRequired(1);
+  // How many coefficients are required for one time segment?
+  int C = numCoefficientsRequired(1);
+  // What is the vector coefficient dimension
+  int D = p_0.size();
+
+  // Initialize a uniform knot sequence
+  real_t dt = t_1 - t_0;
+  std::vector<real_t> knots(K);
+  for(int i = 0; i < K; i++)
+  {
+    knots[i] = t_0 + (i - spline_order_ + 1) * dt;
+  }
+  // Set the knots and zero the coefficients
+  setKnotsAndCoefficients(knots, MatrixX::Zero(D,C));
+
+  // Now we have to solve an Ax = b linear system to determine the correct coefficient vectors.
+  int coefficientDim = C * D;
+  // We always need an even number of constraints.
+  int constraintsRequired = C + (C & 0x1);
+  int constraintSize = constraintsRequired * D;
+
+  MatrixX A = MatrixX::Zero(constraintSize, coefficientDim);
+  VectorX b = VectorX::Zero(constraintSize);
+
+  // Add the position constraints.
+  int brow = 0;
+  int bcol = 0;
+  A.block(brow,bcol,D,coefficientDim) = Phi(t_min(),0);
+  b.segment(brow,D) = p_0;
+  brow += D;
+  A.block(brow,bcol,D,coefficientDim) = Phi(t_max(),0);
+  b.segment(brow,D) = p_1;
+  brow += D;
+
+  if(spline_order_ > 2)
+  {
+    // At the very minimum we have to add velocity constraints.
+    VectorX v = (p_1 - p_0)/dt;
+    A.block(brow,bcol,D,coefficientDim) = Phi(t_min(),1);
+    b.segment(brow,D) = v;
+    brow += D;
+    A.block(brow,bcol,D,coefficientDim) = Phi(t_max(),1);
+    b.segment(brow,D) = v;
+    brow += D;
+
+    if(spline_order_ > 4)
+    {
+      // Now we add the constraint that all higher-order derivatives are zero.
+      int derivativeOrder = 2;
+      VectorX z = VectorX::Zero(D);
+      while(brow < A.rows())
+      {
+        A.block(brow,bcol,D,coefficientDim) = Phi(t_min(),derivativeOrder);
+        b.segment(brow,D) = z;
+        brow += D;
+        A.block(brow,bcol,D,coefficientDim) = Phi(t_max(),derivativeOrder);
+        b.segment(brow,D) = z;
+        brow += D;
+        ++derivativeOrder;
+      }
+    }
+  }
+
+  // Now we solve the Ax=b system
+  if(A.rows() != A.cols())
+  {
+    // The system is over constrained. This happens for odd ordered splines.
+    b = (A.transpose() * b).eval();
+    A = (A.transpose() * A).eval();
+  }
+
+  // Solve for the coefficient vector.
+  VectorX c = A.householderQr().solve(b);
+  // ldlt doesn't work for this problem. It may be because the ldlt decomposition
+  // requires the matrix to be positive or negative semidefinite
+  // http://eigen.tuxfamily.org/dox-devel/TutorialLinearAlgebra.html#TutorialLinAlgRankRevealing
+  // which may imply that it is symmetric. Our A matrix is only symmetric in the over-constrained case.
+  //VectorX c = A.ldlt().solve(b);
+  setCoefficientVector(c);
+}
+
+void BSpline::addCurveSegment(real_t t, const VectorX& p_1)
+{
+  CHECK_GT(t, t_max())
+      << "The new time must be past the end of the last valid segment";
+  CHECK_EQ(p_1.size(), coefficients_.rows()) << "Invalid coefficient vector size";
+
+  // Get the final valid time interval.
+  int NT = numValidTimeSegments();
+  std::pair<real_t, real_t> interval_km1 = timeInterval(NT-1);
+
+  VectorX p_0;
+
+  // Store the position of the spline at the  end of the interval.
+  // We will use these as constraints as we don't want them to change.
+  p_0 = eval(interval_km1.second);
+
+  // Retool the knot vector.
+  real_t du;
+  int km1;
+  std::tie(du,km1) = computeTIndex(interval_km1.first);
+
+  // leave knots km1 and k alone but retool the other knots.
+  real_t dt = t - knots_[km1 + 1];
+  real_t kt = t;
+
+  // add another knot.
+  std::vector<real_t> knots(knots_);
+  knots.push_back(0.0);
+  // space the further knots uniformly.
+  for(unsigned k = km1 + 2; k < knots.size(); k++)
+  {
+    knots[k] = kt;
+    kt += dt;
+  }
+  // Tack on an new, uninitialized coefficient column.
+  MatrixX c(coefficients_.rows(), coefficients_.cols() + 1);
+  c.topLeftCorner(coefficients_.rows(), coefficients_.cols()) = coefficients_;
+  setKnotsAndCoefficients(knots,c);
+
+  // Now, regardless of the order of the spline, we should only have to add
+  // a single knot and coefficient vector.
+  // In this case, we should solve for the last two coefficient vectors
+  // (i.e., the new one and the one before the new one).
+
+  // Get the time interval of the new time segment.
+  real_t t_0, t_1;
+  std::tie(t_0,t_1) = timeInterval(NT);
+
+  // what is the coefficient dimension?
+  int D = coefficients_.rows();
+  // How many vector-valued coefficients are required? In this case, 2.
+  // We will leave the others fixed.
+  int C = 2;
+  // Now we have to solve an Ax = b linear system to determine the
+  // correct coefficient vectors.
+  int coefficientDim = C * D;
+  // We always need an even number of constraints.
+  int constraintsRequired = 2;
+  int constraintSize = constraintsRequired * D;
+
+  MatrixX A = MatrixX::Zero(constraintSize, coefficientDim);
+  VectorX b = VectorX::Zero(constraintSize);      // Build the A matrix.
+
+  int phiBlockColumnOffset = D * std::max(0,(spline_order_ - 2));
+  VectorX fixedCoefficients = localCoefficientVector(t_0).segment(0,
+                                                                  phiBlockColumnOffset);
+
+  // Add the position constraints.
+  int brow = 0;
+  int bcol = 0;
+  MatrixX P;
+  P = Phi(t_0,0);
+  A.block(brow,bcol,D,coefficientDim) = P.block(0, phiBlockColumnOffset,
+                                                D, coefficientDim);
+  b.segment(brow,D) = p_0 - P.block(0,0,D,phiBlockColumnOffset) * fixedCoefficients;
+  brow += D;
+
+  P = Phi(t_1,0);
+  A.block(brow,bcol,D,coefficientDim) = P.block(0,phiBlockColumnOffset,
+                                                D, coefficientDim);
+  b.segment(brow,D) = p_1 - P.block(0,0,D,phiBlockColumnOffset) * fixedCoefficients;;
+  brow += D;
+
+  // Add regularization constraints (keep the coefficients small)
+  //A.block(brow,bcol,coefficientDim,coefficientDim) = 1e-4 * MatrixX::Identity(coefficientDim, coefficientDim);
+  //b.segment(brow,coefficientDim) = VectorX::Zero(coefficientDim);
+  //brow += coefficientDim;
+
+  // Now we solve the Ax=b system
+  if(A.rows() != A.cols())
+  {
+    // The system is over constrained. This happens for odd ordered splines.
+    b = (A.transpose() * b).eval();
+    A = (A.transpose() * A).eval();
+  }
+
+  VectorX cstar = A.householderQr().solve(b);
+  coefficients_.col(coefficients_.cols() - 2) = cstar.head(D);
+  coefficients_.col(coefficients_.cols() - 1) = cstar.tail(D);
+}
+
+
+void BSpline::removeCurveSegment()
+{
+  if(knots_.size() > 0 && coefficients_.cols() > 0)
+  {
+    knots_.erase(knots_.begin());
+    coefficients_ = coefficients_.block(0,
+                                        1,
+                                        coefficients_.rows(),
+                                        coefficients_.cols() - 1).eval();
+  }
+}
+
+void BSpline::setLocalCoefficientVector(real_t t, const VectorX& c)
+{
+  CHECK_EQ(c.size(), spline_order_ * coefficients_.rows())
+      << "The local coefficient vector is the wrong size";
+  std::pair<real_t,int> ui = computeTIndex(t);
+  int bidx = ui.second - spline_order_ + 1;
+  for(int i = 0; i < spline_order_; i++)
+  {
+    coefficients_.col(i + bidx) = c.segment(i * coefficients_.rows(),
+                                            coefficients_.rows());
+  }
+
+}
+
+void BSpline::initSpline2(const VectorX& times,
+                          const MatrixX& interpolation_points,
+                          int num_segments,
+                          real_t lambda)
+{
+  CHECK_EQ(times.size(), interpolation_points.cols())
+      << "The number of times and the number of interpolation points must be equal";
+  CHECK_GE(times.size(), 2) << "There must be at least two times";
+  CHECK_GE(num_segments, 1) << "There must be at least one time segment";
+  for(int i = 1; i < times.size(); i++)
+  {
+    CHECK_LE(times[i-1], times[i])
+        << "The time sequence must be nondecreasing. time " << i
+        << " was not greater than or equal to time " << (i-1);
+  }
+
+  // Initialize the spline so that it interpolates the N points
+
+  // How many knots are required for one time segment?
+  int K = numKnotsRequired(num_segments);
+  // How many coefficients are required for one time segment?
+  int C = numCoefficientsRequired(num_segments);
+  // What is the vector coefficient dimension
+  int D = interpolation_points.rows();
+
+  // Initialize a uniform knot sequence
+  real_t dt = (times[times.size() - 1] - times[0]) / num_segments;
+  std::vector<real_t> knots(K);
+  for(int i = 0; i < K; i++)
+  {
+    knots[i] = times[0] + (i - spline_order_ + 1) * dt;
+  }
+  // Set the knots and zero the coefficients
+  setKnotsAndCoefficients(knots, MatrixX::Zero(D,C));
+
+  // Now we have to solve an Ax = b linear system to determine the correct coefficient vectors.
+  int coefficientDim = C * D;
+
+  int numConstraints = (knots.size() - 2 * spline_order_ + 2) + interpolation_points.cols();
+  int constraintSize = numConstraints * D;
+
+  MatrixX A = MatrixX::Zero(constraintSize, coefficientDim);
+  VectorX b = VectorX::Zero(constraintSize);
+
+  int brow = 0;
+  //int bcol = 0;
+  // Now add the regularization constraint.
+  //A.block(brow,bcol,coefficientDim,coefficientDim) = 1e-1* MatrixX::Identity(coefficientDim, coefficientDim);
+  //b.segment(brow,coefficientDim) = VectorX::Zero(coefficientDim);
+  //brow += coefficientDim;
+  for(int i = spline_order_ - 1; i < (int)knots.size() - spline_order_ + 1; i++)
+  {
+    VectorXi coeffIndices = localCoefficientVectorIndices(knots[i]);
+
+    A.block(brow,coeffIndices[0],D,coeffIndices.size()) = lambda * Phi(knots[i],2);
+    b.segment(brow,D) = VectorX::Zero(D);
+    brow += D;
+  }
+
+  // Add the position constraints.
+  for(int i = 0; i < interpolation_points.cols(); i++)
+  {
+    VectorXi coeffIndices = localCoefficientVectorIndices(times[i]);
+    A.block(brow,coeffIndices[0],D,coeffIndices.size()) = Phi(times[i],0);
+
+    b.segment(brow,D) = interpolation_points.col(i);
+    brow += D;
+  }
+
+  // Now we solve the Ax=b system
+  //if(A.rows() != A.cols())
+  //  {
+  // The system is over constrained. This happens for odd ordered splines.
+  b = (A.transpose() * b).eval();
+  A = (A.transpose() * A).eval();
+  //  }
+
+  // Solve for the coefficient vector.
+  VectorX c = A.ldlt().solve(b);
+  // ldlt doesn't work for this problem. It may be because the ldlt decomposition
+  // requires the matrix to be positive or negative semidefinite
+  // http://eigen.tuxfamily.org/dox-devel/TutorialLinearAlgebra.html#TutorialLinAlgRankRevealing
+  // which may imply that it is symmetric. Our A matrix is only symmetric in the over-constrained case.
+  // VectorX c = A.ldlt().solve(b);
+  setCoefficientVector(c);
+}
+
+void BSpline::initSpline3(const VectorX& times,
+                          const MatrixX& interpolation_points,
+                          int num_segments,
+                          real_t lambda)
+{
+  CHECK_EQ(times.size(), interpolation_points.cols())
+      << "The number of times and the number of interpolation points must be equal";
+  CHECK_GE(times.size(), 2) << "There must be at least two times";
+  CHECK_GE(num_segments, 1) << "There must be at least one time segment";
+  for(int i = 1; i < times.size(); i++)
+  {
+    CHECK_LE(times[i-1], times[i])
+        <<  "The time sequence must be nondecreasing. time " << i
+        << " was not greater than or equal to time " << (i-1);
+  }
+
+  // How many knots are required for one time segment?
+  int K = numKnotsRequired(num_segments);
+  // How many coefficients are required for one time segment?
+  int C = numCoefficientsRequired(num_segments);
+  // What is the vector coefficient dimension
+  int D = interpolation_points.rows();
+
+  // Initialize a uniform knot sequence
+  real_t dt = (times[times.size() - 1] - times[0]) / num_segments;
+  std::vector<real_t> knots(K);
+  for(int i = 0; i < K; i++)
+  {
+    knots[i] = times[0] + (i - spline_order_ + 1) * dt;
+  }
+
+  setKnotsAndCoefficients(knots, MatrixX::Zero(D,C));
+
+  // Now we have to solve an Ax = b linear system to determine the correct coefficient vectors.
+  int coefficientDim = C * D;
+
+  int numConstraints = interpolation_points.cols();
+  int constraintSize = numConstraints * D;
+
+  MatrixX A = MatrixX::Zero(constraintSize, coefficientDim);
+  VectorX b = VectorX::Zero(constraintSize);
+
+  int brow = 0;
+  // Add the position constraints.
+  for(int i = 0; i < interpolation_points.cols(); i++)
+  {
+    VectorXi coeffIndices = localCoefficientVectorIndices(times[i]);
+
+    A.block(brow,coeffIndices[0],D,coeffIndices.size()) = Phi(times[i],0);
+
+    b.segment(brow,D) = interpolation_points.col(i);
+    brow += D;
+  }
+
+  b = (A.transpose() * b).eval();
+  A = (A.transpose() * A).eval();
+
+  // Add the motion constraint.
+  VectorX W = VectorX::Constant(D,lambda);
+
+  A += curveQuadraticIntegralDiag(W, 2);
+
+  VectorX c = A.ldlt().solve(b);
+  setCoefficientVector(c);
+
+}
+
+void BSpline::addCurveSegment2(real_t t,
+                               const VectorX& p_1,
+                               real_t lambda)
+{
+  CHECK_GT(t, t_max())
+      << "The new time must be past the end of the last valid segment";
+  CHECK_EQ(p_1.size(), coefficients_.rows())
+      << "Invalid coefficient vector size";
+
+  // Get the final valid time interval.
+  int NT = numValidTimeSegments();
+  std::pair<real_t, real_t> interval_km1 = timeInterval(NT-1);
+
+  VectorX p_0;
+
+  // Store the position of the spline at the  end of the interval.
+  // We will use these as constraints as we don't want them to change.
+  p_0 = eval(interval_km1.second);
+
+  // Retool the knot vector.
+  real_t du;
+  int km1;
+  std::tie(du,km1) = computeTIndex(interval_km1.first);
+
+  // leave knots km1 and k alone but retool the other knots.
+  real_t dt = t - knots_[km1 + 1];
+  real_t kt = t;
+
+  // add another knot.
+  std::vector<real_t> knots(knots_);
+  knots.push_back(0.0);
+  // space the further knots uniformly.
+  for(unsigned k = km1 + 2; k < knots.size(); k++)
+  {
+    knots[k] = kt;
+    kt += dt;
+  }
+  // Tack on an new, uninitialized coefficient column.
+  MatrixX c(coefficients_.rows(), coefficients_.cols() + 1);
+  c.topLeftCorner(coefficients_.rows(), coefficients_.cols()) = coefficients_;
+  setKnotsAndCoefficients(knots,c);
+
+  // Now, regardless of the order of the spline, we should only have to
+  // add a single knot and coefficient vector.
+  // In this case, we should solve for the last two coefficient
+  // vectors (i.e., the new one and the one before the
+  // new one).
+
+  // Get the time interval of the new time segment.
+  real_t t_0, t_1;
+  std::tie(t_0,t_1) = timeInterval(NT);
+
+  // what is the coefficient dimension?
+  int D = coefficients_.rows();
+  // How many vector-valued coefficients are required? In this case, 2.
+  // We will leave the others fixed.
+  int C = 2;
+  // Now we have to solve an Ax = b linear system to determine the correct
+  // coefficient vectors.
+  int coefficientDim = C * D;
+  // We always need an even number of constraints.
+  int constraintsRequired = 2 + 2;
+  int constraintSize = constraintsRequired * D;
+
+  MatrixX A = MatrixX::Zero(constraintSize, coefficientDim);
+  VectorX b = VectorX::Zero(constraintSize);      // Build the A matrix.
+
+  int phiBlockColumnOffset = D * std::max(0,(spline_order_ - 2));
+  VectorX fixedCoefficients = localCoefficientVector(t_0).segment(0,
+                                                                  phiBlockColumnOffset);
+
+  // Add the position constraints.
+  int brow = 0;
+  int bcol = 0;
+  MatrixX P;
+  P = Phi(t_0,0);
+  A.block(brow,bcol,D,coefficientDim) = P.block(0, phiBlockColumnOffset,
+                                                D, coefficientDim);
+  b.segment(brow,D) = p_0 - P.block(0,0,D,phiBlockColumnOffset) * fixedCoefficients;
+  brow += D;
+
+  P = Phi(t_1,0);
+  A.block(brow,bcol,D,coefficientDim) = P.block(0, phiBlockColumnOffset,
+                                                D, coefficientDim);
+  b.segment(brow,D) = p_1 - P.block(0,0,D,phiBlockColumnOffset) * fixedCoefficients;;
+  brow += D;
+
+
+  // Add regularization constraints (keep the acceleration small)
+  P = Phi(t_0,2);
+  A.block(brow,bcol,D,coefficientDim) = lambda * P.block(0, phiBlockColumnOffset,
+                                                         D, coefficientDim);
+  b.segment(brow,D) = VectorX::Zero(D);
+  brow += D;
+
+  P = Phi(t_1,2);
+  A.block(brow,bcol,D,coefficientDim) = lambda * P.block(0, phiBlockColumnOffset,
+                                                         D, coefficientDim);
+  b.segment(brow,D) = VectorX::Zero(D);
+  brow += D;
+
+  //A.block(brow,bcol,coefficientDim,coefficientDim) = 1e-4 * MatrixX::Identity(coefficientDim, coefficientDim);
+  //b.segment(brow,coefficientDim) = VectorX::Zero(coefficientDim);
+  //brow += coefficientDim;
+
+  // Now we solve the Ax=b system
+  if(A.rows() != A.cols())
+  {
+    // The system is over constrained. This happens for odd ordered splines.
+    b = (A.transpose() * b).eval();
+    A = (A.transpose() * A).eval();
+  }
+
+  VectorX cstar = A.householderQr().solve(b);
+  coefficients_.col(coefficients_.cols() - 2) = cstar.head(D);
+  coefficients_.col(coefficients_.cols() - 1) = cstar.tail(D);
+}
+
+MatrixX BSpline::Vi(int segment_index) const
+{
+  CHECK_LT(segment_index, numValidTimeSegments())
+      << "Segment index out of bounds";
+  CHECK_LT(0, numValidTimeSegments())
+      << "Segment index out of bounds";
+
+  VectorX vals(spline_order_*2);
+  for (int i = 0; i < vals.size(); ++i)
+  {
+    vals[i] = 1.0/(i + 1.0);
+  }
+
+  MatrixX V(spline_order_,spline_order_);
+  for(int r = 0; r < V.rows(); r++)
+  {
+    for(int c = 0; c < V.cols(); c++)
+    {
+      V(r,c) = vals[r + c];
+    }
+  }
+
+  real_t t_0,t_1;
+  std::tie(t_0,t_1) = timeInterval(segment_index);
+
+  V *= t_1 - t_0;
+
+  return V;
+}
+
+VectorX BSpline::evalIntegral(real_t t1, real_t t2) const
+{
+  if(t1 > t2)
+  {
+    return -evalIntegral(t2,t1);
+  }
+
+  std::pair<real_t,int> u1 = computeTIndex(t1);
+  std::pair<real_t,int> u2 = computeTIndex(t2);
+
+  VectorX integral = VectorX::Zero(coefficients_.rows());
+
+  // LHS remainder.
+  real_t lhs_remainder = t1 - knots_[u1.second];
+  if(lhs_remainder > 1e-16 && u1.first > 1e-16)
+  {
+    lhs_remainder /= u1.first;
+    VectorX v(spline_order_);
+    real_t du = lhs_remainder;
+    for(int i = 0; i < spline_order_; i++)
+    {
+      v(i) = du/(i + 1.0);
+      du *= lhs_remainder;
+    }
+    int bidx = basisMatrixIndexFromStartingKnotIndex(u1.second);
+    integral -= u1.first * coefficients_.block(0,
+                                               bidx,coefficients_.rows(),
+                                               spline_order_)
+                * basis_matrices_[bidx].transpose() * v;
+  }
+
+  // central time segments.
+  VectorX v = VectorX::Zero(spline_order_);
+  for(int i = 0; i < spline_order_; i++)
+  {
+    v(i) = 1.0/(i + 1.0);
+  }
+
+  for(int s = u1.second; s < u2.second; s++)
+  {
+    int bidx = basisMatrixIndexFromStartingKnotIndex(s);
+    integral += (knots_[s+1] - knots_[s])
+        * coefficients_.block(0, bidx, coefficients_.rows(), spline_order_)
+        * basis_matrices_[bidx].transpose() * v;
+  }
+
+  // RHS remainder.
+  real_t rhs_remainder = t2 - knots_[u2.second];
+  if(rhs_remainder > 1e-16 && u2.first > 1e-16)
+  {
+    rhs_remainder /= u2.first;
+
+    VectorX v(spline_order_);
+    real_t du = rhs_remainder;
+    for(int i = 0; i < spline_order_; i++)
+    {
+      v(i) = du / (i + 1.0);
+      du *= rhs_remainder;
+    }
+
+    int bidx = basisMatrixIndexFromStartingKnotIndex(u2.second);
+    integral += u2.first
+                * coefficients_.block(0,bidx,coefficients_.rows(),spline_order_)
+                * basis_matrices_[bidx].transpose()
+                * v;
+  }
+
+  return integral;
+}
+
+int BSpline::basisMatrixIndexFromStartingKnotIndex(int starting_knot_index) const
+{
+  return starting_knot_index - spline_order_ + 1;
+}
+
+int BSpline::startingKnotIndexFromBasisMatrixIndex(int basis_matrix_index) const
+{
+  return spline_order_ + basis_matrix_index - 1;
+}
+
+
+MatrixX BSpline::Bij(int segment_index, int column_index) const
+{
+  CHECK_LE(segment_index, (int)basis_matrices_.size()) << "Out of range";
+  CHECK_LT(0, (int)basis_matrices_.size()) << "Out of range";
+  CHECK_LE(column_index, spline_order_) << "Out of range";
+  CHECK_LT(0, spline_order_) << "Out of range";
+  int D = coefficients_.rows();
+  MatrixX B = MatrixX::Zero(spline_order_*D,D);
+  for(int i = 0; i < D; i++)
+  {
+    B.block(i*spline_order_,i,spline_order_,1) = basis_matrices_[segment_index].col(column_index);
+  }
+
+  return B;
+}
+
+MatrixX BSpline::Mi(int segment_index) const
+{
+  CHECK_LE(segment_index, (int)basis_matrices_.size()) << "Out of range";
+  CHECK_LT(0, (int)basis_matrices_.size()) << "Out of range";
+  int D = coefficients_.rows();
+  MatrixX M = MatrixX::Zero(spline_order_*D,spline_order_*D);
+
+  for(int j = 0; j < spline_order_; j++)
+  {
+    M.block(0,j*D,D*spline_order_, D) = Bij(segment_index,j);
+  }
+
+  return M;
+}
+
+VectorX BSpline::getLocalBiVector(real_t t, int derivative_order) const
+{
+  VectorX ret = VectorX::Zero(spline_order_);
+  getLocalBiInto(t, ret, derivative_order);
+
+  return ret;
+}
+
+void BSpline::getLocalBiInto(real_t t, VectorX& ret, int derivative_order) const
+{
+  int si = segmentIndex(t);
+  VectorX lu = u(t, derivative_order);
+  for(int j = 0; j < spline_order_; j++) {
+    ret[j] = lu.dot(basis_matrices_[si].col(j));
+  }
+}
+
+VectorX BSpline::getLocalCumulativeBiVector(real_t t, int derivative_order) const
+{
+  VectorX bi = getLocalBiVector(t, derivative_order);
+  int maxIndex = bi.rows() - 1;
+  // tildeB(i) = np.sum(bi[i+1:]) :
+  for(int i = 1; i <= maxIndex; i ++)
+  {
+    real_t sum = 0;
+    for(int j = maxIndex; j > i; j--)
+    {
+      sum += bi[j];
+    }
+    bi[i] += sum;
+  }
+  if (derivative_order == 0)
+  {
+    bi[0] = 1; // the sum of k successive spline basis functions is always 1
+  }
+  else
+  {
+    bi[0] = 0;
+  }
+  return bi;
+}
+
+int BSpline::segmentIndex(real_t t) const
+{
+  std::pair<real_t,int> ui = computeTIndex(t);
+
+  return basisMatrixIndexFromStartingKnotIndex(ui.second);
+}
+
+MatrixX BSpline::U(real_t t, int derivative_order) const
+{
+  VectorX uvec = u(t,derivative_order);
+  int D = coefficients_.rows();
+  MatrixX Umat = MatrixX::Zero(spline_order_ * D, D);
+
+  for(int i = 0; i < D; i++)
+  {
+    Umat.block(i*spline_order_,i,spline_order_,1) = uvec;
+  }
+
+  return Umat;
+}
+
+VectorX BSpline::u(real_t t, int derivative_order) const
+{
+  std::pair<real_t,int> ui = computeUAndTIndex(t);
+
+  return computeU(ui.first, ui.second, derivative_order);
+}
+
+MatrixX BSpline::Di(int segment_index) const
+{
+  int D = coefficients_.rows();
+  MatrixX fullD = MatrixX::Zero(spline_order_*D, spline_order_*D);
+
+  MatrixX subD = Dii(segment_index);
+
+  for(int d = 0; d < D; d++)
+  {
+    fullD.block(d*spline_order_,d*spline_order_,spline_order_,spline_order_) = subD;
+  }
+
+  return fullD;
+}
+
+MatrixX BSpline::Dii(int segment_index) const
+{
+  CHECK_LE(segment_index, (int)basis_matrices_.size()) << "Out of range";
+  CHECK_LT(0, (int)basis_matrices_.size()) << "Out of range";
+  real_t t_0,t_1;
+  std::tie(t_0,t_1) = timeInterval(segment_index);
+  real_t dt = t_1 - t_0;
+
+  real_t recip_dt = 0.0;
+  if(dt > 0)
+  {
+    recip_dt = 1.0/dt;
+  }
+  MatrixX D = MatrixX::Zero(spline_order_,spline_order_);
+  for(int i = 0; i < spline_order_ - 1; i++)
+  {
+    D(i,i+1) = (i+1.0) * recip_dt;
+  }
+
+  return D;
+}
+
+MatrixX BSpline::segmentQuadraticIntegral(const MatrixX& W,
+                                          int segment_idx,
+                                          int derivative_order) const
+{
+  int D = coefficients_.rows();
+  //CHECK_GE(segmentIndex, (int)basisMatrices_.size()) << "Out of range";
+  CHECK_LT(0, (int)basis_matrices_.size()) << "Out of range";
+  CHECK_EQ(W.rows(), D)
+      <<"W must be a square matrix the size of a single vector-valued coefficient";
+  CHECK_EQ(W.cols(), D)
+      << "W must be a square matrix the size of a single vector-valued coefficient";
+
+  int N = D * spline_order_;
+  MatrixX Q;// = MatrixX::Zero(N,N);
+  MatrixX Dm = Dii(segment_idx);
+  MatrixX V = Vi(segment_idx);
+  MatrixX M = Mi(segment_idx);
+
+  // Calculate the appropriate derivative version of V
+  // using the matrix multiplication version of the derivative.
+  for(int i = 0; i < derivative_order; i++)
+  {
+    V = (Dm.transpose() * V * Dm).eval();
+  }
+
+  MatrixX WV = MatrixX::Zero(N,N);
+
+  for(int r = 0; r < D; r++)
+  {
+    for(int c = 0; c < D; c++)
+    {
+      CHECK_GE(1e-14, std::abs(W(r,c) - W(c,r))) << "W must be symmetric";
+      //std::cout << "Size WV: " << WV.rows() << ", " << WV.cols() << std::endl;
+      //std::cout << "Size V: " << V.rows() << ", " << V.cols() << std::endl;
+      WV.block(spline_order_*r,
+               spline_order_*c,
+               spline_order_,
+               spline_order_) = W(r,c) * V;
+    }
+  }
+
+  Q = M.transpose() * WV * M;
+
+  return Q;
+}
+
+MatrixX BSpline::segmentQuadraticIntegralDiag(const VectorX& Wdiag,
+                                              int segment_idx,
+                                              int derivative_order) const
+{
+  int D = coefficients_.rows();
+  //CHECK_GE(segmentIndex, (int)basisMatrices_.size()) << "Out of range";
+  CHECK_LT(0, (int)basis_matrices_.size()) << "Out of range";
+  CHECK_EQ(Wdiag.size(), D) << "Wdiag must be the length of a single vector-valued coefficient";
+
+  int N = D * spline_order_;
+  MatrixX Q;// = MatrixX::Zero(N,N);
+  MatrixX Dm = Dii(segment_idx);
+  MatrixX V = Vi(segment_idx);
+  MatrixX M = Mi(segment_idx);
+
+  // Calculate the appropriate derivative version of V
+  // using the matrix multiplication version of the derivative.
+  for(int i = 0; i < derivative_order; i++)
+  {
+    V = (Dm.transpose() * V * Dm).eval();
+  }
+
+  MatrixX WV = MatrixX::Zero(N,N);
+
+  for(int d = 0; d < D; d++)
+  {
+    //std::cout << "Size WV: " << WV.rows() << ", " << WV.cols() << std::endl;
+    //std::cout << "Size V: " << V.rows() << ", " << V.cols() << std::endl;
+    WV.block(spline_order_*d, spline_order_*d,spline_order_,spline_order_) = Wdiag(d) * V;
+  }
+
+  Q = M.transpose() * WV * M;
+
+  return Q;
+}
+
+MatrixX BSpline::curveQuadraticIntegral(const MatrixX& W,
+                                        int derivative_order) const
+{
+  int D = coefficients_.rows();
+  CHECK_EQ(W.rows(), D)
+      << "W must be a square matrix the size of a single vector-valued coefficient";
+  CHECK_EQ(W.cols(), D)
+      << "W must be a square matrix the size of a single vector-valued coefficient";
+  int N = coefficients_.cols();
+
+  MatrixX Q = MatrixX::Zero(D*N, D*N);
+
+  int QiSize = spline_order_ * D;
+  for(int s = 0; s < numValidTimeSegments(); s++)
+  {
+    Q.block(s*D,s*D,QiSize,QiSize) += segmentQuadraticIntegral(W, s, derivative_order);
+  }
+
+  return Q;
+}
+
+MatrixX BSpline::curveQuadraticIntegralDiag(const VectorX& Wdiag,
+                                            int derivative_order) const
+{
+  int D = coefficients_.rows();
+  CHECK_EQ(Wdiag.size(), D)
+      << "Wdiag must be the length of a single vector-valued coefficient";
+  int N = coefficients_.cols();
+
+  MatrixX Q = MatrixX::Zero(D*N, D*N);
+
+  int QiSize = spline_order_ * D;
+  for(int s = 0; s < numValidTimeSegments(); s++)
+  {
+    Q.block(s*D,s*D,QiSize,QiSize) += segmentQuadraticIntegralDiag(Wdiag,
+                                                                   s,
+                                                                   derivative_order);
+  }
+
+  return Q;
+}
+
+int BSpline::coefficientVectorLength() const
+{
+  return coefficients_.rows() * coefficients_.cols();
+}
+
+void BSpline::initConstantSpline(real_t t_min,
+                                 real_t t_max,
+                                 int num_segments,
+                                 const VectorX& constant)
+{
+  CHECK_GT(t_max, t_min) << "The max time is less than the min time";
+  CHECK_GE(num_segments, 1) << "There must be at least one segment";
+  CHECK_GE(constant.size(), 1)
+      << "The constant vector must be of at least length 1";
+
+  int K = numKnotsRequired(num_segments);
+  int C = numCoefficientsRequired(num_segments);
+  real_t dt = (t_max - t_min) / (real_t)num_segments;
+
+  real_t minTime = t_min - (spline_order_ - 1)*dt;
+  real_t maxTime = t_max + (spline_order_ - 1)*dt;
+  VectorX knotVector = VectorX::LinSpaced(K,minTime,maxTime);
+  // std::cout << "K: " << K << std::endl;
+  // std::cout << "S: " << numSegments << std::endl;
+  // std::cout << "segTime: " << t_min << ", " << t_max << std::endl;
+  // std::cout << "dt: " << dt << std::endl;
+  // std::cout << "time: " << minTime << ", " << maxTime << std::endl;
+  // std::cout << "order: " << splineOrder_ << std::endl;
+  // std::cout << knotVector.transpose() << std::endl;
+  MatrixX coeff(constant.size(),C);
+  for(int i = 0; i < C; i++)
+  {
+    coeff.col(i) = constant;
+  }
+
+  setKnotVectorAndCoefficients(knotVector,coeff);
+}
+
+int BSpline::numCoefficients() const
+{
+  return coefficients_.rows() * coefficients_.cols();
+}
+
+Eigen::Map<VectorX> BSpline::vvCoefficientVector(int i)
+{
+  CHECK_LE(i, coefficients_.cols()) << "Index out of range";
+  CHECK_LT(0, coefficients_.cols()) << "Index out of range";
+  return Eigen::Map<VectorX>(&coefficients_(0,i),coefficients_.rows());
+}
+
+Eigen::Map<const VectorX> BSpline::vvCoefficientVector(int i) const
+{
+  CHECK_LE(i, coefficients_.cols()) << "Index out of range";
+  CHECK_LT(0, coefficients_.cols()) << "Index out of range";
+  return Eigen::Map<const VectorX>(&coefficients_(0,i),coefficients_.rows());
+}
+
+int BSpline::numVvCoefficients() const
+{
+  return coefficients_.cols();
+}
+
+}  // namespace ze
diff --git a/RWR/src/ze_oss/ze_splines/src/bspline_pose_minimal.cpp b/RWR/src/ze_oss/ze_splines/src/bspline_pose_minimal.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..49a952a0bed1bf84f3313100ed8c573750bb473a
--- /dev/null
+++ b/RWR/src/ze_oss/ze_splines/src/bspline_pose_minimal.cpp
@@ -0,0 +1,511 @@
+// Copyright (c) 2014, Paul Furgale, Jérôme Maye and Jörn Rehder, Autonomous Systems Lab, ETH Zurich, Switzerland
+// Copyright (c) 2014, Thomas Schneider, Skybotix AG, Switzerland
+// Copyright (c) 2016, Luc Oth
+// Copyright (C) 2016 ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+//
+// Derived from https://github.com/ethz-asl/kalibr/ (2016)
+
+#include <ze/splines/bspline_pose_minimal.hpp>
+
+#include <ze/common/time_conversions.hpp>
+
+namespace ze {
+
+template<class RP>
+BSplinePoseMinimal<RP>::BSplinePoseMinimal(int spline_order)
+  : BSpline(spline_order)
+{
+}
+
+template<class RP>
+BSplinePoseMinimal<RP>::~BSplinePoseMinimal()
+{
+}
+
+template<class RP>
+Matrix4 BSplinePoseMinimal<RP>::transformation(real_t tk) const
+{
+  return curveValueToTransformation(eval(tk));
+}
+
+template<class RP>
+Matrix4 BSplinePoseMinimal<RP>::transformationAndJacobian(
+    real_t tk,
+    MatrixX* J,
+    VectorXi* coefficient_indices) const
+{
+  MatrixX JS;
+  VectorX p;
+  p = evalDAndJacobian(tk, 0, &JS, coefficient_indices);
+
+  MatrixX JT;
+  Matrix4 T = curveValueToTransformationAndJacobian(p, &JT);
+
+  if(J)
+  {
+    *J = JT * JS;
+  }
+
+  return T;
+}
+
+template<class RP>
+Matrix3 BSplinePoseMinimal<RP>::orientationAndJacobian(
+    real_t tk,
+    MatrixX* J,
+    VectorXi* coefficient_indices) const
+{
+  Matrix3 C;
+  MatrixX JS;
+  VectorX p;
+  p = evalDAndJacobian(tk,0,&JS, coefficient_indices);
+
+  Matrix3 S;
+
+  RP rp(Vector3(p.tail<3>()));
+  C = rp.getRotationMatrix();
+  S = rp.toSMatrix();
+
+  MatrixX JO = MatrixX::Zero(3,6);
+  JO.block(0,3,3,3) = S;
+  if(J)
+  {
+    *J = JO * JS;
+  }
+
+  return C;
+}
+
+template<class RP>
+Matrix3 BSplinePoseMinimal<RP>::inverseOrientationAndJacobian(
+    real_t tk,
+    MatrixX* J,
+    VectorXi* coefficient_indices) const
+{
+  Matrix3 C;
+  MatrixX JS;
+  VectorX p;
+  p = evalDAndJacobian(tk, 0, &JS, coefficient_indices);
+
+  Matrix3 S;
+  RP rp(Vector3(p.tail<3>()));
+  C = rp.getRotationMatrix().transpose();
+  S = rp.toSMatrix();
+
+  MatrixX JO = MatrixX::Zero(3,6);
+  JO.block(0,3,3,3) = S;
+  if(J)
+  {
+    *J = -C * JO * JS;
+  }
+
+  return C;
+}
+
+template<class RP>
+Matrix4 BSplinePoseMinimal<RP>::inverseTransformationAndJacobian(
+    real_t tk,
+    MatrixX* J,
+    VectorXi* coefficient_indices) const
+{
+  MatrixX JS;
+  VectorX p;
+  p = evalDAndJacobian(tk,0,&JS, coefficient_indices);
+
+  MatrixX JT;
+  Matrix4 T = curveValueToTransformationAndJacobian( p, &JT );
+  // Invert the transformation.
+  T.topLeftCorner<3,3>().transposeInPlace();
+  T.topRightCorner<3,1>() = (-T.topLeftCorner<3,3>()
+                             * T.topRightCorner<3,1>()).eval();
+
+  if(J)
+  {
+    // The "box times" is the linearized transformation way of
+    // inverting the jacobian.
+     *J = -ze::sm::boxTimes(T) * JT * JS;
+  }
+
+  if(coefficient_indices)
+  {
+    *coefficient_indices = localCoefficientVectorIndices(tk);
+  }
+
+  return T;
+}
+
+template<class RP>
+Matrix4 BSplinePoseMinimal<RP>::inverseTransformation(real_t tk) const
+{
+  Matrix4 T = curveValueToTransformation(eval(tk));
+  T.topLeftCorner<3,3>().transposeInPlace();
+  T.topRightCorner<3,1>() = (-T.topLeftCorner<3,3>() * T.topRightCorner<3,1>()).eval();
+  return T;
+}
+
+template<class RP>
+Vector4 BSplinePoseMinimal<RP>::transformVectorAndJacobian(
+    real_t tk,
+    const Vector4& v_tk,
+    MatrixX* J,
+    VectorXi* coefficient_indices) const
+{
+  MatrixX JT;
+  Matrix4 T_n_vk = transformationAndJacobian(tk, &JT, coefficient_indices);
+  Vector4 v_n = T_n_vk * v_tk;
+
+  if(J)
+  {
+     *J = ze::sm::boxMinus(v_n) * JT;
+  }
+
+  return v_n;
+}
+
+template<class RP>
+Vector3 BSplinePoseMinimal<RP>::position(real_t tk) const
+{
+  Matrix61 v = eval(tk);
+  return v.head<3>();
+}
+
+template<class RP>
+Matrix3 BSplinePoseMinimal<RP>::orientation(real_t tk) const
+{
+  Matrix61 v = eval(tk);
+  return RP(Vector3(v.tail<3>())).getRotationMatrix();
+}
+
+template<class RP>
+Matrix3 BSplinePoseMinimal<RP>::inverseOrientation(real_t tk) const
+{
+  Matrix61 v = eval(tk);
+  return RP(Vector3(v.tail<3>())).getRotationMatrix().transpose();
+}
+
+template<class RP>
+Vector3 BSplinePoseMinimal<RP>::linearVelocity(real_t tk) const
+{
+  Matrix61 v = evalD(tk, 1);
+  return v.head<3>();
+}
+
+template<class RP>
+Vector3 BSplinePoseMinimal<RP>::linearVelocityBodyFrame(real_t tk) const
+{
+  Matrix61 r = evalD(tk, 0);
+  Matrix61 v = evalD(tk, 1);
+  Matrix3 C_wb = RP(Vector3(r.tail<3>())).getRotationMatrix();
+  return C_wb.transpose() * v.head<3>();
+}
+
+template<class RP>
+Vector3 BSplinePoseMinimal<RP>::linearAcceleration(real_t tk) const
+{
+  Matrix61 v = evalD(tk, 2);
+  return v.head<3>();
+}
+
+template<class RP>
+Vector3 BSplinePoseMinimal<RP>::linearAccelerationBodyFrame(real_t tk) const
+{
+  VectorX r = evalD(tk, 0);
+  Matrix61 v = evalD(tk, 2);
+  Matrix3 C_wb = RP(Vector3(r.tail<3>())).getRotationMatrix();
+  return C_wb.transpose() * v.head<3>();
+}
+
+template<class RP>
+Vector3 BSplinePoseMinimal<RP>::linearAccelerationAndJacobian(
+    real_t tk,
+    MatrixX* J,
+    VectorXi* coefficient_indices) const
+{
+
+  Matrix61 v = evalDAndJacobian(tk, 2, J, coefficient_indices);
+  Vector3 a = v.head<3>();
+  if(J)
+  {
+    J->conservativeResize(3,J->cols());
+  }
+  return a;
+}
+
+// \omega_w_{b,w} (angular velocity of the body frame as seen from the world
+// frame, expressed in the world frame)
+template<class RP>
+Vector3 BSplinePoseMinimal<RP>::angularVelocity(real_t tk) const
+{
+  Vector3 omega;
+  VectorX r = evalD(tk,0);
+  VectorX v = evalD(tk,1);
+
+  // \omega = S(\bar \theta) \dot \theta
+  RP rp(Vector3(r.tail<3>()));
+  Matrix3 S = rp.toSMatrix();
+
+  omega = -S * v.tail<3>();
+  return omega;
+}
+
+// \omega_b_{w,b} (angular velocity of the world frame as seen from the body
+// frame, expressed in the body frame)
+template<class RP>
+Vector3 BSplinePoseMinimal<RP>::angularVelocityBodyFrame(real_t tk) const
+{
+  Vector3 omega;
+  VectorX r = evalD(tk,0);
+  VectorX v = evalD(tk,1);
+  Matrix3 S;
+  RP rp(Vector3(r.tail<3>()));
+  Matrix3 C_w_b = rp.getRotationMatrix();
+
+  // \omega = S(\bar \theta) \dot \theta
+  S = rp.toSMatrix();
+  omega = -C_w_b.transpose() * S * v.tail<3>();
+
+  return omega;
+}
+
+// \omega_b_{w,b} (angular velocity of the world frame as seen from the body
+// frame, expressed in the body frame)
+template<class RP>
+Vector3 BSplinePoseMinimal<RP>::angularVelocityBodyFrameAndJacobian(
+    real_t tk,
+    MatrixX* J,
+    VectorXi* coefficient_indices) const
+{
+  Vector3 omega;
+  Vector3 p;
+  Vector3 pdot;
+  MatrixX Jp;
+  MatrixX Jpdot;
+  Matrix61 v = evalDAndJacobian(tk, 0, &Jp, NULL);
+  Matrix61 vdot = evalDAndJacobian(tk, 1, &Jpdot, coefficient_indices);
+  p = v.tail<3>();
+  pdot = vdot.tail<3>();
+
+  MatrixX Jr;
+  Matrix3 C_w_b = inverseOrientationAndJacobian(tk,&Jr,NULL);
+
+  // Rearrange the spline jacobian matrices. Now Jpdot is the
+  // jacobian of p wrt the spline coefficients stacked on top
+  // of the jacobian of pdot wrt the spline coefficients.
+  Jpdot.block(0, 0, 3, Jpdot.cols()) = Jp.block(3, 0, 3, Jp.cols());
+
+  Matrix36 Jo;
+  RP rp(p);
+  omega = -C_w_b * rp.angularVelocityAndJacobian(pdot, &Jo);
+  Jo = (-C_w_b * Jo).eval();
+  //std::cout << "Jo:\n" << Jo << std::endl;
+  if(J)
+  {
+    *J = Jo * Jpdot + skewSymmetric(omega) * Jr;
+  }
+
+  return omega;
+}
+
+// \omega_w_{b,w} (angular velocity of the body frame as seen from the world
+// frame, expressed in the world frame)
+template<class RP>
+Vector3 BSplinePoseMinimal<RP>::angularVelocityAndJacobian(
+    real_t tk,
+    MatrixX* J,
+    VectorXi* coefficient_indices) const
+{
+
+  Vector3 omega;
+  Vector3 p;
+  Vector3 pdot;
+  MatrixX Jp;
+  MatrixX Jpdot;
+  Matrix61 v = evalDAndJacobian(tk, 0, &Jp, NULL);
+  Matrix61 vdot = evalDAndJacobian(tk, 1, &Jpdot, coefficient_indices);
+  p = v.tail<3>();
+  pdot = vdot.tail<3>();
+
+  // Rearrange the spline jacobian matrices. Now Jpdot is the
+  // jacobian of p wrt the spline coefficients stacked on top
+  // of the jacobian of pdot wrt the spline coefficients.
+  Jpdot.block(0,0,3,Jpdot.cols()) = Jp.block(3,0,3,Jp.cols());
+
+  //std::cout << "Jpdot\n" << Jpdot << std::endl;
+
+  Matrix36 Jo;
+  RP rp(p);
+  omega = rp.angularVelocityAndJacobian(pdot, &Jo);
+
+  //std::cout << "Jo:\n" << Jo << std::endl;
+  if(J) {
+    *J = Jo * Jpdot;
+  }
+
+  return omega;
+}
+
+template<class RP>
+void BSplinePoseMinimal<RP>::initPoseSpline(
+    real_t t0,
+    real_t t1,
+    const Matrix4& T_n_t0,
+    const Matrix4& T_n_t1)
+{
+  VectorX v0 = transformationToCurveValue(T_n_t0);
+  VectorX v1 = transformationToCurveValue(T_n_t1);
+
+  initSpline(t0,t1,v0,v1);
+}
+
+template<class RP>
+void BSplinePoseMinimal<RP>::addPoseSegment(real_t tk, const Matrix4& T_n_tk)
+{
+  VectorX vk = transformationToCurveValue(T_n_tk);
+
+  addCurveSegment(tk, vk);
+}
+
+template<class RP>
+void BSplinePoseMinimal<RP>::addPoseSegment2(
+    real_t tk,
+    const Matrix4& T_n_tk,
+    real_t lambda)
+{
+  VectorX vk = transformationToCurveValue(T_n_tk);
+
+  addCurveSegment2(tk, vk, lambda);
+}
+
+template<class RP>
+Matrix4 BSplinePoseMinimal<RP>::curveValueToTransformation(const VectorX& c) const
+{
+  CHECK_EQ(c.size(), 6) << "The curve value is an unexpected size!";
+  Matrix4 T = Matrix4::Identity();
+  T.topLeftCorner<3,3>() = RP(Vector3(c.tail<3>())).getRotationMatrix();
+  T.topRightCorner<3,1>() = c.head<3>();
+
+  return T;
+}
+
+template<class RP>
+Matrix4 BSplinePoseMinimal<RP>::curveValueToTransformationAndJacobian(
+    const VectorX& p, MatrixX * J) const
+{
+  CHECK_EQ(p.size(), 6) << "The curve value is an unexpected size!";
+  Matrix4 T = Matrix4::Identity();
+  Matrix3 S;
+  RP rp(Vector3(p.tail<3>()));
+  T.topLeftCorner<3,3>() = rp.getRotationMatrix();
+  T.topRightCorner<3,1>() = p.head<3>();
+  S = rp.toSMatrix();
+
+  if(J)
+  {
+    *J = MatrixX::Identity(6,6);
+    J->topRightCorner<3,3>() = -skewSymmetric(p.head<3>()) * S;
+    J->bottomRightCorner<3,3>() = S;
+  }
+
+  return T;
+}
+
+template<class RP>
+VectorX BSplinePoseMinimal<RP>::transformationToCurveValue(
+    const Matrix4& T) const
+{
+  VectorX c(6);
+  c.head<3>() = T.topRightCorner<3,1>();
+  Matrix3 Tlc = T.topLeftCorner<3, 3>();
+  RP rp(Tlc);
+  c.tail<3>() = rp.getParameters();
+
+  return c;
+}
+
+template<class RP>
+void BSplinePoseMinimal<RP>::initPoseSpline2(
+    const VectorX& times,
+    const Eigen::Matrix<real_t,6,Eigen::Dynamic>& poses,
+    int num_segments,
+    real_t lambda)
+{
+  initSpline2(times, poses, num_segments, lambda);
+}
+
+template<class RP>
+void BSplinePoseMinimal<RP>::initPoseSpline3(
+    const VectorX& times,
+    const Eigen::Matrix<real_t,6,Eigen::Dynamic>& poses,
+    int num_segments,
+    real_t lambda)
+{
+  initSpline3(times, poses, num_segments, lambda);
+}
+
+template<class RP>
+void BSplinePoseMinimal<RP>::initPoseSplinePoses(
+    const VectorX& times,
+    const std::vector<Matrix4>& poses,
+    int num_segments,
+    real_t lambda)
+{
+  Eigen::Matrix<real_t, 6, Eigen::Dynamic> parameters;
+  parameters.resize(6, poses.size());
+  for (size_t i = 0; i < poses.size(); ++i)
+  {
+    parameters.col(i) = transformationToCurveValue(poses[i]);
+  }
+
+  initPoseSpline3(times, parameters, num_segments, lambda);
+}
+
+template<class RP>
+void BSplinePoseMinimal<RP>::initPoseSplinePoses(
+    const StampedTransformationVector& poses,
+    int num_segments,
+    real_t lambda)
+{
+  Eigen::Matrix<real_t, 6, Eigen::Dynamic> parameters;
+  parameters.resize(6, poses.size());
+  VectorX times(poses.size());
+  for (size_t i = 0; i < poses.size(); ++i)
+  {
+    const StampedTransformation& pose = poses[i];
+    parameters.col(i) =
+        transformationToCurveValue(pose.second.getTransformationMatrix());
+    times(i) = nanosecToSecTrunc(pose.first);
+  }
+
+  initPoseSpline3(times, parameters, num_segments, lambda);
+}
+
+// explicit specialization
+template class BSplinePoseMinimal<ze::sm::RotationVector>;
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_splines/src/viz_splines.cpp b/RWR/src/ze_oss/ze_splines/src/viz_splines.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..7eaaee724217eed62c8530e73f579310ea1c3715
--- /dev/null
+++ b/RWR/src/ze_oss/ze_splines/src/viz_splines.cpp
@@ -0,0 +1,167 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include <ze/splines/viz_splines.hpp>
+
+#include <ze/matplotlib/matplotlibcpp.hpp>
+#include <ze/visualization/viz_interface.hpp>
+
+namespace ze {
+
+SplinesVisualizer::SplinesVisualizer(const std::shared_ptr<Visualizer>& viz)
+  : viz_(viz)
+{}
+
+void SplinesVisualizer::plotSpline(const BSpline& bs,
+                                   const size_t dimension,
+                                   real_t step_size)
+{
+  real_t start = bs.t_min();
+  real_t end = bs.t_max();
+  size_t samples = (end - start) / step_size - 1;
+  size_t spline_dimension = bs.dimension();
+  CHECK_LE(dimension, spline_dimension);
+
+  ze::VectorX points(samples);
+  ze::VectorX times(samples);
+
+  for (size_t i = 0; i < samples; ++i)
+  {
+    points(i) = (bs.eval(start + i * step_size))(dimension);
+    times(i) = start + i * step_size;
+  }
+
+  plt::plot(times, points);
+  plt::show();
+}
+
+void SplinesVisualizer::displaySplineTrajectory(const BSpline& bs,
+                                                const std::string& topic,
+                                                const size_t id,
+                                                const Color& color,
+                                                real_t step_size)
+{
+  CHECK_LE(bs.dimension(), 3) << "For splines with more than 3 dimensions specify"
+                              << "which dimenions to draw.";
+
+  switch (bs.dimension())
+  {
+    case 1:
+      return displaySplineTrajectory(bs, topic, id, color, {0}, step_size);
+    case 2:
+      return displaySplineTrajectory(bs, topic, id, color, {0, 1}, step_size);
+    case 3:
+      return displaySplineTrajectory(bs, topic, id, color, {0, 1, 2}, step_size);
+  }
+}
+
+void SplinesVisualizer::displaySplineTrajectory(
+    const BSpline& bs,
+    const std::string& topic,
+    const size_t id,
+    const Color& color,
+    const std::vector<size_t>& draw_dimensions,
+    real_t step_size)
+{
+  real_t start = bs.t_min();
+  real_t end = bs.t_max();
+  size_t samples = (end - start) / step_size - 1;
+  size_t dimension = bs.dimension();
+
+  ze::Positions points(3, samples);
+  points.setZero();
+
+  //! @todo: this is terribly inefficient.
+  for (size_t i = 0; i < samples; ++i)
+  {
+    VectorX v = bs.eval(start + i * step_size);
+    for (size_t j = 0; j < draw_dimensions.size(); ++j)
+    {
+      points(j, i) = v(draw_dimensions[j]);
+    }
+  }
+
+  viz_->drawPoints(topic, id, points, color);
+}
+
+void SplinesVisualizer::displaySplineTrajectory(
+    const BSplinePoseMinimalRotationVector& bs,
+    const std::string& topic,
+    const size_t id,
+    real_t step_size)
+{
+  ze::TransformationVector poses;
+
+  real_t start = bs.t_min();
+  real_t end = bs.t_max();
+  size_t samples = (end - start) / step_size - 1;
+
+  for (size_t i = 0; i <= samples; ++i)
+  {
+    Transformation T(bs.transformation(start + i * step_size));
+    poses.push_back(T);
+  }
+
+  viz_->drawCoordinateFrames(topic, id, poses,  1);
+}
+
+void SplinesVisualizer::plotSpline(
+    const BSplinePoseMinimalRotationVector& bs,
+    real_t step_size)
+{
+  real_t start = bs.t_min();
+  real_t end = bs.t_max();
+  size_t samples = (end - start) / step_size - 1;
+
+  ze::MatrixX points(6, samples);
+  ze::VectorX times(samples);
+
+  for (size_t i = 0; i < samples; ++i)
+  {
+    points.col(i) = bs.eval(start + i * step_size);
+    times(i) = start + i * step_size;
+  }
+
+  // translation
+  plt::subplot(3, 1, 1);
+  plt::plot(times, points.row(0));
+  plt::subplot(3, 1, 2);
+  plt::plot(times, points.row(1));
+  plt::subplot(3, 1, 3);
+  plt::plot(times, points.row(2));
+  plt::show(false);
+
+  // rotation
+  plt::figure();
+  plt::subplot(3, 1, 1);
+  plt::plot(times, points.row(3));
+  plt::subplot(3, 1, 2);
+  plt::plot(times, points.row(4));
+  plt::subplot(3, 1, 3);
+  plt::plot(times, points.row(5));
+  plt::show();
+}
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_splines/src/viz_splines_test.cpp b/RWR/src/ze_oss/ze_splines/src/viz_splines_test.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..adfedebc9b290fadf3cf295cd70e4fa9e05f3531
--- /dev/null
+++ b/RWR/src/ze_oss/ze_splines/src/viz_splines_test.cpp
@@ -0,0 +1,72 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include <ros/ros.h>
+#include <ze/common/logging.hpp>
+#include <gflags/gflags.h>
+
+#include <ze/visualization/viz_ros.hpp>
+#include <ze/splines/viz_splines.hpp>
+#include <ze/splines/bspline.hpp>
+#include <ze/splines/bspline_pose_minimal.hpp>
+#include <ze/common/types.hpp>
+
+int main(int argc, char** argv)
+{
+  google::InitGoogleLogging(argv[0]);
+  google::ParseCommandLineFlags(&argc, &argv, true);
+
+  // Visualizer internally initializes ROS and creates a node handle.
+  std::shared_ptr<ze::VisualizerRos> rv = std::make_shared<ze::VisualizerRos>();
+  ze::SplinesVisualizer sv(rv);
+
+  // Create a random 3d spline.
+  ze::BSpline bs(3);
+
+  ze::VectorX times = ze::VectorX::LinSpaced(500, 0, 100);
+  ze::MatrixX points;
+  points.resize(3, times.size());
+  points.setRandom();
+
+  bs.initSpline3(times, points, 100, 1e-5);
+  sv.displaySplineTrajectory(bs, "spline", 0, ze::Colors::Green);
+
+  // Create a random pose spline.
+  ze::BSplinePoseMinimalRotationVector pbs(3);
+  ze::MatrixX poses;
+  poses.resize(6, times.size());
+  poses.setRandom();
+  poses.topRows(3) *= 10;
+
+  pbs.initPoseSpline3(times, poses, 100, 1e-5);
+
+  sv.displaySplineTrajectory(pbs, "poses", 0);
+
+  // 2d Plots
+  sv.plotSpline(bs, 0);
+
+  // plot a pose spline as 6 dimensions
+  sv.plotSpline(pbs);
+}
diff --git a/RWR/src/ze_oss/ze_splines/test/test_bspline.cpp b/RWR/src/ze_oss/ze_splines/test/test_bspline.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..1c921519ed7d7304aa6858c53d89d3c94d28c1d7
--- /dev/null
+++ b/RWR/src/ze_oss/ze_splines/test/test_bspline.cpp
@@ -0,0 +1,229 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include <ze/splines/bspline.hpp>
+
+// Bring in gtest
+#include <ze/common/test_entrypoint.hpp>
+#include <ze/common/numerical_derivative.hpp>
+#include <ze/common/manifold.hpp>
+
+namespace ze {
+
+// A wrapper around a bspline to numerically estimate Jacobians at a given
+// instance of time and derivative order
+class FixedTimeBSpline
+{
+public:
+  //! t: time of evaluation
+  //! d: derivative order (0..splineOrder -1)
+  FixedTimeBSpline(BSpline* bs, real_t t, int d)
+    : bs_(bs), t_(t), d_(d)
+  {
+  }
+
+  //! get the vector of active coefficients at the evaluation time
+  VectorX coefficientVector()
+  {
+    return bs_->localCoefficientVector(t_);
+  }
+
+  //! set the coefficient vector at the evaluation time
+  void setCoefficientVector(VectorX c)
+  {
+    return bs_->setLocalCoefficientVector(t_, c);
+  }
+
+  //! evaluate the splines given a local coefficient vector
+  VectorX eval(VectorX c)
+  {
+    VectorX old_c = coefficientVector();
+    setCoefficientVector(c);
+    VectorX value = bs_->evalD(t_, d_);
+    setCoefficientVector(old_c);
+
+    return value;
+  }
+
+private:
+  BSpline* bs_;
+  real_t t_;
+  int d_;
+};
+} // namespace ze
+
+// Check that the Jacobian calculation is correct.
+TEST(SplineTestSuite, testBSplineJacobian)
+{
+  using namespace ze;
+
+  const int segments = 2;
+  for(int order = 2; order < 10; order++)
+  {
+    BSpline bs(order);
+    int nk = bs.numKnotsRequired(segments);
+    std::vector<real_t> knots;
+    for (int i = 0; i < nk; ++i)
+    {
+      knots.push_back(i);
+    }
+
+    for (int dim = 1; dim < 4; ++dim)
+    {
+      int nc = bs.numCoefficientsRequired(segments);
+      MatrixX C = MatrixX::Random(dim,nc);
+      bs.setKnotsAndCoefficients(knots, C);
+      for (int derivative = 0; derivative < order; ++derivative)
+      {
+        for (real_t t = bs.t_min(); t < bs.t_max(); t += 0.1)
+        {
+
+          FixedTimeBSpline fixed_bs(&bs, t, derivative);
+
+          Eigen::Matrix<real_t, Eigen::Dynamic, 1> point =
+              fixed_bs.coefficientVector();
+          MatrixX estJ =
+              numericalDerivative<MatrixX, VectorX>(
+                std::bind(&FixedTimeBSpline::eval, &fixed_bs,
+                          std::placeholders::_1), point);
+
+          MatrixX J;
+          VectorX v;
+          std::tie(v,J) = bs.evalDAndJacobian(t, derivative);
+
+          EXPECT_TRUE(EIGEN_MATRIX_NEAR(J, estJ, 1e-5));
+        }
+      }
+    }
+  }
+}
+
+TEST(SplineTestSuite, testCoefficientMap)
+{
+  using namespace ze;
+
+  const int order = 4;
+  const int segments = 10;
+  const int dim = 5;
+  BSpline bs(order);
+  int nk = bs.numKnotsRequired(segments);
+  int nc = bs.numCoefficientsRequired(segments);
+
+  std::vector<real_t> knots;
+  for (int i = 0; i < nk; ++i)
+  {
+    knots.push_back(i);
+  }
+
+  MatrixX C = MatrixX::Random(dim,nc);
+  bs.setKnotsAndCoefficients(knots, C);
+
+  const MatrixX & CC = bs.coefficients();
+  for (int i = 0; i < bs.numVvCoefficients(); ++i)
+  {
+    Eigen::Map<VectorX> m = bs.vvCoefficientVector(i);
+    // Test pass by value...
+    Eigen::Map<Eigen::Matrix<real_t, 5, 1> > m2=
+        bs.fixedSizeVvCoefficientVector<dim>(i);
+    for (int r = 0; r < m.size(); ++r)
+    {
+      ASSERT_TRUE( &m[r] == &CC(r,i) );
+      ASSERT_TRUE( &m[r] == &m2[r] );
+      m[r] = rand();
+      ASSERT_EQ( m[r], CC(r,i) );
+      ASSERT_EQ( m[r], m2[r] );
+    }
+  }
+}
+
+TEST(SplineTestSuite, testGetBi)
+{
+  using namespace ze;
+
+  const int order = 4;
+  const int segments = 10;
+  const real_t startTime = 0;
+  const real_t endTime = 5;
+  const int numTimeSteps = 43;
+  BSpline bs(order);
+  bs.initConstantSpline(startTime, endTime, segments, VectorX::Zero(1));
+
+  for (int i = 0; i <= numTimeSteps; ++i)
+  {
+    real_t t = startTime + (endTime - startTime) * ((real_t) i / numTimeSteps);
+    VectorX localBiVector = bs.getLocalBiVector(t);
+
+    EXPECT_NEAR(localBiVector.sum(), 1.0, 1e-10)
+        << "the bis at a given time should always sum up to 1";
+
+    VectorX biVector = bs.getBiVector(t);
+    EXPECT_NEAR(localBiVector.sum(), 1.0, 1e-10)
+        << "the bis at a given time should always sum up to 1";
+
+    VectorX cumulativeBiVector = bs.getCumulativeBiVector(t);
+    VectorX localCumulativeBiVector = bs.getLocalCumulativeBiVector(t);
+
+    VectorXi localCoefficientVectorIndices =
+        bs.localCoefficientVectorIndices(t);
+    int firstIndex = localCoefficientVectorIndices[0];
+    EXPECT_EQ(localCoefficientVectorIndices.size(), order)
+        << "localCoefficientVectorIndices has to have exactly " << order << " entries";
+
+    for (int j = 0; j < order; ++j)
+    {
+      EXPECT_EQ(localCoefficientVectorIndices[j], firstIndex + j)
+          << "localCoefficientVectorIndices have to be successive";
+    }
+
+    for (int j = 0, n = biVector.size(); j < n; ++j)
+    {
+      if (j < firstIndex)
+      {
+        EXPECT_EQ(biVector[j], 0);
+        EXPECT_EQ(cumulativeBiVector[j], 1);
+      }
+      else if (j < firstIndex + order)
+      {
+        EXPECT_EQ(biVector[j], localBiVector[j - firstIndex])
+            << "localBiVector should be a slice of the biVector";
+        EXPECT_EQ(cumulativeBiVector[j], localCumulativeBiVector[j - firstIndex])
+            << "localCumulativeBiVector must be a slice of the cumulativeBiVector";
+        EXPECT_NEAR(cumulativeBiVector[j],
+                    localBiVector.segment(j - firstIndex, order - (j - firstIndex)).sum(),
+                    1e-13)
+            << "cumulativeBiVector must be the sum of the localBiVector"
+            << "where it overlaps, but it is not at " << (j - firstIndex)
+            << " (localBiVector=" << localBiVector << ")";
+      }
+      else
+      {
+        EXPECT_EQ(biVector[j], 0) << "at position " << j;
+        EXPECT_EQ(cumulativeBiVector[j], 0) << "at position " << j;
+      }
+    }
+  }
+}
+
+ZE_UNITTEST_ENTRYPOINT
diff --git a/RWR/src/ze_oss/ze_splines/test/test_bspline_pose_minimal.cpp b/RWR/src/ze_oss/ze_splines/test/test_bspline_pose_minimal.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6a21afa4ee9b3a816fdb21233141946bf81db87b
--- /dev/null
+++ b/RWR/src/ze_oss/ze_splines/test/test_bspline_pose_minimal.cpp
@@ -0,0 +1,372 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include <ze/splines/bspline_pose_minimal.hpp>
+
+// Bring in gtest
+#include <ze/common/test_entrypoint.hpp>
+#include <ze/common/numerical_derivative.hpp>
+#include <ze/common/transformation.hpp>
+#include <ze/splines/operators.hpp>
+
+namespace ze {
+
+// A wrapper around a bspline to numerically estimate Jacobians at a given
+// instance of time and derivative order
+template<class ROTATION>
+class FixedTimeBSplinePoseMinimal
+{
+public:
+  //! t: time of evaluation
+  //! d: derivative order (0..splineOrder -1)
+  //! v: homogeneous coordinates vector to transform (transformation Jacobians)
+  FixedTimeBSplinePoseMinimal(
+      BSplinePoseMinimal<ROTATION>* bs,
+      real_t t, int d, Vector4 v = Vector4::Zero())
+    : bs_(bs)
+    , t_(t)
+    , d_(d)
+    , v_(v)
+  {
+  }
+
+  //! get the vector of active coefficients at the evaluation time
+  VectorX coefficientVector()
+  {
+    return bs_->localCoefficientVector(t_);
+  }
+
+  //! set the coefficient vector at the evaluation time
+  void setCoefficientVector(VectorX c)
+  {
+    return bs_->setLocalCoefficientVector(t_, c);
+  }
+
+  //! evaluate the splines given a local coefficient vector
+  VectorX eval(VectorX c)
+  {
+    VectorX old_c = coefficientVector();
+    setCoefficientVector(c);
+    VectorX value = bs_->evalD(t_, d_);
+    setCoefficientVector(old_c);
+
+    return value;
+  }
+
+  //! evaluate the transformation given a local coefficient vector
+  //! strictly it is T * v that is evaluated and not T itself
+  Vector4 transformation(VectorX c)
+  {
+    VectorX old_c = coefficientVector();
+    setCoefficientVector(c);
+    Matrix4 value = bs_->transformation(t_);
+    setCoefficientVector(old_c);
+
+    return value * v_;
+  }
+
+  //! evaluate the inverse transformation given a local coefficient vector
+  //! strictly it is T^1 * v that is evaluated and not T itself
+  Vector4 inverseTransformation(VectorX c)
+  {
+    VectorX old_c = coefficientVector();
+    setCoefficientVector(c);
+    Matrix4 value = bs_->inverseTransformation(t_);
+    setCoefficientVector(old_c);
+
+    return value * v_;
+  }
+
+  Vector3 linearAcceleration(VectorX c)
+  {
+    VectorX old_c = coefficientVector();
+    setCoefficientVector(c);
+    Vector3 value = bs_->linearAccelerationAndJacobian(t_, NULL, NULL);
+    setCoefficientVector(old_c);
+
+    return value;
+  }
+
+  Vector3 angularVelocity(VectorX c)
+  {
+    VectorX old_c = coefficientVector();
+    setCoefficientVector(c);
+    Vector3 value = bs_->angularVelocity(t_);
+    setCoefficientVector(old_c);
+
+    return value;
+  }
+
+  Vector3 angularVelocityBodyFrame(VectorX c)
+  {
+    VectorX old_c = coefficientVector();
+    setCoefficientVector(c);
+    Vector3 value = bs_->angularVelocityBodyFrame(t_);
+    setCoefficientVector(old_c);
+
+    return value;
+  }
+
+private:
+  BSplinePoseMinimal<ROTATION>* bs_;
+  real_t t_;
+  int d_;
+  Vector4 v_;
+};
+} // namespace ze
+
+TEST(BSplinePoseMinimalTestSuite, testCurveValueToTransformation)
+{
+  using namespace ze;
+
+  BSplinePoseMinimal<ze::sm::RotationVector> bs(3);
+
+  Vector6 point = Vector6::Random();
+  Matrix4 T = bs.curveValueToTransformation(point);
+
+  EXPECT_TRUE(EIGEN_MATRIX_NEAR(
+                bs.transformationToCurveValue(T),
+                point,
+                1e-6));
+}
+
+// Check that the Jacobian calculation is correct.
+TEST(BSplinePoseMinimalTestSuite, testBSplineTransformationJacobian)
+{
+  using namespace ze;
+
+  for (int order = 2; order < 10; ++order)
+  {
+    // Create a two segment spline.
+    BSplinePoseMinimal<ze::sm::RotationVector> bs(order);
+    bs.initPoseSpline(0.0, 1.0, bs.curveValueToTransformation(VectorX::Random(6)),
+                      bs.curveValueToTransformation(VectorX::Random(6)));
+    bs.addPoseSegment(2.0,bs.curveValueToTransformation(VectorX::Random(6)));
+
+    // Create a random homogeneous vector.
+    Vector4 v = Vector4::Random() * 10.0;
+
+    for (real_t t = bs.t_min(); t <= bs.t_max(); t+= 0.413)
+    {
+      FixedTimeBSplinePoseMinimal<ze::sm::RotationVector> fixed_bs(
+            &bs, t, 0, v);
+
+      Eigen::Matrix<real_t, Eigen::Dynamic, 1> point =
+          fixed_bs.coefficientVector();
+      MatrixX estJ =
+          numericalDerivative<MatrixX, VectorX>(
+            std::bind(
+              &FixedTimeBSplinePoseMinimal<ze::sm::RotationVector>::transformation,
+              &fixed_bs, std::placeholders::_1), point);
+
+      MatrixX JT;
+      Matrix4 T = bs.transformationAndJacobian(t, &JT);
+
+      MatrixX J = ze::sm::boxMinus(T*v) * JT;
+
+      EXPECT_TRUE(EIGEN_MATRIX_NEAR(J, estJ, 1e-6));
+
+      // Try again with the lumped function.
+      Vector4 v_n = bs.transformVectorAndJacobian(t, v, &J);
+      EXPECT_TRUE(EIGEN_MATRIX_NEAR(v_n, T*v, 1e-6));
+      EXPECT_TRUE(EIGEN_MATRIX_NEAR(J, estJ, 1e-6));
+
+      return;
+    }
+  }
+}
+
+// Check that the Jacobian calculation is correct.
+TEST(BSplinePoseMinimalTestSuite, testBSplineInverseTransformationJacobian)
+{
+  using namespace ze;
+
+  for (int order = 2; order < 10; ++order) {
+    // Create a two segment spline.
+    BSplinePoseMinimal<ze::sm::RotationVector> bs(order);
+    bs.initPoseSpline(0.0, 1.0, bs.curveValueToTransformation(VectorX::Random(6)),
+                      bs.curveValueToTransformation(VectorX::Random(6)));
+    bs.addPoseSegment(2.0,bs.curveValueToTransformation(VectorX::Random(6)));
+
+    // Create a random homogeneous vector.
+    Vector4 v = Vector4::Random() * 10.0;
+
+    for (real_t t = bs.t_min(); t <= bs.t_max(); t+= 0.413) {
+      FixedTimeBSplinePoseMinimal<ze::sm::RotationVector> fixed_bs(
+            &bs, t, 0, v);
+
+      Eigen::Matrix<real_t, Eigen::Dynamic, 1> point =
+          fixed_bs.coefficientVector();
+      MatrixX estJ =
+          numericalDerivative<MatrixX, VectorX>(
+            std::bind(
+              &FixedTimeBSplinePoseMinimal<ze::sm::RotationVector>::inverseTransformation,
+              &fixed_bs, std::placeholders::_1), point);
+
+      MatrixX JT;
+      MatrixX J;
+
+      Matrix4 T = bs.inverseTransformationAndJacobian(t, &JT);
+
+      J = ze::sm::boxMinus(T*v) * JT;
+      EXPECT_TRUE(EIGEN_MATRIX_NEAR(J, estJ, 1e-6));
+    }
+  }
+}
+
+TEST(BSplinePoseMinimalTestSuite, testBSplineAccelerationJacobian)
+{
+  using namespace ze;
+
+  for (int order = 2; order < 10; ++order) {
+    // Create a two segment spline.
+    BSplinePoseMinimal<ze::sm::RotationVector> bs(order);
+    bs.initPoseSpline(0.0, 1.0, bs.curveValueToTransformation(VectorX::Random(6)),
+                      bs.curveValueToTransformation(VectorX::Random(6)));
+    bs.addPoseSegment(2.0,bs.curveValueToTransformation(VectorX::Random(6)));
+
+    for (real_t t = bs.t_min(); t <= bs.t_max(); t+= 0.1) {
+      MatrixX J;
+      bs.linearAccelerationAndJacobian(t, &J, NULL);
+
+      FixedTimeBSplinePoseMinimal<ze::sm::RotationVector> fixed_bs(
+            &bs, t, 0);
+
+      Eigen::Matrix<real_t, Eigen::Dynamic, 1> point =
+          fixed_bs.coefficientVector();
+      MatrixX estJ =
+          numericalDerivative<VectorX, VectorX>(
+            std::bind(
+              &FixedTimeBSplinePoseMinimal<ze::sm::RotationVector>::linearAcceleration,
+              &fixed_bs, std::placeholders::_1), point);
+
+      EXPECT_TRUE(EIGEN_MATRIX_NEAR(J, estJ, 1e-6));
+
+    }
+  }
+}
+
+TEST(BSplinePoseMinimalTestSuite, testBSplineAngularVelocityJacobian)
+{
+  using namespace ze;
+
+  for (int order = 2; order < 10; ++order)
+  {
+    // Create a two segment spline.
+    BSplinePoseMinimal<ze::sm::RotationVector> bs(order);
+    bs.initPoseSpline(0.0, 1.0, bs.curveValueToTransformation(VectorX::Random(6)),
+                      bs.curveValueToTransformation(VectorX::Random(6)));
+    bs.addPoseSegment(2.0,bs.curveValueToTransformation(VectorX::Random(6)));
+
+    for (real_t t = bs.t_min(); t <= bs.t_max(); t+= 0.1) {
+      MatrixX J;
+      bs.angularVelocityAndJacobian(t, &J, NULL);
+
+      FixedTimeBSplinePoseMinimal<ze::sm::RotationVector> fixed_bs(
+            &bs, t, 0);
+
+      Eigen::Matrix<real_t, Eigen::Dynamic, 1> point =
+          fixed_bs.coefficientVector();
+      MatrixX estJ =
+          numericalDerivative<VectorX, VectorX>(
+            std::bind(
+              &FixedTimeBSplinePoseMinimal<ze::sm::RotationVector>::angularVelocity,
+              &fixed_bs, std::placeholders::_1), point);
+
+      // opposite sign due to perturbation choice
+      EXPECT_TRUE(EIGEN_MATRIX_NEAR(J, -estJ, 1e-6));
+
+    }
+  }
+}
+
+TEST(BSplinePoseMinimalTestSuite, testBSplineAngularVelocityBodyFrameJacobian)
+{
+  using namespace ze;
+
+  for (int order = 2; order < 10; ++order) {
+    // Create a two segment spline.
+    BSplinePoseMinimal<ze::sm::RotationVector> bs(order);
+    bs.initPoseSpline(0.0, 1.0, bs.curveValueToTransformation(VectorX::Random(6)),
+                      bs.curveValueToTransformation(VectorX::Random(6)));
+    bs.addPoseSegment(2.0,bs.curveValueToTransformation(VectorX::Random(6)));
+
+    for (real_t t = bs.t_min(); t <= bs.t_max(); t+= 0.1)
+    {
+      MatrixX J;
+      bs.angularVelocityBodyFrameAndJacobian(t, &J, NULL);
+
+      FixedTimeBSplinePoseMinimal<ze::sm::RotationVector> fixed_bs(
+            &bs, t, 0);
+
+      Eigen::Matrix<real_t, Eigen::Dynamic, 1> point =
+          fixed_bs.coefficientVector();
+      MatrixX estJ =
+          numericalDerivative<VectorX, VectorX>(
+            std::bind(
+              &FixedTimeBSplinePoseMinimal<ze::sm::RotationVector>::angularVelocityBodyFrame,
+              &fixed_bs, std::placeholders::_1), point);
+
+      EXPECT_TRUE(EIGEN_MATRIX_NEAR(J, estJ, 1e-6));
+
+    }
+  }
+}
+
+TEST(BSplinePoseMinimalTestSuite, testInitializePoses)
+{
+  using namespace ze;
+
+  BSplinePoseMinimal<ze::sm::RotationVector> bs(3);
+  bs.initPoseSpline(0.0, 1.0, bs.curveValueToTransformation(VectorX::Random(6)),
+                    bs.curveValueToTransformation(VectorX::Random(6)));
+  bs.addPoseSegment(2.0,bs.curveValueToTransformation(VectorX::Random(6)));
+
+  // get a vector matrice
+  std::vector<Matrix4> poses;
+  Eigen::Matrix<real_t, 1, 20> times;
+  size_t i = 0;
+  for (real_t t = bs.t_min(); t <= bs.t_max(); t += 0.1)
+  {
+    times(i) = t;
+    poses.push_back(bs.transformation(t));
+    ++i;
+  }
+
+  // init another spline
+  BSplinePoseMinimal<ze::sm::RotationVector> bs2(3);
+  bs2.initPoseSplinePoses(times, poses, 8, 1e-6);
+
+  // compare
+  for (real_t t = bs.t_min(); t <= bs.t_max(); t += 0.1)
+  {
+    EXPECT_TRUE(EIGEN_MATRIX_NEAR(
+                  bs.transformation(t),
+                  bs2.transformation(t),
+                  1e-2));
+  }
+}
+
+ZE_UNITTEST_ENTRYPOINT
diff --git a/RWR/src/ze_oss/ze_trajectory_analysis/CATKIN_IGNORE b/RWR/src/ze_oss/ze_trajectory_analysis/CATKIN_IGNORE
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/RWR/src/ze_oss/ze_trajectory_analysis/CMakeLists.txt b/RWR/src/ze_oss/ze_trajectory_analysis/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..682c9f7b2d1809fd0db143207cecf8bbce7d37bb
--- /dev/null
+++ b/RWR/src/ze_oss/ze_trajectory_analysis/CMakeLists.txt
@@ -0,0 +1,39 @@
+project(ze_trajectory_analysis)
+cmake_minimum_required(VERSION 2.8.3)
+
+find_package(catkin_simple REQUIRED)
+catkin_simple(ALL_DEPS_REQUIRED)
+
+# This macro ensures modules and global scripts declared therein get installed
+# See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+catkin_python_setup()
+
+include(ze_setup)
+
+#############
+# LIBRARIES #
+#############
+set(HEADERS
+  include/ze/trajectory_analysis/kitti_evaluation.hpp
+  )
+
+set(SOURCES
+  src/kitti_evaluation.cpp
+  )
+
+cs_add_library(${PROJECT_NAME} ${SOURCES} ${HEADERS})
+
+###############
+# EXECUTABLES #
+###############
+cs_add_executable(match_stamps src/match_stamps_node.cpp)
+target_link_libraries(match_stamps ${PROJECT_NAME})
+
+cs_add_executable(kitti_evaluation src/kitti_evaluation_node.cpp)
+target_link_libraries(kitti_evaluation ${PROJECT_NAME})
+
+##########
+# EXPORT #
+##########
+cs_install()
+cs_export()
diff --git a/RWR/src/ze_oss/ze_trajectory_analysis/README.md b/RWR/src/ze_oss/ze_trajectory_analysis/README.md
new file mode 100644
index 0000000000000000000000000000000000000000..f947dc83347c73d52211ca4c016ef094bf310fd5
--- /dev/null
+++ b/RWR/src/ze_oss/ze_trajectory_analysis/README.md
@@ -0,0 +1,97 @@
+# Trajectory Analysis
+
+A series of scripts and tools to compare trajectories among each other.
+
+## Inputs
+
+### Trajectories
+
+Provided as CSV-file with the structure:
+
+1. Pose Series (`pose`)
+  ```
+  # timestamp, x, y, z, qx, qy, qz, qw
+  ```
+  Where the  `timestamp` is in nanoseconds; `x`, `y`, `z` are metric and `qx`, `qy`, `qz`, `qw` are the elements of a unit quaternion.
+
+2. `swe`
+  ```
+  timestamp, x, y, z, qx, qy, qz, qw, vx, vy, vz, bgx, bgy, bgz, bax, bay, baz
+  ```
+  The first 7 components are equivalent to the pose series representation. `vx`, `vy`, `vz` are the componenents of the velocity. `bgx`, `bgy`, `bgz` the bias of the gyroscope and `bax`, `bay` and `baz` the bias of the accelerometer.
+
+3. `euroc`
+  ```
+  #timestamp, p_RS_R_x [m], p_RS_R_y [m], p_RS_R_z [m], q_RS_w [], q_RS_x [], q_RS_y [], q_RS_z [], v_RS_R_x [m s^-1], v_RS_R_y [m s^-1], v_RS_R_z [m s^-1], b_w_RS_S_x [rad s^-1], b_w_RS_S_y [rad s^-1], b_w_RS_S_z [rad s^-1], b_a_RS_S_x [m s^-2], b_a_RS_S_y [m s^-2], b_a_RS_S_z [m s^-2]
+  ```
+
+### Hand-Eye Calibration
+Specified as yaml file with the keys:
+```
+T_sensor_trackable:
+  qx: ...
+  qy: ... 
+  qz: ... 
+  qw: ...
+  tx: ...
+  ty: ...
+  tz: ...
+```
+Again, `tx`, `ty` and `tz` are the matric translational values and `qx`, `qy`, `qz`, `qw` represent a unit quaternion. The transformation matrix described by `t_` and `q_` is `T_B_H`, transforming point from the frame of the "Hand" to the body-frame.
+
+### Naming conventions in data directory:
+The scripts expect a rather strict naming convention in the data directory:
+* `traj_gt.csv`: the groundtruth trajectory,
+* `traj_es.csv`: the estimated trajectory to evaluate,
+* `traj_es_gt_matches.csv`: [optional] this file is automatically generated when running `analyse.py` but if provided, it will skip this computation.
+
+## Common Tasks
+
+### Trajectory Analysis (`analyse.py`)
+
+Takes a results folder with groundtruth and estimated trajectories, aligns them and generates a series of statistics and plots.
+
+Command line arguments:
+```
+--data_dir: Full path to the directory containing the result files (traj_gt.csv, traj_es.csv)
+--format_gt: Format of the provided ground-truth [swe, pose, euroc] (Defalt: pose)
+--format_es: Format of the estimate [swe, pose, euroc] (Default: pose)
+--alignment: Trajectory alignment to perform (Default: se3):
+             - se3: Estimate a 3d transform that minimizes the squared error between the groundtruth and estimated trajectory.
+             - sim3: Estimate a 3d transform and a scaling factor that minimizes the squared error between the groundtruth and estimated trajectory.
+             - first_frame: align the starting pose of the two trajectories.
+--alignment_first_frame: the first frame to consider for trajectory alignment (Default: 0)
+--alignment_last_frame: the last frame to consider for trajectory alignment, -1 for disabled. (Default: -1)
+--plot_size: Scaling factor for the plots (Default: 0.2)
+--skip_frames: Number of frames to skip between segment evaluations (Default: 1)
+```
+
+### Compare two results folders (`compare.py`)
+
+Takes two results folders performs a comparative analysis on the two.
+
+Command line arguments:
+```
+--trace_dir: full path to the results to compare to the reference
+--reference: full path to the results that serve as reference for trace
+```
+The results folder must contain a `job.yaml` with at least an `experiment_name` key.
+
+## Relative errors (`relative_errors.py`)
+
+Compure relative errors of two trajectories with respect to different segment lengths along the trajectory.
+
+Command line arguments:
+```
+--data_dir: Full path to the directory containing the results
+--format_gt: Format of the provided ground-truth [swe, pose, euroc] (Defalt: pose)
+--format_es: Format of the estimate [swe, pose, euroc] (Default: pose)
+--skip_frames: Number of frames to skip between segment evaluations (Default: 1)
+--segment_lengths: The segments lengths to split the trajectory into for statistical evaluations as comma separated string (Default: 10, 40, 90, 160)
+--segment_align_lsq: Align every segment pair to minimize the squared errors (on SE3)
+--segment_align_lsq_translation_only:  Align every segment pair considering only translations
+--plot_size: Scaling factor for the plots (Default: 0.2)
+--plot_errors_along_trajectory: Plot the relative errors along the trajectory (Default: False)
+```
+
+
diff --git a/RWR/src/ze_oss/ze_trajectory_analysis/examples/trajectory_alignment.py b/RWR/src/ze_oss/ze_trajectory_analysis/examples/trajectory_alignment.py
new file mode 100644
index 0000000000000000000000000000000000000000..ff313015006c3a2957222cef5fa50d4a2b5d762f
--- /dev/null
+++ b/RWR/src/ze_oss/ze_trajectory_analysis/examples/trajectory_alignment.py
@@ -0,0 +1,29 @@
+#!/usr/bin/env python3
+"""
+Zurich Eye
+"""
+
+# This example shows how to:
+# 1) Load two trajectories
+# 2) Find closest timestamps
+# 3) Align the trajectories
+# 4) Compute error statistics
+
+import logging
+import ze_py.test.utils as test_utils
+import ze_trajectory_analysis.analyse as traj_analysis
+
+# Init logging.
+logging.basicConfig(level=logging.DEBUG)
+logger = logging.getLogger(__name__)
+logger.info('Trajectory alignment example.')
+
+data_dir = test_utils.get_test_data_dir('ze_applanix_gt_data')
+
+ta = traj_analysis.TrajectoryAnalysis('.')
+ta.load_data(data_dir)
+ta.align_trajectory('se3', 300, -1)
+ta.plot_aligned_trajectory()
+ta.plot_estimator_results(data_dir)
+ta.compute_rms_errors()
+
diff --git a/RWR/src/ze_oss/ze_trajectory_analysis/include/ze/trajectory_analysis/kitti_evaluation.hpp b/RWR/src/ze_oss/ze_trajectory_analysis/include/ze/trajectory_analysis/kitti_evaluation.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..b5b75813389021e233d50c4eb49004b4f5ceaa90
--- /dev/null
+++ b/RWR/src/ze_oss/ze_trajectory_analysis/include/ze/trajectory_analysis/kitti_evaluation.hpp
@@ -0,0 +1,64 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#pragma once
+
+#include <ze/common/transformation.hpp>
+
+namespace ze {
+
+struct RelativeError
+{
+  size_t first_frame;
+  Vector3 W_t_gt_es;  //!< Relative translation error represented in world frame.
+  Vector3 W_R_gt_es;  //!< Relative rotation error (Angle-Axis) in world frame.
+  real_t len;
+  real_t scale_error;
+  int num_frames;
+
+  RelativeError(
+      size_t first_frame, Vector3 W_t_gt_es, Vector3 W_R_gt_es,
+      real_t segment_length, real_t scale_error,
+      int num_frames_in_between);
+};
+
+std::vector<real_t> trajectoryDistances(
+    const TransformationVector& poses);
+
+int32_t lastFrameFromSegmentLength(
+    const std::vector<real_t>& dist,
+    const size_t first_frame,
+    const real_t segment_length);
+
+std::vector<RelativeError> calcSequenceErrors(
+    const TransformationVector& poses_gt,
+    const TransformationVector& poses_es,
+    const real_t& segment_length,
+    const size_t skip_num_frames_between_segment_evaluation,
+    const bool use_least_squares_alignment,
+    const double least_squares_align_range,
+    const bool least_squares_align_translation_only);
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_trajectory_analysis/include/ze/trajectory_analysis/ts_match.hpp b/RWR/src/ze_oss/ze_trajectory_analysis/include/ze/trajectory_analysis/ts_match.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..cf60ebff679260de9d4ef354326ce9d0bb8f7272
--- /dev/null
+++ b/RWR/src/ze_oss/ze_trajectory_analysis/include/ze/trajectory_analysis/ts_match.hpp
@@ -0,0 +1,59 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#pragma once
+
+#include <cstdint>
+#include <Eigen/Core>
+
+#include <ze/common/time_conversions.hpp>
+#include <ze/common/buffer.hpp>
+
+namespace ze {
+
+template<typename BuffScalar, int BuffDim>
+bool findNearestTimeStamp(Buffer<BuffScalar, BuffDim>& in_buff,
+                          const int64_t& in_ts,
+                          int64_t& out_ts,
+                          Eigen::Matrix<BuffScalar, BuffDim, 1>& out_data,
+                          double max_diff_secs=0.02,
+                          double offset_secs=0.0)
+{
+  bool success;
+  int64_t offset_nsecs = ze::secToNanosec(offset_secs);
+  std::tie(out_ts, out_data, success) = in_buff.getNearestValue(in_ts+offset_nsecs);
+  if(!success)
+  {
+    return false;
+  }
+  int64_t max_diff_nsecs = ze::secToNanosec(max_diff_secs);
+  if(std::abs(out_ts - in_ts) > max_diff_nsecs)
+  {
+    return false;
+  }
+  else return true;
+}
+
+} // ze namespace
diff --git a/RWR/src/ze_oss/ze_trajectory_analysis/package.xml b/RWR/src/ze_oss/ze_trajectory_analysis/package.xml
new file mode 100644
index 0000000000000000000000000000000000000000..f4b07311361b9b5f790f9fd2501a44d654fd3bbe
--- /dev/null
+++ b/RWR/src/ze_oss/ze_trajectory_analysis/package.xml
@@ -0,0 +1,24 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>ze_trajectory_analysis</name>
+  <version>0.1.4</version>
+  <description>
+    Analyse trajectories: compute drift, RMS error, etc. and plot results.
+  </description>
+  <maintainer email="christian.forster@WyssZurich.ch">Christian Forster</maintainer>
+  <license>ZE</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <buildtool_depend>catkin_simple</buildtool_depend>
+  <depend>rospy</depend>
+  <depend>ze_cmake</depend>
+  <depend>ze_common</depend>
+  <depend>ze_geometry</depend>
+  <depend>eigen_catkin</depend>
+  <depend>glog_catkin</depend>
+  <depend>gflags_catkin</depend>
+
+  <test_depend>gtest</test_depend>
+
+  <export></export>
+</package>
diff --git a/RWR/src/ze_oss/ze_trajectory_analysis/py/ze_trajectory_analysis/__init__.py b/RWR/src/ze_oss/ze_trajectory_analysis/py/ze_trajectory_analysis/__init__.py
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/RWR/src/ze_oss/ze_trajectory_analysis/py/ze_trajectory_analysis/align.py b/RWR/src/ze_oss/ze_trajectory_analysis/py/ze_trajectory_analysis/align.py
new file mode 100644
index 0000000000000000000000000000000000000000..82af791862fecb94e20d0b58207eb604619d7fff
--- /dev/null
+++ b/RWR/src/ze_oss/ze_trajectory_analysis/py/ze_trajectory_analysis/align.py
@@ -0,0 +1,132 @@
+#!/usr/bin/python
+"""
+Zurich Eye
+"""
+
+import numpy as np
+
+def align_sim3(p_gt, p_es):
+    """Implementation of the paper: S. Umeyama, Least-Squares Estimation 
+    of Transformation Parameters Between Two Point Patterns,
+    IEEE Trans. Pattern Anal. Mach. Intell., vol. 13, no. 4, 1991.
+
+    Input:
+    p_gt -- first trajectory (nx3), numpy array type
+    p_es -- second trajectory (nx3), numpy array type
+    
+    Output:
+    s -- scale factor (scalar)
+    R_gt_es -- rotation matrix (3x3)
+    gt_t_gt_es -- translation vector (3x1)
+
+    p_es_aligned = s * np.transpose(np.dot(R_gt_es, np.transpose(p_es))) + gt_t_gt_es
+    """
+
+    # substract mean
+    mu_M = p_gt.mean(0)
+    mu_D = p_es.mean(0)
+    gt_zerocentered = p_gt - mu_M
+    es_zerocentered = p_es - mu_D
+    n = np.shape(p_gt)[0]
+
+    # correlation
+    C = 1.0/n*np.dot(gt_zerocentered.transpose(), es_zerocentered)
+    sigma2 = 1.0/n*np.multiply(es_zerocentered, es_zerocentered).sum()
+    U_svd,D_svd,V_svd = np.linalg.linalg.svd(C)
+    D_svd = np.diag(D_svd)
+    V_svd = np.transpose(V_svd)
+    S = np.eye(3)
+
+    if(np.linalg.det(U_svd)*np.linalg.det(V_svd) < 0):
+        S[2,2] = -1
+
+    R_gt_es = np.dot(U_svd, np.dot(S, np.transpose(V_svd)))
+    s = 1.0/sigma2 * np.trace(np.dot(D_svd, S))
+    gt_t_gt_es = mu_M - s * np.dot(R_gt_es, mu_D)
+    
+   
+    return s, R_gt_es, gt_t_gt_es
+
+def align_se3(p_gt, p_es):
+    """Align two trajectories using the method of Horn (closed-form). 
+        
+    Input:
+    model -- first trajectory (nx3), numpy array type
+    data -- second trajectory (nx3), numpy array type
+    
+    Output:
+    R_gt_es -- rotation matrix (3x3)
+    gt_t_gt_es -- translation vector (3x1)
+    """
+    np.set_printoptions(precision=3,suppress=True)
+    mu_M = p_gt.mean(0)
+    mu_D = p_es.mean(0)
+    gt_zerocentered = p_gt - mu_M
+    es_zerocentered = p_es - mu_D
+    
+    W = np.zeros( (3,3) )
+    for row in range(p_gt.shape[0]):
+        W += np.outer(gt_zerocentered[row,:], es_zerocentered[row,:])
+    U,d,Vh = np.linalg.linalg.svd(W.transpose())
+    S = np.eye(3)
+    if np.linalg.det(U) * np.linalg.det(Vh) < 0:
+        S[2,2] = -1
+    R = np.dot(U, np.dot(S, Vh))
+    t = mu_D - np.dot(R, mu_M)
+    
+    R_gt_es = np.transpose(R)
+    gt_t_gt_es = - np.dot(R_gt_es, t)
+
+    return R_gt_es, gt_t_gt_es
+
+def _matrix_log(A):
+    theta = np.arccos((np.trace(A)-1.0)/2.0)
+    log_theta = 0.5*theta/np.sin(theta) * (A - A.transpose())
+    x = np.array([log_theta[2,1], log_theta[0,2], log_theta[1,0]])
+    return x
+
+def hand_eye_calib(q_gt, q_es, I=[], p_gt=[], p_es=[], delta=1, verbose=True):
+    """Implementation of the least squares solution described in the paper:
+    Robot Sensor Calibration: Solving AX=XB on the Euclidean Group
+    by Frank C. Park and Bryan J. Martin
+    """
+    estimate_translation_offset = (len(p_es) != 0)
+    if len(I) == 0:
+        I = range(0,len(q_gt))
+    n = len(I)
+    M = np.zeros([3,3])
+    C = np.zeros([3*n, 3])
+    b_A = np.zeros([3*n,1])
+    b_B = np.zeros([3*n,1])
+    for ix, i in enumerate(I[0:-delta]):
+      A1 = ru.quat2dcm(q_es[i,:])
+      A2 = ru.quat2dcm(q_es[i+delta,:])
+      A  = np.dot(A1.transpose(), A2)
+      B1 = ru.quat2dcm(q_gt[i,:])
+      B2 = ru.quat2dcm(q_gt[i+delta,:])
+      B  = np.dot(B1.transpose(), B2)
+      alpha = _matrix_log(A)
+      beta = _matrix_log(B)
+      M = M + np.dot(np.matrix(beta).transpose(), np.matrix(alpha))
+      if estimate_translation_offset:
+          C[3*ix:3*ix+3,:] = np.eye(3) - A
+          b_A[3*ix:3*ix+3,0] = np.dot(np.transpose(A1), p_es[i+delta,:]-p_es[i,:])
+          b_B[3*ix:3*ix+3,0] = np.dot(np.transpose(B1), p_gt[i+delta,:]-p_gt[i,:])
+
+    # compute rotation  
+    D,V = np.linalg.linalg.eig(np.dot(M.transpose(), M))
+    Lambda = np.diag([np.sqrt(1.0/D[0]), np.sqrt(1.0/D[1]), np.sqrt(1.0/D[2])])
+    Vinv = np.linalg.linalg.inv(V)
+    X = np.dot(V, np.dot(Lambda, np.dot(Vinv, M.transpose())))
+
+    if estimate_translation_offset:
+        # compute translation
+        d = np.zeros([3*n,1])
+        for i in range(n):
+            d[3*i:3*i+3,:] = b_A[3*i:3*i+3,:] - np.dot(X, b_B[3*i:3*i+3,:])
+    
+        b = np.dot(np.linalg.inv(np.dot(np.transpose(C),C)),  np.dot(np.transpose(C),d))
+
+        return np.array(X), b
+    else:
+        return np.array(X)
\ No newline at end of file
diff --git a/RWR/src/ze_oss/ze_trajectory_analysis/py/ze_trajectory_analysis/align_test.py b/RWR/src/ze_oss/ze_trajectory_analysis/py/ze_trajectory_analysis/align_test.py
new file mode 100644
index 0000000000000000000000000000000000000000..eb48ea2b9b29e6ea527c15456bc586b9a3d49780
--- /dev/null
+++ b/RWR/src/ze_oss/ze_trajectory_analysis/py/ze_trajectory_analysis/align_test.py
@@ -0,0 +1,58 @@
+#!/usr/bin/env python3
+"""
+Zurich Eye
+"""
+
+import unittest
+import numpy as np
+import numpy.testing as npt
+import ze_py.transformations as tf
+import ze_trajectory_analysis.align as align
+
+class TestAlign(unittest.TestCase):
+    
+    def test_align_se3(self):
+        for i in range(100):
+            # Random data
+            n_points = 100
+            T_gt_es = tf.random_transformation()
+            T_es_gt = tf.inverse_matrix(T_gt_es)
+            p_gt = np.random.random((n_points,3))
+            p_es = np.transpose(np.dot(T_es_gt[:3,:3], np.transpose(p_gt))) + T_es_gt[:3,3]
+            
+            # Compute alignment
+            R_gt_es, gt_t_gt_es = align.align_se3(p_gt, p_es)
+            T = np.eye(4)
+            T[:3,:3] = R_gt_es
+            T[:3,3]  = gt_t_gt_es
+            npt.assert_array_almost_equal(T_gt_es, T)
+            
+            # Check alignment
+            p_es_aligned = np.transpose(np.dot(R_gt_es, np.transpose(p_es))) + gt_t_gt_es
+            npt.assert_array_almost_equal(p_es_aligned, p_gt)
+            
+    def test_align_sim3(self):
+        for i in range(100):
+            # Random data
+            n_points = 100
+            T_gt_es = tf.random_transformation()
+            s_inv = np.random.random()
+            T_es_gt = tf.inverse_matrix(T_gt_es)
+            p_gt = np.random.random((n_points,3))
+            p_es = s_inv * np.transpose(np.dot(T_es_gt[:3,:3], np.transpose(p_gt))) + s_inv * T_es_gt[:3,3]
+            
+            # Compute alignment
+            s, R_gt_es, gt_t_gt_es = align.align_sim3(p_gt, p_es)
+            T = np.eye(4)
+            T[:3,:3] = R_gt_es
+            T[:3,3]  = gt_t_gt_es
+            npt.assert_almost_equal(s, 1.0 / s_inv)
+            npt.assert_array_almost_equal(T_gt_es, T)
+            
+            # Check alignment
+            p_es_aligned = s * np.transpose(np.dot(R_gt_es, np.transpose(p_es))) + gt_t_gt_es
+            npt.assert_array_almost_equal(p_es_aligned, p_gt)
+   
+if __name__ == '__main__':
+    suite = unittest.TestLoader().loadTestsFromTestCase(TestAlign)
+    unittest.TextTestRunner(verbosity=2).run(suite)
\ No newline at end of file
diff --git a/RWR/src/ze_oss/ze_trajectory_analysis/py/ze_trajectory_analysis/analyse.py b/RWR/src/ze_oss/ze_trajectory_analysis/py/ze_trajectory_analysis/analyse.py
new file mode 100755
index 0000000000000000000000000000000000000000..0fdfd6d48aad45b1c6b106cac58d79dc6a2036a3
--- /dev/null
+++ b/RWR/src/ze_oss/ze_trajectory_analysis/py/ze_trajectory_analysis/analyse.py
@@ -0,0 +1,310 @@
+#!/usr/bin/env python3
+"""
+Zurich Eye
+"""
+
+import os
+import yaml
+import logging
+import argparse
+import numpy as np
+import numpy.testing as npt
+import ze_trajectory_analysis.align as align_trajectory
+import ze_trajectory_analysis.utils as utils
+import ze_trajectory_analysis.load as traj_loading
+import ze_trajectory_analysis.plot as traj_plot
+import ze_py.transformations as tf
+           
+class TrajectoryAnalysis:
+    
+    def __init__(self, result_dir, plot_size = 0.2):
+        """Analyse trajectory. 
+        
+        result_dir: The results of the analysis are saved at this location.
+        """
+        self.logger = logging.getLogger(__name__)
+        self.reset()
+        self.result_dir = utils.check_folder_exists(result_dir)
+        self.plot_size = plot_size
+        self.statistics_filename = os.path.join(self.result_dir, 'results.yaml')
+       
+    def reset(self):
+        self.data_loaded = False
+        self.data_aligned = False
+          
+    def load_data(self, data_dir, data_format='csv',
+                  filename_gt='traj_gt.csv', filename_es='traj_es.csv', 
+                  filename_matches='traj_es_gt_matches.csv', rematch_timestamps=True, 
+                  match_timestamps_offset=0.0, rematch_timestamps_max_difference_sec=0.02,
+                  discard_n_frames_at_start = 0):
+        """Loads the trajectory data.
+        
+        The resuls {p_es, q_es, p_gt, q_gt} is synchronized and has the same length.
+        
+        filename_matches:   Optional, if it does not exist, it is created.
+        rematch_timestamps: If True, finds matching timestamps between estimated
+                            and groundtruth trajectory. Saves the results to
+                            filename_matches.
+        """
+
+        # Load trajectory data
+        self.logger.info('Loading trajectory data..')
+        if data_format == 'csv':
+            utils.check_file_exists(os.path.join(data_dir, filename_gt))
+            utils.check_file_exists(os.path.join(data_dir, filename_es))
+            self.t_es, self.p_es, self.q_es, self.t_gt, self.p_gt, self.q_gt, self.matches_lut =\
+                traj_loading.load_dataset_csv(data_dir, filename_gt, filename_es,
+                                              filename_matches, rematch_timestamps,
+                                              match_timestamps_offset,
+                                              rematch_timestamps_max_difference_sec)
+        elif data_format == 'svo_gtsam':
+            self.t_es, self.p_es, self.q_es, self.t_gt, self.p_gt, self.q_gt =\
+                traj_loading.load_data_svo_gtsam(data_dir)
+        else:
+            raise ValueError('data_format' + self.data_format + ' not known.')
+        self.logger.info('...done.')
+        
+        # Distard frames at start:
+        if discard_n_frames_at_start > 0:
+            self.t_es = self.t_es[discard_n_frames_at_start:]
+            self.p_es = self.p_es[discard_n_frames_at_start:,:]
+            self.q_es = self.q_es[discard_n_frames_at_start:,:]
+            self.t_gt = self.t_gt[discard_n_frames_at_start:]
+            self.p_gt = self.p_gt[discard_n_frames_at_start:,:]
+            self.q_gt = self.q_gt[discard_n_frames_at_start:,:]
+        
+        # Compute distance along trajectory from start to each measurement.
+        self.distances = utils.get_distance_from_start(self.p_gt) 
+        traj_plot.plot_travelled_distance(self.distances, self.result_dir)
+        
+        self.data_dir = data_dir
+        self.data_loaded = True
+        
+    def apply_hand_eye_calibration_to_groundtruth(self):
+        if not self.data_loaded:
+            raise ValueError("You need to first load the data")  
+        self.logger.info('Apply hand-eye calibration to groundtruth data.')
+        self.T_B_V = traj_loading.load_hand_eye_calib_from_file(
+            os.path.join(self.result_dir, "dataset.yaml"))
+        self.T_V_B = tf.inverse_matrix(self.T_B_V)
+        n = np.shape(self.p_gt)[0]
+        for i in range(n):
+            T_W_V = tf.matrix_from_quaternion(self.q_gt[i,:])
+            T_W_V[:3,3] = self.p_gt[i,:]
+            T_W_B = np.dot(T_W_V, self.T_V_B)
+            self.q_gt[i,:] = tf.quaternion_from_matrix(T_W_B)
+            self.p_gt[i,:] = T_W_B[:3,3]
+        
+    def plot_estimator_results(self, data_dir, data_format='swe',
+                               filename = 'traj_es.csv', skip_frames = 1):
+        self.logger.info('Loading estimator data')
+        filename = utils.check_file_exists(os.path.join(data_dir, filename))
+        if not self.matches_lut:
+            raise ValueError("You need to first load the data")           
+        if data_format == 'swe':
+            self.estimator_ts, self.vel_es, self.bias_gyr_es, self.bias_acc_es = \
+                traj_loading.load_estimator_results(filename, self.matches_lut)
+        else:
+            self.logger.error("Estimator results format \""+data_format+"\" not known.")
+            return
+            
+        # Plot estimated biases
+        self.logger.info('Plotting estimator data')
+        traj_plot.plot_imu_biases(
+            self.estimator_ts, self.bias_gyr_es, self.bias_acc_es, self.result_dir)
+        traj_plot.plot_imu_state_along_trajectory( 
+            self.result_dir, self.bias_gyr_es[::skip_frames,:],
+            self.bias_acc_es[::skip_frames,:], self.vel_es[::skip_frames,:],
+            self.p_es[::skip_frames,:], self.plot_size)                          
+        
+               
+    def align_trajectory(self, align_type = 'se3', first_idx = 0, last_idx = -1):
+        """Align trajectory segment with ground-truth trajectory.
+        
+        first_idx: First index of data to align.
+        last_idx:  Last index of data to align.
+        align_type: 'se3' - translation and orientation
+                    'sim3' - translation, orientation, and scale
+                    'identity' - no alignment
+                    'first_frame' - align just the first frame
+        """
+        if not self.data_loaded:
+            raise ValueError("You need to first load the data")           
+
+        if last_idx < 0:
+            if first_idx == 0:
+                self.logger.info('Align trajectory using all frames.')
+            else:
+                self.logger.info('Align trajectory from index '+str(first_idx)+' to the end.')
+            last_idx = len(self.p_es)
+        else:
+            self.logger.info('Align trajectory from index ' + str(first_idx) + \
+                             ' to ' + str(first_idx) + '.')
+
+        # Compute alignment parameters
+        if align_type == 'sim3':
+            self.logger.info('Align Sim3 - rotation, translation and scale.')
+            self.scale, self.R_Wgt_Wes, self.t_Wgt_Wes = \
+                align_trajectory.align_sim3(self.p_gt[first_idx:last_idx,:],
+                                            self.p_es[first_idx:last_idx,:])
+        elif align_type == 'se3':
+            self.logger.info('Align SE3 - rotation and translation.')
+            self.R_Wgt_Wes, self.t_Wgt_Wes = \
+                align_trajectory.align_se3(self.p_gt[first_idx:last_idx,:],
+                                           self.p_es[first_idx:last_idx,:])
+            self.scale = 1.0 
+        elif align_type == 'identity':
+            self.logger.info('Align Identity.')
+            self.t_Wgt_Wes = np.zeros((3,))
+            self.R_Wgt_Wes = np.eye(3)            
+            self.scale = 1.0
+        elif align_type == 'first_frame':
+            self.logger.info('Align first frame.')
+            T_Wgt_B = tf.matrix_from_quaternion(self.q_gt[0,:])
+            T_Wgt_B[:3,3] = self.p_gt[0,:]
+            T_Wes_B = tf.matrix_from_quaternion(self.q_es[0,:])
+            T_Wes_B[:3,3] = self.p_es[0,:]
+            T_Wgt_Wes = np.dot(T_Wgt_B, tf.inverse_matrix(T_Wes_B))
+            self.t_Wgt_Wes = T_Wgt_Wes[:3,3]
+            self.R_Wgt_Wes = T_Wgt_Wes[:3,:3]   
+            self.scale = 1.0
+            
+        self.logger.info('Alignment translation t_Wgt_Wes: \n' + str(self.t_Wgt_Wes))
+        self.logger.info('Alignment rotation R_Wgt_Wes: \n' + str(self.R_Wgt_Wes))
+        self.logger.info('Alignment scale: \n' + str(self.scale))
+        npt.assert_almost_equal(np.linalg.det(self.R_Wgt_Wes), 1.0)
+            
+        self.logger.info('Apply alignment to estimated trajectory to fit groundtruth')
+        q = tf.quaternion_from_matrix(tf.convert_3x3_to_4x4(self.R_Wgt_Wes))
+        self.p_es_aligned = np.zeros(np.shape(self.p_es))
+        self.q_es_aligned = np.zeros(np.shape(self.q_es))
+        for i in range(np.shape(self.p_es)[0]):
+            self.p_es_aligned[i,:] = self.scale * np.dot(self.R_Wgt_Wes, self.p_es[i,:]) \
+                                     + self.t_Wgt_Wes
+            self.q_es_aligned[i,:] = tf.quaternion_multiply(q, self.q_es[i,:])
+        
+        self.align_first_idx = first_idx
+        self.align_last_idx = last_idx
+        self.data_aligned = True         
+            
+            
+    def plot_aligned_trajectory(self, plot_format = 'png'):
+        """Saves a plot of the aligned trajectory to 'result_dir'"""
+        
+        if not self.data_aligned:
+            raise ValueError("You need to first load and align the data")   
+            
+        self.logger.info('Save a plot of the aligned trajectory to: ' + self.result_dir)
+        traj_plot.plot_trajectory(self.result_dir, self.p_gt, self.p_es_aligned,
+                                  self.align_first_idx, self.align_last_idx)
+                                  
+    def compute_rms_errors(self):
+        """Compute Root Mean Square Error (RMSE) of aligned trajectory w.r.t
+        groundtruth trajectory.
+        """
+        
+        if not self.data_aligned:
+            raise ValueError("You need to first load and align the data")   
+        
+        # position error 
+        e_trans = (self.p_gt-self.p_es_aligned)  
+        e_trans_euclidean = np.sqrt(np.sum(e_trans**2, 1))
+        
+        self.logger.info('Compute orientation error')
+        e_rot = np.zeros((len(e_trans_euclidean,)))
+        e_rpy = np.zeros(np.shape(self.p_es_aligned))
+        for i in range(np.shape(self.p_es_aligned)[0]):
+            R_we = tf.matrix_from_quaternion(self.q_es_aligned[i,:])
+            R_wg = tf.matrix_from_quaternion(self.q_gt[i,:])
+            R_ge = np.dot(np.linalg.inv(R_wg), R_we)
+            e_rpy[i,:] = tf.euler_from_matrix(R_ge, 'rzyx')
+            e_rot[i] = np.linalg.norm(tf.logmap_so3(R_ge[:3,:3]))
+        
+        self.logger.info('Compute scale drift')
+        motion_gt = np.diff(self.p_gt, 0)
+        motion_es = np.diff(self.p_es_aligned, 0)
+        dist_gt = np.sqrt(np.sum(np.multiply(motion_gt,motion_gt),1))
+        dist_es = np.sqrt(np.sum(np.multiply(motion_es,motion_es),1))
+        e_scale_rel = np.divide(dist_es,dist_gt)-1.0
+
+        self.logger.info('Save error plots')
+        traj_plot.plot_translation_error(self.distances, e_trans, self.result_dir)
+        traj_plot.plot_rotation_error(self.distances, e_rpy, self.result_dir)
+        traj_plot.plot_scale_error(self.distances, e_scale_rel*100.0, self.result_dir)
+        
+        # compute error statistics:
+        self.compute_and_save_statistics(e_trans_euclidean, 'trans')
+        self.compute_and_save_statistics(e_rot, 'rot')
+        self.compute_and_save_statistics(e_scale_rel, 'scale')
+       
+    def get_trajectory_length(self):
+        assert(self.data_loaded)
+        return self.distances[-1]
+        
+    def compute_and_save_statistics(self, data_vec, label):
+        # Load statistics
+        stats = dict()
+        if os.path.exists(self.statistics_filename):
+            stats = yaml.load(open(self.statistics_filename,'r'))
+         
+        # Compute new statistics
+        if not 'trajectory_align' in stats:
+            stats['trajectory_align'] = dict()
+        stats['trajectory_align'][label] = dict()
+        stats['trajectory_align'][label]['rmse']   = float(np.sqrt(np.dot(data_vec,data_vec) / len(data_vec)))
+        stats['trajectory_align'][label]['mean']   = float(np.mean(data_vec))
+        stats['trajectory_align'][label]['median'] = float(np.median(data_vec))
+        stats['trajectory_align'][label]['std']    = float(np.std(data_vec))
+        stats['trajectory_align'][label]['min']    = float(np.min(data_vec))
+        stats['trajectory_align'][label]['max']    = float(np.max(data_vec))
+        stats['trajectory_align'][label]['num_samples'] = int(len(data_vec))
+        
+        # Save updated statistics
+        with open(self.statistics_filename,'w') as outfile:
+            outfile.write(yaml.dump(stats, default_flow_style=False))
+
+if __name__=='__main__':
+    
+    # parse command line
+    parser = argparse.ArgumentParser(description='Compute errors')
+    parser.add_argument('--data_dir', help='folder with the results',
+                        default='')
+    parser.add_argument('--format_gt', help='format groundtruth {swe,pose,euroc}',
+                        default='pose')
+    parser.add_argument('--format_es', help='format estimate {swe,pose,euroc}',
+                        default='pose')
+
+    parser.add_argument('--alignment', help='trajectory alignment {se3, sim3, first_frame}',
+                        default='se3')
+    parser.add_argument('--alignment_first_frame', help='first frame to align', default=0)
+    parser.add_argument('--alignment_last_frame', help='las frame to align', default=-1)
+    parser.add_argument('--plot_size', default=0.2, help='size of circle')
+    parser.add_argument('--skip_frames', default=1,
+                        help='frames skipped between segment evaluation')
+
+    parser.add_argument('--match_ts_offset', 
+                        help='offset between experiment and GT timestamps (in seconds)',
+                        default=0.0)
+                        
+    options = parser.parse_args()    
+    
+    logging.basicConfig(level=logging.DEBUG)
+    logger = logging.getLogger(__name__)
+    logger.info('Compute relative errors')
+
+    if options.data_dir:
+        ta = TrajectoryAnalysis(result_dir = options.data_dir,
+                                plot_size = float(options.plot_size))
+        
+        ta.load_data(data_dir=options.data_dir,
+                     data_format='csv', 
+                     match_timestamps_offset=options.match_ts_offset)
+                     
+        ta.align_trajectory(options.alignment, int(options.alignment_first_frame),
+                            int(options.alignment_last_frame))
+        ta.plot_aligned_trajectory()
+        ta.compute_rms_errors()
+        ta.plot_estimator_results(options.data_dir,
+                                  data_format = options.format_es,
+                                  filename = 'traj_es.csv',
+                                  skip_frames = int(options.skip_frames))
diff --git a/RWR/src/ze_oss/ze_trajectory_analysis/py/ze_trajectory_analysis/compare.py b/RWR/src/ze_oss/ze_trajectory_analysis/py/ze_trajectory_analysis/compare.py
new file mode 100755
index 0000000000000000000000000000000000000000..e0e8ffb6589255f82a4456de85bb15095766d971
--- /dev/null
+++ b/RWR/src/ze_oss/ze_trajectory_analysis/py/ze_trajectory_analysis/compare.py
@@ -0,0 +1,256 @@
+#!/usr/bin/python
+"""
+Zurich Eye
+"""
+
+import os
+import yaml
+import numpy as np
+import argparse
+import rospkg
+from tabulate import tabulate
+import matplotlib.pyplot as plt
+import seaborn.apionly as sns # https://stanford.edu/~mwaskom/software/seaborn/tutorial/color_palettes.html
+from jinja2 import Environment, FileSystemLoader # http://pbpython.com/pdf-reports.html
+from weasyprint import HTML
+from matplotlib import rc
+rc('font',**{'family':'serif','serif':['Cardo']})
+rc('text', usetex=True)
+
+FORMAT='.pdf'
+
+def print_tables_to_pdf(comparison_dir, tables, title):
+    """
+    tables must be a list of dictionaries that contain 'title' and 'data'
+    """
+    # print to file
+    template_dir = os.path.join(rospkg.RosPack().get_path('svo_analysis'), 
+                                'src/svo_analysis/templates')
+    env = Environment(loader=FileSystemLoader(template_dir))
+    template = env.get_template("report.html")
+    template_vars = dict()
+    template_vars['title'] = title
+    template_vars['tables'] = tables
+    html_out = template.render(template_vars)
+    HTML(string=html_out).write_pdf(os.path.join(comparison_dir, 'report.pdf'))
+
+
+def compute_average_table(table):
+    """
+    the first column in the table always contains the label
+    """
+    
+    # find unique labels
+    summarized_table = dict() # label -> list of data
+    for row in table:
+        label = row[0]
+        data = row[1:]
+        if label not in summarized_table:
+            summarized_table[label] = list()
+        summarized_table[label].append(data)
+        
+    averaged_table = list()
+    for key, data in summarized_table.iteritems():
+        data = np.array(data)
+        mean = np.mean(data,0)
+        std  = np.std(data,0)
+        new_data = [key]
+        for i in range(len(mean)):
+            new_data.append('{:.4f} +/- {:.4f}'.format(mean[i], std[i]))
+        averaged_table.append(new_data)
+            
+    return averaged_table
+        
+    
+def statistics_table(label, comparison_dir, trace_dirs):
+    
+    header = ['', label+' Mean', label+' Median', label+' RMSE', 'Sdt', '# Samples']
+    table = list()
+    for trace_dir in trace_dirs:
+        params_filename = os.path.join(trace_dir, 'params.yaml')
+        stats_filename  = os.path.join(trace_dir, 'analysis_statistics.yaml')
+        assert os.path.exists(params_filename)
+        assert os.path.exists(stats_filename)
+        params = yaml.load(open(params_filename,'r'))
+        
+        if not os.path.exists(stats_filename):
+            print('ERROR: ' + stats_filename + ' does not exist!')
+            continue
+        
+        stats = yaml.load(open(stats_filename,'r'))
+
+        if label not in stats:
+            print('ERROR: ' +label+ ' not in '+trace_dir)
+            continue
+        if 'mean' not in stats[label]:
+            print('ERROR: ' +label+ ' in '+trace_dir + ' has no data!')
+            continue
+        
+        data = [params['experiment_label'],
+                stats[label]['mean'],
+                stats[label]['median'],
+                stats[label]['rmse'],
+                stats[label]['std'],
+                stats[label]['num_samples']]
+            
+        table.append(data)
+    print(tabulate(table, headers=header))
+    
+    trans_table_avg = compute_average_table(table)
+    print(tabulate(trans_table_avg, headers=header))
+
+    return tabulate(table, header, tablefmt='html'), tabulate(trans_table_avg, header, tablefmt='html')
+    
+def print_table(comparison_dir, trace_dirs):
+
+    tables = list()
+
+    # Translation
+    trans_table, trans_table_avg = statistics_table('trans', comparison_dir, trace_dirs)
+    tables.append({'title': 'Translation Error - Averaged', 'data': trans_table_avg})
+    tables.append({'title': 'Translation Error', 'data': trans_table})  
+    
+    # Timing
+    timing_table, timing_table_avg = statistics_table('timing_ms', comparison_dir, trace_dirs)
+    tables.append({'title': 'Timing - Averaged', 'data': timing_table_avg})
+    tables.append({'title': 'Timing', 'data': timing_table})  
+    
+    # CPU
+    cpu_table, cpu_table_avg = statistics_table('cpu_usage', comparison_dir, trace_dirs)
+    tables.append({'title': 'CPU - Averaged', 'data': cpu_table_avg})
+    #tables.append({'title': 'CPU', 'data': cpu_table})  
+    
+    # Memory
+    mem_table, mem_table_avg = statistics_table('memory_usage', comparison_dir, trace_dirs)
+    tables.append({'title': 'Memory - Averaged', 'data': mem_table_avg})
+    #tables.append({'title': 'Memory', 'data': mem_table})  
+                        
+    print_tables_to_pdf(comparison_dir, tables, 'Report')
+
+
+
+    
+def set_colors(ax, n):
+    ax.set_color_cycle(sns.color_palette("deep", n))    
+    
+def plot_trajectory(trace_dir, experiments):
+    fig_traj_top = plt.figure(figsize=(8,8))
+    fig_traj_side = plt.figure(figsize=(8,3))
+    ax_traj_top = fig_traj_top.add_subplot(111, aspect='equal', xlabel='x [m]', ylabel='y [m]')
+    ax_traj_side = fig_traj_side.add_subplot(111, aspect='equal', xlabel='x [m]', ylabel='z [m]')
+    set_colors(ax_traj_top, len(experiments))    
+    set_colors(ax_traj_side, len(experiments))    
+    ax_traj_top.xaxis.grid(ls='--', color='0.7')    
+    ax_traj_top.yaxis.grid(ls='--', color='0.7')
+    ax_traj_side.xaxis.grid(ls='--', color='0.7')    
+    ax_traj_side.yaxis.grid(ls='--', color='0.7')
+    
+    for i, exp in enumerate(experiments):
+        D = yaml.load(open(os.path.join(exp, 'job.yaml'), 'r'))
+        data = np.genfromtxt(os.path.join(exp, 'trajectory.csv'), delimiter=',', skip_header=1, dtype=np.float64)
+        ax_traj_top.plot(data[:,0], data[:,1], linestyle='-', label=D['experiment_name'].replace("_", ""))
+        ax_traj_side.plot(data[:,0], data[:,2], linestyle='-')
+        #if i == 0:
+        #    ax_traj_top.plot(data[:,3], data[:,4], color='black', linestyle='-', label='Groundtruth')
+        #    ax_traj_side.plot(data[:,3], data[:,5], color='black', linestyle='-')
+        
+    fig_traj_top.tight_layout()
+    fig_traj_side.tight_layout()
+    ax_traj_top.legend()
+    fig_traj_top.savefig(os.path.join(trace_dir, 'trajectory_comparison_top'+FORMAT), bbox_inches="tight")
+    fig_traj_side.savefig(os.path.join(trace_dir, 'trajectory_comparison_side'+FORMAT), bbox_inches="tight")
+    
+    
+def plot_translation_errors(trace_dir, experiments):
+    fig_trans_error = plt.figure(figsize=(6,5))
+    ax_trans_error_x = fig_trans_error.add_subplot(311, xlabel='Distance [m]', ylabel='x-Error [m]')
+    ax_trans_error_y = fig_trans_error.add_subplot(312, xlabel='Distance [m]', ylabel='y-Error [m]')
+    ax_trans_error_z = fig_trans_error.add_subplot(313, xlabel='Distance [m]', ylabel='z-Error [m]')
+    ax_trans_error_x.yaxis.grid(ls='--', color='0.7')
+    ax_trans_error_y.yaxis.grid(ls='--', color='0.7')
+    ax_trans_error_z.yaxis.grid(ls='--', color='0.7')
+    set_colors(ax_trans_error_x, len(experiments))    
+    set_colors(ax_trans_error_y, len(experiments))    
+    set_colors(ax_trans_error_z, len(experiments))    
+    
+    for i, exp in enumerate(experiments):
+        D = yaml.load(open(os.path.join(exp, 'job.yaml'), 'r'))
+        data = np.genfromtxt(os.path.join(exp, 'translation_error.csv'), delimiter=',', skip_header=1, dtype=np.float64)
+        ax_trans_error_x.plot(data[:,0], data[:,1], linestyle='-', label='r'+D['experiment_name'].replace("_", ""))
+        ax_trans_error_y.plot(data[:,0], data[:,2], linestyle='-', label='r'+D['experiment_name'].replace("_", ""))
+        ax_trans_error_z.plot(data[:,0], data[:,3], linestyle='-', label='r'+D['experiment_name'].replace("_", ""))
+        
+    fig_trans_error.tight_layout()
+    ax_trans_error_x.legend()
+    fig_trans_error.savefig(os.path.join(trace_dir, 'translation_error'+FORMAT), bbox_inches="tight")
+    
+def plot_relative_errors(trace_dir, experiments):
+    fig_relative_errors = plt.figure(figsize=(6,5))
+    ax_rel_1 = fig_relative_errors.add_subplot(311, xlabel='Measurement', ylabel='Translation-Error [m/s]')
+    ax_rel_2 = fig_relative_errors.add_subplot(312, xlabel='Measurement', ylabel='Yaw-Error [deg/s]')
+    ax_rel_3 = fig_relative_errors.add_subplot(313, xlabel='Measurement', ylabel='Roll/Pitch-Error [deg/s]')
+    ax_rel_1.yaxis.grid(ls='--', color='0.7')
+    ax_rel_2.yaxis.grid(ls='--', color='0.7')
+    ax_rel_3.yaxis.grid(ls='--', color='0.7')
+    set_colors(ax_rel_1, len(experiments)) 
+    set_colors(ax_rel_2, len(experiments)) 
+    set_colors(ax_rel_3, len(experiments)) 
+    
+    for i, exp in enumerate(experiments):
+        D = yaml.load(open(os.path.join(exp, 'job.yaml'), 'r'))
+        data = np.genfromtxt(os.path.join(exp, 'traj_relative_errors_10.csv'), delimiter=',', skip_header=2, dtype=np.float64)
+        ax_rel_1.plot(data[:,0], linestyle='-', label=D['experiment_name'].replace("_", ""))
+        ax_rel_2.plot(data[:,2], linestyle='-', label=D['experiment_name'].replace("_", ""))
+        ax_rel_3.plot(data[:,3], linestyle='-', label=D['experiment_name'].replace("_", ""))
+        
+    fig_relative_errors.tight_layout()
+    ax_rel_1.legend()
+    fig_relative_errors.savefig(os.path.join(trace_dir, 'relative_errors'+FORMAT), bbox_inches="tight")
+    
+    
+def plot_rms_errors(trace_dir, experiments):
+    fig_relative_errors = plt.figure(figsize=(6,4))
+    ax_rms_1 = fig_relative_errors.add_subplot(211, ylabel='Pos. RMSE [m]')
+    ax_rms_2 = fig_relative_errors.add_subplot(212, xlabel='Distance [m]', ylabel='Rot. RMSE [deg]')
+    ax_rms_1.yaxis.grid(ls='--', color='0.7')
+    ax_rms_2.yaxis.grid(ls='--', color='0.7')
+    set_colors(ax_rms_1, len(experiments)) 
+    set_colors(ax_rms_2, len(experiments)) 
+    max_x = 0
+    for i, exp in enumerate(experiments):
+        D = yaml.load(open(os.path.join(exp, 'job.yaml'), 'r'))
+        data_trans = np.genfromtxt(os.path.join(exp, 'translation_error.csv'), delimiter=',', skip_header=1, dtype=np.float64)
+        data_rot = np.genfromtxt(os.path.join(exp, 'orientation_error.csv'), delimiter=',', skip_header=1, dtype=np.float64)
+        rms_trans = np.sqrt(np.sum(data_trans[:,1:4]**2,1))
+        rms_rot   = np.sqrt(np.sum(data_rot[:,1:4]**2,1))
+        ax_rms_1.plot(data_trans[:,0], rms_trans, linestyle='-', label=D['experiment_name'].replace("_", ""))
+        ax_rms_2.plot(data_trans[:,0], rms_rot,   linestyle='-')
+        max_x = max(max_x, data_trans[-1,0])
+    fig_relative_errors.tight_layout()
+    #ax_rms_1.legend(loc='upper left')
+    ax_rms_1.legend(bbox_to_anchor=(0., 1.02, 1., .102), loc=3,
+                    ncol=1, mode="expand", borderaxespad=0.)
+    ax_rms_1.set_xlim([0, max_x])
+    ax_rms_2.set_xlim([0, max_x])
+    fig_relative_errors.savefig(os.path.join(trace_dir, 'rms_errors'+FORMAT), bbox_inches="tight")
+    
+if __name__=='__main__':
+    
+    # parse command line
+    parser = argparse.ArgumentParser(description='''
+    Analyse trajectory
+    ''')
+    parser.add_argument('--trace_dir', help='folder with the results', default='')
+    parser.add_argument('--reference', help='', type=str, default="")
+    args = parser.parse_args()
+    
+    ###############################################################################    
+    
+    trace_dir = args.trace_dir
+    experiments = [experiment for experiment in args.reference.split(',')]
+    experiments.insert(-1, args.trace_dir)
+
+    plot_rms_errors(trace_dir, experiments)
+    plot_relative_errors(trace_dir, experiments)
+    plot_translation_errors(trace_dir, experiments)
+    plot_trajectory(trace_dir, experiments)
\ No newline at end of file
diff --git a/RWR/src/ze_oss/ze_trajectory_analysis/py/ze_trajectory_analysis/consistency_single_run.py b/RWR/src/ze_oss/ze_trajectory_analysis/py/ze_trajectory_analysis/consistency_single_run.py
new file mode 100644
index 0000000000000000000000000000000000000000..1c3bdca0bbd1a7ab4b6286aab45327684c4f6227
--- /dev/null
+++ b/RWR/src/ze_oss/ze_trajectory_analysis/py/ze_trajectory_analysis/consistency_single_run.py
@@ -0,0 +1,191 @@
+#!/usr/bin/python3
+"""
+Zurich Eye
+"""
+
+import os
+import yaml
+import logging
+import argparse
+import numpy as np
+import ze_trajectory_analysis.analyse as traj_analysis
+import ze_py.transformations as tf
+import matplotlib.pyplot as plt
+import matplotlib.gridspec as gridspec
+import vikit_py.transformations as tf
+from matplotlib import rc
+from matplotlib.ticker import FuncFormatter
+rc('font',**{'family':'serif','serif':['Cardo']})
+rc('text', usetex=True)
+
+_EPS = np.finfo(float).eps * 4.0
+FORMAT = '.pdf'
+
+# Init logging.
+logging.basicConfig(level=logging.DEBUG)
+logger = logging.getLogger(__name__)
+logger.info('Trajectory alignment example.')
+
+# Load Data
+data_dir = '/home/cfo/vin_ws/src/svo_gtsam/trace/data/20160407_1459_gtsam_vicon_asl_140313_vicon_aslam_2'
+
+ta = traj_analysis.TrajectoryAnalysis(data_dir)
+ta.load_data(data_dir, data_format='svo_gtsam')
+ta.apply_hand_eye_calibration_to_groundtruth()
+ta.align_trajectory('first_frame')
+#ta.align_trajectory(align_type='se3', first_idx=0, last_idx=100)
+ta.plot_aligned_trajectory()
+#ta.compute_rms_errors()
+
+t_es = ta.t_es
+p_es = ta.p_es_aligned
+q_es = ta.q_es_aligned
+p_gt = ta.p_gt
+q_gt = ta.q_gt
+distances = ta.distances
+
+# Load covariances:
+cov_data = np.genfromtxt(os.path.join(data_dir, 'estimate_covariance.csv'), delimiter=',')
+t_cov = cov_data[:,0]
+
+# Load covariance
+n = len(cov_data)
+yaw_sigma_3 = np.zeros(n)
+yaw_error = np.zeros(n)
+roll_sigma_3 = np.zeros(n)
+roll_error = np.zeros(n)
+pitch_sigma_3 = np.zeros(n)
+pitch_error = np.zeros(n)
+error_pos_W = np.zeros((3,n))
+error_pos_W_sigma_3 = np.zeros((3,n))
+nees_rot = np.zeros(n)
+nees_pos = np.zeros(n)
+nees_se3 = np.zeros(n)
+for i in range(1000):
+    #assert t_cov[i] == t_es[i]
+    Cov_T_B = np.reshape(cov_data[i,1:],(6,6))
+    Cov_R_B = Cov_T_B[:3,:3]
+    Cov_t_B = Cov_T_B[3:6,3:6]        
+    p_W_Bes = p_es[i,:]
+    p_W_Bgt = p_gt[i,:]
+    R_W_Bes = tf.quaternion_matrix(q_es[i,:])[:3,:3]
+    R_W_Bgt = tf.quaternion_matrix(q_gt[i,:])[:3,:3]
+    Cov_R_W = np.dot(R_W_Bes, np.dot(Cov_R_B, np.transpose(R_W_Bes)))
+    Cov_T_W = np.dot(R_W_Bes, np.dot(Cov_t_B, np.transpose(R_W_Bes)))
+    yaw_sigma_3[i] = np.sqrt(Cov_R_W[2,2])*3.0*180/np.pi
+    pitch_sigma_3[i] = np.sqrt(Cov_R_W[1,1])*3.0*180/np.pi
+    roll_sigma_3[i] = np.sqrt(Cov_R_W[0,0])*3.0*180/np.pi
+    R_Bgt_Bes = np.dot(R_W_Bgt, np.transpose(R_W_Bes))
+    
+    yaw_error[i], pitch_error[i], roll_error[i] = tf.euler_from_matrix(R_Bgt_Bes, 'rzyx')
+    
+    # compute normalized estimation error squared (in estimated body frame)
+    error_rot_B = tf.logmap_so3(np.transpose(R_Bgt_Bes))
+    error_pos_B = np.dot(np.transpose(R_W_Bes), (p_W_Bgt - p_W_Bes))
+    error_se3_B = np.concatenate((error_rot_B, error_pos_B))
+    nees_rot[i] = np.dot(error_rot_B, np.dot(np.linalg.inv(Cov_R_B), error_rot_B))        
+    nees_pos[i] = np.dot(error_pos_B, np.dot(np.linalg.inv(Cov_t_B), error_pos_B))
+    nees_se3[i] = np.dot(error_se3_B, np.dot(np.linalg.inv(Cov_T_B), error_se3_B))
+
+    # translation error in world coordiantes
+    error_pos_W[:,i] = p_W_Bgt - p_W_Bes
+    error_pos_W_sigma_3[0,i] = np.sqrt(Cov_T_W[0,0])*3.0
+    error_pos_W_sigma_3[1,i] = np.sqrt(Cov_T_W[1,1])*3.0
+    error_pos_W_sigma_3[2,i] = np.sqrt(Cov_T_W[2,2])*3.0
+    
+    
+yaw_error *= 180/np.pi
+pitch_error *= 180/np.pi
+roll_error *= 180/np.pi
+n_max = 1000
+
+# rotation error
+D = distances[:n_max]
+y_lim = 5 #args.rpy_ylim
+fig = plt.figure(figsize=(6,8))
+gs1 = gridspec.GridSpec(3, 1)
+gs1.update(wspace=0.005) # set the spacing between axes.
+ax = fig.add_subplot(611, ylabel='Err. Yaw [deg]')
+ax.locator_params(axis = 'y', nbins = 4)
+ax.yaxis.set_major_formatter(FuncFormatter(lambda y, pos: '%.2f'%y))
+ax.plot(D, yaw_sigma_3[:n_max], 'r-', alpha=0.5, lw=0.7)
+ax.plot(D, -yaw_sigma_3[:n_max], 'r-', alpha=0.5, lw=0.7)
+ax.plot(D, yaw_error[:n_max], 'r-', lw=1)
+ax.set_xticks([])
+ax.set_ylim([-y_lim,y_lim])
+y_lim = 4 #args.rpy_ylim
+ax = fig.add_subplot(612, ylabel='Err. Pitch [deg]')
+ax.locator_params(axis = 'y', nbins = 4)
+ax.yaxis.set_major_formatter(FuncFormatter(lambda y, pos: '%.2f'%y))
+ax.plot(D, pitch_sigma_3[:n_max], 'g-', alpha=0.5, lw=0.7)
+ax.plot(D, -pitch_sigma_3[:n_max], 'g-', alpha=0.5, lw=0.7)
+ax.plot(D, pitch_error[:n_max], 'g-', lw=1)
+ax.set_xticks([])
+ax.set_ylim([-y_lim,y_lim])
+ax = fig.add_subplot(613, ylabel='Err. Roll [deg]')
+ax.locator_params(axis = 'y', nbins = 4)
+ax.yaxis.set_major_formatter(FuncFormatter(lambda y, pos: '%.2f'%y))
+ax.plot(D, roll_sigma_3[:n_max], 'b-', alpha=0.5, lw=0.7)
+ax.plot(D, -roll_sigma_3[:n_max], 'b-', alpha=0.5, lw=0.7)
+ax.plot(D, roll_error[:n_max], 'b-', lw=1)
+ax.set_ylim([-y_lim,y_lim])
+ax.set_xticks([])
+
+# translation error
+y_lim = 0.9
+ax = fig.add_subplot(614, ylabel='Err. x [m]')
+ax.locator_params(axis = 'y', nbins = 4)
+ax.yaxis.set_major_formatter(FuncFormatter(lambda y, pos: '%.2f'%y))
+ax.plot(D, error_pos_W_sigma_3[0,:n_max], 'r-', alpha=0.5, lw=0.7)
+ax.plot(D, -error_pos_W_sigma_3[0,:n_max], 'r-', alpha=0.5, lw=0.7)
+ax.plot(D, error_pos_W[0,:n_max], 'r-', lw=1)
+ax.set_xticks([])
+ax.set_ylim([-y_lim,y_lim])
+ax = fig.add_subplot(615, ylabel='Err. y [m]')
+ax.locator_params(axis = 'y', nbins = 4)
+ax.yaxis.set_major_formatter(FuncFormatter(lambda y, pos: '%.2f'%y))
+ax.plot(D, error_pos_W_sigma_3[1,:n_max], 'g-', alpha=0.5, lw=0.7)
+ax.plot(D, -error_pos_W_sigma_3[1,:n_max], 'g-', alpha=0.5, lw=0.7)
+ax.plot(D, error_pos_W[1,:n_max], 'g-', lw=1)    
+ax.set_ylim([-y_lim,y_lim])
+ax.set_xticks([])
+ax = fig.add_subplot(616, xlabel='Distance Travelled [m]', ylabel='Err. z [m]')
+ax.locator_params(axis = 'y', nbins = 4)
+ax.yaxis.set_major_formatter(FuncFormatter(lambda y, pos: '%.2f'%y))
+ax.plot(D, error_pos_W_sigma_3[2,:n_max], 'b-', alpha=0.5, lw=0.7)
+ax.plot(D, -error_pos_W_sigma_3[2,:n_max], 'b-', alpha=0.5, lw=0.7)
+ax.plot(D, error_pos_W[2,:n_max], 'b-', lw=1)
+ax.set_ylim([-y_lim,y_lim])
+ax.tick_params('x',top='off')
+fig.tight_layout()
+fig.savefig(os.path.join(data_dir,'consistency_single_run'+FORMAT), bbox_inches="tight")
+
+# write to file
+file_out = open(os.path.join(data_dir, 'consistency_errors.csv'), 'w')
+file_out.write('# trans_error -x, -y, -z, rot_error -yaw, -pitch, -roll\n')
+for i in range(len(yaw_error)):
+    file_out.write(
+        '%.8f, %.8f, %.8f, %.8f, %.8f, %.8f\n' %
+        (error_pos_W[0,i], error_pos_W[1,i], error_pos_W[2,i], yaw_error[i], pitch_error[i], roll_error[i])) 
+file_out.close()
+
+# NEES Rot and Pos
+fig = plt.figure(figsize=(6,3))
+ax = fig.add_subplot(211, ylabel='Rot. NEES')
+ax.plot(nees_rot)
+ax = fig.add_subplot(212, ylabel='Pos. NEES', xlabel='Keyframes')
+ax.plot(nees_pos)
+fig.savefig(os.path.join(data_dir,'consistency_nees_posrot'+FORMAT), bbox_inches="tight")
+
+# NEES Pose
+fig = plt.figure(figsize=(6,1.5))
+ax = fig.add_subplot(111, ylabel='Pose NEES', xlabel='Keyframes')
+ax.plot(nees_se3)
+fig.savefig(os.path.join(data_dir,'consistency_pose'+FORMAT), bbox_inches="tight")
+
+# write to file
+file_out = open(os.path.join(data_dir, 'consistency_nees.csv'), 'w')
+file_out.write('# NEES orientation, NEES position \n')
+for i in range(len(nees_rot)):
+    file_out.write('%.8f, %.8f, %.8f\n' % (nees_rot[i], nees_pos[i], nees_se3[i])) 
+file_out.close()
\ No newline at end of file
diff --git a/RWR/src/ze_oss/ze_trajectory_analysis/py/ze_trajectory_analysis/load.py b/RWR/src/ze_oss/ze_trajectory_analysis/py/ze_trajectory_analysis/load.py
new file mode 100755
index 0000000000000000000000000000000000000000..c79f62eb8ba453030f96b2e7721df01bfa601599
--- /dev/null
+++ b/RWR/src/ze_oss/ze_trajectory_analysis/py/ze_trajectory_analysis/load.py
@@ -0,0 +1,238 @@
+#!/usr/bin/python3
+"""
+Zurich Eye
+"""
+
+import os
+import logging
+import yaml
+import numpy as np
+import ze_trajectory_analysis.utils as utils
+
+# -----------------------------------------------------------------------------
+# Dataset loading
+  
+def load_dataset_csv(data_dir, filename_gt = 'traj_gt.csv', filename_es = 'traj_es.csv',
+                     filename_matches = 'traj_es_gt_matches.csv', rematch_timestamps = False,
+                     match_timestamps_offset = 0.0, match_timestamps_max_difference_sec = 0.02):
+    logger = logging.getLogger(__name__)
+    
+    # generate or load matches:
+    if not os.path.exists(os.path.join(data_dir, filename_matches)) or rematch_timestamps:
+        logger.info("Find timestamp matches.")
+        match_command = "rosrun ze_trajectory_analysis match_stamps" \
+                        + " --data_dir=" + data_dir \
+                        + " --filename_es=" + filename_es \
+                        + " --filename_gt=" + filename_gt \
+                        + " --filename_matches=" + filename_matches \
+                        + " --offset_sec=" + str(match_timestamps_offset) \
+                        + " --max_difference_sec=" + str(match_timestamps_max_difference_sec)
+        logger.info("Executing command: "+match_command)
+        os.system(match_command)
+                            
+    # load data:
+    filename_gt = utils.check_file_exists(os.path.join(data_dir, filename_gt))
+    filename_es = utils.check_file_exists(os.path.join(data_dir, filename_es))
+    filename_matches = os.path.join(data_dir, filename_matches)
+    keys_gt = utils.read_nanosecond_timestamps_from_csv_file(filename_gt, 0, ',')        
+    keys_es = utils.read_nanosecond_timestamps_from_csv_file(filename_es, 0, ',')
+    data_es = np.genfromtxt(filename_es, delimiter=',', dtype=np.float64, skip_header=1)[:,1:]
+    data_gt = np.genfromtxt(filename_gt, delimiter=',', dtype=np.float64, skip_header=1)[:,1:]
+    matches = np.genfromtxt(filename_matches, dtype=np.int64, delimiter=',', skip_header=1)
+    
+    # Create look-up table { es -> gt }
+    matches_lut = dict([(row[0], row[1]) for row in matches])
+    
+    groundtruth = dict([ (keys_gt[i], data_gt[i,:]) for i in range(len(keys_gt))])
+    
+    p_es = []
+    p_gt = []
+    q_es = []
+    q_gt = []
+    t_gt = []
+    t_es = []
+    for i in range(len(keys_es)):
+        if keys_es[i] in matches_lut:
+            t_es_val = keys_es[i]
+            t_gt_val = matches_lut[t_es_val]
+            gt_data = groundtruth[t_gt_val]
+            p_gt.append(gt_data[:3])
+            tmp = gt_data[3:7] # quaternion order x y z w
+            q_gt.append([tmp[1], tmp[2], tmp[3], tmp[0]])            
+            p_es.append(data_es[i,0:3])
+            q_es.append(data_es[i,3:7])
+            t_es.append(t_es_val)
+            t_gt.append(t_gt_val)
+    p_es = np.array(p_es)
+    p_gt = np.array(p_gt)
+    q_es = np.array(q_es)
+    q_gt = np.array(q_gt)
+    t_gt = np.array(t_gt)
+    t_es = np.array(t_es)
+
+    return t_es, p_es, q_es, t_gt, p_gt, q_gt, matches_lut 
+    
+
+def load_estimator_results(filename, matches_lut):
+    data = np.genfromtxt(filename, delimiter=',', dtype=np.float64, skip_header=1)[:,1:]
+    keys_es = utils.read_nanosecond_timestamps_from_csv_file(filename, 0, ',')
+    
+    velocity = []
+    bias_gyr = []
+    bias_acc = [] 
+    stamps = []
+    for i in range(len(keys_es)):
+        if keys_es[i] in matches_lut:
+            velocity.append(data[i,7:10])
+            bias_gyr.append(data[i,10:13])
+            bias_acc.append(data[i,13:17])
+            stamps.append(keys_es[i])
+            
+    velocity = np.array(velocity)
+    bias_gyr = np.array(bias_gyr)
+    bias_acc = np.array(bias_acc)
+    stamps = np.array(stamps)
+    return stamps, velocity, bias_gyr, bias_acc
+        
+
+def load_relative_errors_from_file(data_dir, segment_length,
+                                   filename_result_prefix = 'traj_relative_errors'):
+                                       
+    data = np.genfromtxt(os.path.join(data_dir, filename_result_prefix+'_'+str(segment_length)+'.csv'),
+                         delimiter=',', dtype=np.float64, skip_header=1)
+    assert data[0, 7] == segment_length
+    rel_pos_errors = np.abs(data[:,1:4]) # / segment_length
+    rel_rot_errors = np.abs(data[:,4:7]) # / segment_length
+
+    rel_pos_errors_norm = np.sqrt(np.sum(rel_pos_errors**2, 1))
+    rel_roll_pitch_errors = np.sqrt(np.sum(rel_rot_errors[:,0:2]**2, 1))
+    rel_yaw_errors = rel_rot_errors[:,2]
+    
+    start_indices = data[:,0].astype(int)
+    scale_errors = data[:,9]
+    
+    return rel_pos_errors_norm, rel_roll_pitch_errors, rel_yaw_errors, scale_errors, start_indices
+
+def load_data_svo_gtsam(trace_dir):
+    logger = logging.getLogger(__name__)
+    logger.info("Loading SVO-GTSAM data...")
+    
+    # load estimate.
+    if os.path.exists(os.path.join(trace_dir, 'estimate_states.csv')):
+        data = np.loadtxt(os.path.join(trace_dir, 'estimate_states.csv'))
+    else:
+        data = np.loadtxt(os.path.join(trace_dir, 'trace_states.txt'))
+
+    last_state = int(data[-1,0])
+    last_data = data[data[:,0]==last_state]
+    frame_ids = [ int(i) for i in last_data[:,1] ]
+    traj_es = dict([ (int(row[1]), row[11:18]) for row in last_data ])
+    
+    # load groundtruth.
+    data = np.loadtxt(os.path.join(trace_dir, 'groundtruth.txt'))
+    groundtruth = dict([ (int(row[0]), row[2:9]) for row in data ])
+    
+    # load matches and create a dictionary.
+    matches = np.loadtxt(os.path.join(trace_dir, 'groundtruth_matches.txt'))
+    matches = dict([(int(m[0]), int(m[1])) for m in matches])
+    
+    logger.info("done.")
+    
+    # remove frame ids that don't have a match.
+    n_removed = 0
+    for i in list(frame_ids):
+       if i not in matches:
+           frame_ids.remove(i)
+           n_removed += 1
+    print('removed '+str(n_removed)+' frames because no groundtruth available')   
+    
+    traj_gt = dict( [ (i, groundtruth[matches[i]]) for i in frame_ids ])
+
+    # generate list of synchronized poses.
+    keys = list(traj_gt.keys())
+    keys.sort()
+    gt = np.array([ traj_gt[i] for i in keys] )
+    es = np.array([ traj_es[i] for i in keys] )
+    t_es = keys
+    p_es = es[:,0:3]
+    q_es = es[:,3:7]
+    p_gt = gt[:,0:3]
+    q_gt = gt[:,3:7]
+    t_gt = keys
+    return t_es, p_es, q_es, t_gt, p_gt, q_gt
+    
+def load_hand_eye_calib_from_file(filename):
+    logger = logging.getLogger(__name__)
+    logger.info("Loading Hand-Eye calib from file: "+filename)
+    params = yaml.load(open(filename,'r'))
+    
+    if 'T_sensor_trackable' in params:
+        T_cm_quat = np.array([params['T_sensor_trackable']['qx'],
+                              params['T_sensor_trackable']['qy'],
+                              params['T_sensor_trackable']['qz'],
+                              params['T_sensor_trackable']['qw']])
+        T_cm_tran = np.array([params['T_sensor_trackable']['tx'],
+                              params['T_sensor_trackable']['ty'],
+                              params['T_sensor_trackable']['tz']])
+        T_B_V = utils.get_rigid_body_trafo(T_cm_quat, T_cm_tran)
+    else:
+        raise ValueError('could not load Hand-Eye calib.')
+    return T_B_V # (V)icon-Markers in (B)ody frame.
+    
+# -----------------------------------------------------------------------------
+# DEPRECATED
+
+
+    
+def load_dataset(results_dir, cam_delay):
+    print('loading dataset in '+results_dir)   
+    print('cam_delay = '+str(cam_delay))
+
+    data_es = np.loadtxt(os.path.join(results_dir, 'traj_estimate.txt'))
+    data_gt = np.loadtxt(os.path.join(results_dir, 'groundtruth.txt'))
+    data_gt = dict([(int(row[0]), row[1:]) for row in data_gt])
+    matches = np.loadtxt(os.path.join(results_dir, 'groundtruth_matches.txt'))
+    matches = dict([(int(row[0]), int(row[1])) for row in matches])
+    p_es = []
+    p_gt = []
+    q_es = []
+    q_gt = []
+    t_gt = []
+    for row_es in data_es:
+        image_id = int(row_es[0])
+        if image_id in matches:
+            groundtruth_id = matches[image_id]
+            row_gt = data_gt[groundtruth_id]
+            p_es.append(row_es[1:4])
+            p_gt.append(row_gt[1:4])
+            q_es.append(row_es[4:8])
+            q_gt.append(row_gt[4:8])
+            t_gt.append(row_gt[0])    
+    p_es = np.array(p_es)
+    p_gt = np.array(p_gt)
+    q_es = np.array(q_es)
+    q_gt = np.array(q_gt)
+    t_gt = np.array(t_gt)
+
+    return t_gt, p_es, q_es, t_gt, p_gt, q_gt
+    
+
+    
+def load_synthetic_dataset(results_dir):
+    gt_trajectory = np.loadtxt(os.path.join(results_dir, 'groundtruth.txt'))
+    es_trajectory = np.loadtxt(os.path.join(results_dir, 'traj_estimate.txt'))
+    N = es_trajectory.shape[0]
+    p_es = es_trajectory[:, 1:4]
+    q_es = es_trajectory[:, 4:8]
+
+    map_index_slot = dict()
+    for i, index in enumerate(gt_trajectory[:, 0]):
+        map_index_slot[int(index)] = i
+    gt_indices = np.zeros(N)
+    for i in range(N):
+        gt_indices[i] = map_index_slot[int(es_trajectory[i,0])]
+        
+    p_gt = gt_trajectory[np.int32(gt_indices), 1:4]
+    q_gt = gt_trajectory[np.int32(gt_indices), 4:8]
+    t_gt = gt_indices
+    return t_gt, p_es, q_es, t_gt, p_gt, q_gt
\ No newline at end of file
diff --git a/RWR/src/ze_oss/ze_trajectory_analysis/py/ze_trajectory_analysis/match_stamps.py b/RWR/src/ze_oss/ze_trajectory_analysis/py/ze_trajectory_analysis/match_stamps.py
new file mode 100755
index 0000000000000000000000000000000000000000..cc5db6b68908f5533b26eee4cce9c5abb63107ee
--- /dev/null
+++ b/RWR/src/ze_oss/ze_trajectory_analysis/py/ze_trajectory_analysis/match_stamps.py
@@ -0,0 +1,83 @@
+#!/usr/bin/python
+"""
+Zurich Eye
+"""
+
+import logging
+import argparse
+import numpy as np
+import ze_trajectory_analysis.utils as utils
+
+def associate(stamps_es, stamps_gt, offset_sec = 0.0, max_difference_sec = 0.02):
+    """
+    Associate two lists of nanosecond timestamps. As the time stamps never match
+    exactly, we aim to find the closest match for every input tuple.
+    
+    Input:
+    stamps_es -- nanosecond timestamps of estimates
+    stamps_gt -- nanosecond timestamps of estimates
+    offset_sec -- time offset between both dictionaries (e.g., to model the delay between the sensors)
+    max_difference_sec -- search radius for candidate generation
+
+    Output:
+    matches -- list of matched stamps [(stamp_es, stamp_gt), (stamp_es, stamp_gt)]
+    
+    """
+    logger = logging.getLogger(__name__)
+    es_keys = stamps_es.tolist()
+    gt_keys = stamps_gt.tolist()
+    offset = np.longlong(offset_sec*1e9)
+    max_difference = np.longlong(max_difference_sec*1e9)
+    logger.info("Computing potential matches...")
+    potential_matches = [(abs(a - (b + offset)), a, b) 
+                         for a in es_keys 
+                         for b in gt_keys 
+                         if abs(a - (b + offset)) < max_difference]
+    logger.info('...done.')
+    potential_matches.sort()
+    matches = []
+    logger.info('Find best matches...')
+    for diff, a, b in potential_matches:
+        if a in es_keys and b in gt_keys:
+            es_keys.remove(a)
+            gt_keys.remove(b)
+            matches.append((a, b))
+    logger.info('...done.')
+    matches.sort()
+    return matches
+    
+def write_matches_to_file(filename, matches):
+    logger = logging.getLogger(__name__)
+    associates_file = open(filename, 'w')     
+    associates_file.write('# Estimate Groundtruth\n')                       
+    for a,b in matches:
+      associates_file.write('{:d},{:d}\n'.format(a, b))
+    logger.info('Wrote ' + str(len(matches)) + ' matches to file: ' + filename)
+    
+if __name__ == '__main__':
+    
+    # parse command line
+    parser = argparse.ArgumentParser(description='''
+    This script takes two data files with timestamps and associates them   
+    ''')
+    parser.add_argument('file_es', help='ESTIMATE estimated trajectory (format: timestamp data)')
+    parser.add_argument('file_gt', help='GROUNDTRUTH (format: timestamp data)')
+    parser.add_argument('--file_es_col', help='Timestamp column in file 1', default=0)
+    parser.add_argument('--file_gt_col', help='Timestamp column in file 2', default=0)
+    parser.add_argument('--offset_sec', help='time offset added to the timestamps of the second file (default: 0.0)', default=0.0)
+    parser.add_argument('--max_difference_sec', help='maximally allowed time difference for matching entries (default: 0.02)', default=0.02)
+    parser.add_argument('--delimiter', help='csv delimiter', default=',')
+    args = parser.parse_args()
+
+    # Load data.
+    stamps_es = utils.read_nanosecond_timestamps_from_csv_file( \
+                    args.file_es, args.file_es_col, args.delimiter)
+    stamps_gt = utils.read_nanosecond_timestamps_from_csv_file( \
+                    args.file_gt, args.file_gt_col, args.delimiter)
+    
+    # Match data.
+    matches = associate(stamps_es, stamps_gt, args.offset_sec, args.max_difference_sec)    
+    
+    # Write to file.
+    write_matches_to_file('matches.csv', matches)
+        
diff --git a/RWR/src/ze_oss/ze_trajectory_analysis/py/ze_trajectory_analysis/plot.py b/RWR/src/ze_oss/ze_trajectory_analysis/py/ze_trajectory_analysis/plot.py
new file mode 100755
index 0000000000000000000000000000000000000000..9f5597952501f51498f7a5a5bc35846a0fabd2e3
--- /dev/null
+++ b/RWR/src/ze_oss/ze_trajectory_analysis/py/ze_trajectory_analysis/plot.py
@@ -0,0 +1,288 @@
+#!/usr/bin/python
+"""
+Zurich Eye
+"""
+
+import os
+import logging
+import argparse
+import numpy as np
+import matplotlib
+# Force matplotlib to not use any Xwindows backend.
+matplotlib.use('Agg')
+import matplotlib.pyplot as plt
+import matplotlib.gridspec as gridspec
+from matplotlib.ticker import FuncFormatter
+import ze_py.plot_utils as plot_utils
+import ze_trajectory_analysis.utils as utils
+#from mpl_toolkits.mplot3d import Axes3D
+#from matplotlib import rc
+#rc('font',**{'family':'serif','serif':['Cardo']})
+#rc('text', usetex=True)
+
+FORMAT = '.pdf'
+
+def plot_pointcloud_3d(results_dir, p_gt, p_es, scale, rot, trans):
+    
+    m = np.loadtxt(os.path.join(results_dir, 'pointcloud.txt'))
+    m_aligned = np.zeros(np.shape(m))
+    for i in range(len(m)):
+        m_aligned[i,:] = scale * np.dot(rot, m[i,:]) + trans        
+        
+    fig = plt.figure()
+    ax = fig.gca(projection='3d')
+    ax.plot(m_aligned[:,0], m_aligned[:,1], m_aligned[:,2], '.', ms=1, color='green')
+    ax.plot(p_es[:,0], p_es[:,1], p_es[:,2], linewidth=3, color='blue', label='SVO Bundle Adjust')
+    ax.plot(p_gt[:,0], p_gt[:,1], p_gt[:,2], linewidth=3, color='k', label='Groundtruth')
+    #ax.set_xlim([-60, 60])
+    #ax.set_ylim([-6, 55])
+    #ax.set_zlim([-6,15])
+    ax.legend()
+    #plt.show()
+    #fig.tight_layout()
+    fig.savefig(results_dir+'/trajectory_3d'+FORMAT)
+    
+def plot_trajectory(results_dir, p_gt, p_es, align_first_idx = 0, align_last_idx = -1):
+    
+    # plot trajectory top
+    fig = plt.figure(figsize=(8, 8))
+    ax = fig.add_subplot(111, aspect='equal', xlabel='x [m]', ylabel='y [m]')
+    ax.grid(ls='--', color='0.7')
+    p_es_0 = p_es - p_gt[0,:]
+    p_gt_0 = p_gt - p_gt[0,:]
+    ax.plot(p_es_0[:,0], p_es_0[:,1], 'b-', label='Estimate')
+    ax.plot(p_gt_0[:,0], p_gt_0[:,1], 'r-', label='Groundtruth')
+    #if align_last_idx < len(p_gt):
+    #    ax.plot(p_es_0[align_first_idx:align_last_idx,0], p_es_0[align_first_idx:align_last_idx,1], 'g-', linewidth=2, label='aligned')
+    for (x1,y1,z1),(x2,y2,z2) in zip(p_es_0[align_first_idx:align_last_idx:10,:],p_gt_0[align_first_idx:align_last_idx:10,:]):
+        ax.plot([x1,x2],[y1,y2],'-',color="gray")
+    
+    ax.legend(bbox_to_anchor=(0., 1.02, 1., .102),
+              loc=3, ncol=3, mode='expand', borderaxespad=0.)
+    #ax.set_ylim([-0.5, 5])
+    #fig.tight_layout()
+    fig.savefig(results_dir+'/trajectory_top'+FORMAT)
+    
+    # plot trajectory side
+    fig = plt.figure(figsize=(8, 5))
+    ax = fig.add_subplot(111, aspect='equal', xlabel='x [m]', ylabel='z [m]')
+    ax.grid(ls='--', color='0.7')
+    ax.plot(p_es[:,0]-p_gt[0,0], p_es[:,2]-p_gt[0,1], 'b-', label='Estimate')
+    ax.plot(p_gt[:,0]-p_gt[0,0], p_gt[:,2]-p_gt[0,1], 'r-', label='Groundtruth')
+    ax.legend(bbox_to_anchor=(0., 1.02, 1., .102),
+              loc=3, ncol=3, mode='expand', borderaxespad=0.)
+    #fig.tight_layout()
+    fig.savefig(os.path.join(results_dir, 'trajectory_side'+FORMAT))
+    
+    # write aligned trajectory to file
+    file_out = open(os.path.join(results_dir, 'trajectory.csv'), 'w')
+    file_out.write('# estimate-x [m], estimate-y [m], estimate-z [m], groundtruth-x [m], groundtruth-y [m], groundtruth-z [m]\n')
+    for i in range(len(p_es)):
+        file_out.write(
+            '%.6f, %.6f, %.6f, %.6f, %.6f, %.6f\n' %
+            (p_es[i,0]-p_gt[0,0], p_es[i,1]-p_gt[0,1], p_es[i,2]-p_gt[0,2],
+             p_gt[i,0]-p_gt[0,0], p_gt[i,1]-p_gt[0,1], p_gt[i,2]-p_gt[0,2]))
+    file_out.close()
+    
+def plot_translation_error(translation, translation_error, results_dir):
+    
+    # plot
+    fig = plt.figure(figsize=(8, 2.5))
+    ax = fig.add_subplot(111, xlabel='Distance [m]', ylabel='Position Drift [mm]', xlim=[0,translation[-1]])
+    ax.plot(translation, translation_error[:,0]*1000, 'r-', label='x')
+    ax.plot(translation, translation_error[:,1]*1000, 'g-', label='y')
+    ax.plot(translation, translation_error[:,2]*1000, 'b-', label='z')
+    ax.legend()
+    #fig.tight_layout()
+    fig.savefig(results_dir+'/translation_error'+FORMAT)
+    
+    # write to file
+    file_out = open(os.path.join(results_dir, 'translation_error.csv'), 'w')
+    file_out.write('# distance from start [m], error x [m], error y [m], error z [m]\n')
+    for i in range(len(translation_error)):
+        file_out.write(
+            '%.8f, %.8f, %.8f, %.8f\n' %
+            (translation[i], translation_error[i,0], translation_error[i,1], translation_error[i,2])) 
+    file_out.close()
+
+def plot_rotation_error(translation, rotation_error, results_dir):
+    
+    # plot
+    fig = plt.figure(figsize=(8, 2.5))
+    ax = fig.add_subplot(111, xlabel='Distance [m]', ylabel='Orient. err. [deg]', xlim=[0,translation[-1]])
+    ax.plot(translation, rotation_error[:,0]*180.0/np.pi, 'r-', label='yaw')
+    ax.plot(translation, rotation_error[:,1]*180.0/np.pi, 'g-', label='pitch')
+    ax.plot(translation, rotation_error[:,2]*180.0/np.pi, 'b-', label='roll')
+    ax.legend()
+    #fig.tight_layout()
+    fig.savefig(results_dir+'/orientation_error'+FORMAT)
+    
+    # write to file
+    file_out = open(os.path.join(results_dir, 'orientation_error.csv'), 'w')
+    file_out.write('# distance from start [m], error yaw [rad], error pitch [rad], error roll [rad]\n')
+    for i in range(len(rotation_error)):
+        file_out.write(
+            '%.8f, %.8f, %.8f, %.8f\n' %
+            (translation[i], rotation_error[i,0], rotation_error[i,1], rotation_error[i,2])) 
+    file_out.close()
+
+def plot_scale_error(translation, scale_error_perc, results_dir):
+    
+    # plot
+    fig = plt.figure(figsize=(8,2.5))
+    ax = fig.add_subplot(111, xlabel='Distance [m]', ylabel='Scale Drift [\%]', xlim=[0,translation[-1]])
+    ax.plot(translation, scale_error_perc, 'b-')
+    ax.set_ylim([-100, 100])
+    #fig.tight_layout()
+    fig.savefig(results_dir+'/scale_drift'+FORMAT)
+    
+    # write to file
+    file_out = open(os.path.join(results_dir, 'scale_error.csv'), 'w')
+    file_out.write('# distance from start [m], scale drift [%]\n')
+    for i in range(len(scale_error_perc)):
+        file_out.write('%.8f, %.8f\n' % (translation[i], scale_error_perc[i])) 
+    file_out.close()
+    
+def plot_travelled_distance(distances, results_dir):
+    fig = plt.figure(figsize=(8,5))
+    ax = fig.add_subplot(111, xlabel='Measurement', ylabel='Distance [m]')
+    ax.plot(range(len(distances)), distances)
+    #fig.tight_layout()
+    fig.savefig(results_dir+'/distance'+FORMAT)
+    
+def plot_imu_biases(stamps, bias_gyr, bias_acc, results_dir):
+    
+    stamps = np.array(stamps) - stamps[0]
+    
+    # Find min-max range of accelerometer bias
+    acc_min = 1.1*np.min(bias_acc)
+    acc_max = 1.1*np.max(bias_acc)
+
+    # Plot Accelerometer Bias.
+    fig = plt.figure(figsize=(8,12))
+    gs1 = gridspec.GridSpec(3, 1)
+    gs1.update(wspace=0.015) # set the spacing between axes. 
+    ax0 = fig.add_subplot(611, ylabel='Acc. Bias x')
+    ax0.set_xticks([])
+    ax0.yaxis.set_major_formatter(FuncFormatter(lambda y, pos: '%.3f'%y))
+    #ax0.locator_params(axis = 'y', nbins = 5)
+    ax0.plot(stamps/1e9, bias_acc[:,0], color='blue')
+    ax1 = fig.add_subplot(612, ylabel='Acc. Bias y')
+    ax1.set_xticks([])
+    ax1.yaxis.set_major_formatter(FuncFormatter(lambda y, pos: '%.3f'%y))
+    #ax1.locator_params(axis = 'y', nbins = 5)
+    ax1.plot(stamps/1e9, bias_acc[:,1], color='blue')
+    ax2 = fig.add_subplot(613, ylabel='Acc. Bias z')
+    #ax2.locator_params(axis = 'y', nbins = 5)
+    ax2.set_xticks([])
+    ax2.yaxis.set_major_formatter(FuncFormatter(lambda y, pos: '%.3f'%y))
+    ax2.plot(stamps/1e9, bias_acc[:,2], color='blue')
+
+    ax0.set_ylim([acc_min, acc_max])
+    ax1.set_ylim([acc_min, acc_max])
+    ax2.set_ylim([acc_min, acc_max])
+    
+    #ax0.legend(ncol=2,  loc='lower left', borderaxespad=0.2)
+    
+    # Find min-max range of gyroscope bias
+    gyro_min = 1.1*np.min(bias_gyr)
+    gyro_max = 1.1*np.max(bias_gyr)
+    
+    # Plot gyroscope bias
+    ax3 = fig.add_subplot(614, ylabel='Gyro. Bias x')
+    ax3.set_xticks([])
+    ax3.yaxis.set_major_formatter(FuncFormatter(lambda y, pos: '%.3f'%y))
+    #ax3.locator_params(axis = 'y', nbins = 5)
+    ax3.plot(stamps/1e9, bias_gyr[:,0], color='blue')
+    ax4 = fig.add_subplot(615, ylabel='Gyro. Bias y')
+    ax4.set_xticks([])
+    ax4.yaxis.set_major_formatter(FuncFormatter(lambda y, pos: '%.3f'%y))
+    #ax4.locator_params(axis = 'y', nbins = 5)
+    ax4.plot(stamps/1e9, bias_gyr[:,1], color='blue')
+    ax5 = fig.add_subplot(616, ylabel='Gyro. Bias z', xlabel='Time [s]')
+    #ax5.locator_params(axis = 'y', nbins = 5)
+    ax5.yaxis.set_major_formatter(FuncFormatter(lambda y, pos: '%.3f'%y))
+    ax5.plot(stamps/1e9, bias_gyr[:,2], color='blue')
+    
+    ax3.set_ylim([gyro_min, gyro_max])
+    ax4.set_ylim([gyro_min, gyro_max])
+    ax5.set_ylim([gyro_min, gyro_max])
+    
+    #ax3.legend(ncol=2, loc='lower left') 
+    
+    ax5.tick_params('x',top='off')
+    #fig.tight_layout()
+    fig.savefig(os.path.join(results_dir,'imu_states'+FORMAT))
+    
+    return 0
+    
+def plot_imu_state_along_trajectory(
+        data_dir, bias_gyr, bias_acc, velocity, pos, circle_size=0.2):
+    
+    # plot bias
+    fig = plt.figure(figsize=(8, 20))
+    ax = fig.add_subplot(311, aspect='equal', xlabel='x [m]', ylabel='y [m]', 
+                         title='Gyroscope Bias')
+    ax.grid(ls='--', color='0.7')
+    ax.plot(pos[:,0], pos[:,1], 'b-', label='Estimate')
+    bias_gyr_norm = np.sqrt(np.sum(bias_gyr**2,1))
+    out = plot_utils.circles(pos[:,0], pos[:,1], circle_size,
+                             c=bias_gyr_norm, ax=ax, edgecolor='none', lw=0)
+    cbar = fig.colorbar(out)
+    cbar.set_label(r"Gyroscope Bias")
+
+    # plot pos drift
+    ax = fig.add_subplot(312, aspect='equal', xlabel='x [m]', ylabel='y [m]', 
+                         title='Accelerometer Bias')
+    ax.grid(ls='--', color='0.7')
+    ax.plot(pos[:,0], pos[:,1], 'b-', label='Estimate')
+    bias_acc_norm = np.sqrt(np.sum(bias_acc**2,1))
+    out = plot_utils.circles(pos[:,0], pos[:,1], circle_size,
+                             c=bias_acc_norm, ax=ax, edgecolor='none', lw=0)
+    cbar = fig.colorbar(out)
+    cbar.set_label(r"Accelerometer Bias")
+ 
+    # plot yaw drift
+    ax = fig.add_subplot(313, aspect='equal', xlabel='x [m]', ylabel='y [m]', 
+                         title='Velocity')
+    ax.grid(ls='--', color='0.7')
+    ax.plot(pos[:,0], pos[:,1], 'b-', label='Estimate')
+    velocity_norm = np.sqrt(np.sum(velocity**2,1))
+    out = plot_utils.circles(pos[:,0], pos[:,1], circle_size,
+                             c=velocity_norm, ax=ax, edgecolor='none', lw=0)
+    cbar = fig.colorbar(out)
+    cbar.set_label(r"Velocity")
+    
+    # Save fig
+    fig.savefig(os.path.join(data_dir,'imu_states_along_trajectory'+FORMAT))
+    
+    
+# -----------------------------------------------------------------------------
+if __name__=='__main__':
+    
+    # parse command line
+    parser = argparse.ArgumentParser(description='Simple plotting of trajectory')
+    
+    parser.add_argument('--data_dir', help='folder with the results',
+                        default='')
+                        
+    options = parser.parse_args()    
+    
+    logging.basicConfig(level=logging.DEBUG)
+    logger = logging.getLogger(__name__)
+    logger.info('Plot trajectory')
+    
+    pos = np.genfromtxt(
+        os.path.join(options.data_dir, "traj_es.csv"), delimiter=',',
+        dtype=np.float64, skip_header=1)[:,1:4]
+    length = utils.get_distance_from_start(pos)[-1]
+    
+    fig = plt.figure(figsize=(5, 8))
+    ax = fig.add_subplot(211, aspect='equal', title="Trajectory length = {:.1f}m".format(length),
+                         xlabel="x [m]", ylabel="y [m]")    
+    ax.plot(pos[:,0], pos[:,1], 'b-', lw=2)
+    ax = fig.add_subplot(212, aspect='equal', xlabel="x [m]", ylabel="z [m]")    
+    ax.plot(pos[:,0], pos[:,2], 'b-', lw=2)
+    fig.patch.set_visible(False)
+    fig.tight_layout()
+    fig.savefig(os.path.join(options.data_dir, "traj_es"+FORMAT))
diff --git a/RWR/src/ze_oss/ze_trajectory_analysis/py/ze_trajectory_analysis/relative_errors.py b/RWR/src/ze_oss/ze_trajectory_analysis/py/ze_trajectory_analysis/relative_errors.py
new file mode 100755
index 0000000000000000000000000000000000000000..44caa38ab4ca1576812db303bf1d874bc893368f
--- /dev/null
+++ b/RWR/src/ze_oss/ze_trajectory_analysis/py/ze_trajectory_analysis/relative_errors.py
@@ -0,0 +1,261 @@
+#!/usr/bin/env python3
+"""
+Zurich Eye
+"""
+
+import os
+import logging
+import yaml
+import argparse
+import numpy as np
+import matplotlib.pyplot as plt
+from matplotlib.ticker import FuncFormatter
+from pylab import setp
+import ze_py.plot_utils as plot_utils
+import ze_trajectory_analysis.load as traj_loading
+from ze_trajectory_analysis.analyse import TrajectoryAnalysis
+from matplotlib import rc
+rc('font',**{'family':'serif','serif':['Cardo']})
+rc('text', usetex=True)
+
+
+def _set_boxplot_colors(boxplot_object, color):
+    setp(boxplot_object['boxes'][0], color=color)
+    setp(boxplot_object['caps'][0], color=color)
+    setp(boxplot_object['caps'][1], color=color)
+    setp(boxplot_object['whiskers'][0], color=color)
+    setp(boxplot_object['whiskers'][1], color=color)
+    setp(boxplot_object['fliers'], color=color)
+    setp(boxplot_object['medians'][0], color=color)
+    
+def plot_relative_errors(data_dirs, segment_lengths):
+    """data_dirs can contain a list of result directories. The plots are stored
+    in the first directory.        
+    """    
+    
+    colors = ['blue', 'black', 'green', 'red', 'mangenta', 'cyan', 'orange']
+
+    # Precompute position of boxplots in plot.     
+    n_exp = len(data_dirs)
+    n_dist = len(segment_lengths)
+    spacing = 1
+    pos = np.arange(0, n_dist*(n_exp+spacing), (n_exp+spacing))
+    
+    # Init axes
+    fig = plt.figure(figsize=(8,10))
+    ax_pos = fig.add_subplot(411, ylabel='Translation error [m]')
+    ax_yaw = fig.add_subplot(412, ylabel='Yaw error [deg]')
+    ax_rap = fig.add_subplot(413, ylabel='Roll and Pitch error [deg]')
+    ax_scale = fig.add_subplot(414, xlabel='Distance traveled [m]', ylabel=r"Scale error [\%]")
+
+    dummy_plots_pos = []
+    dummy_plots_yaw = []
+    dummy_plots_rap = []
+    dummy_plots_scale = []
+    labels = []
+    
+    # Relative errors YAML
+    results = dict()
+    
+    for idx_exp, data_dir in enumerate(data_dirs):
+        
+        # The dummy plots are used to create the legends.
+        dummy_plot_pos = ax_pos.plot([1,1], '-', color=colors[idx_exp])
+        dummy_plots_pos.append(dummy_plot_pos[0])
+        dummy_plot_yaw = ax_yaw.plot([1,1], '-', color=colors[idx_exp])
+        dummy_plots_yaw.append(dummy_plot_yaw[0])
+        dummy_plot_rap = ax_yaw.plot([1,1], '-', color=colors[idx_exp])
+        dummy_plots_rap.append(dummy_plot_rap[0])
+        dummy_plot_scale = ax_yaw.plot([1,1], '-', color=colors[idx_exp])
+        dummy_plots_scale.append(dummy_plot_scale[0])
+        labels.append('exp-'+str(idx_exp)) # TODO: Read label from data_dir
+        
+        for idx_segment_length, segment_length in enumerate(segment_lengths):
+            e_pos, e_rap, e_yaw, e_scale, e_idx = \
+                traj_loading.load_relative_errors_from_file(data_dir, segment_length)
+            pb = ax_pos.boxplot(e_pos, False, '',
+                                positions=[idx_exp + pos[idx_segment_length]], widths=0.8)                    
+            _set_boxplot_colors(pb, colors[idx_exp])
+            pb = ax_yaw.boxplot(e_yaw * 180.0 / np.pi, False, '',
+                                positions=[idx_exp + pos[idx_segment_length]], widths=0.8)
+            _set_boxplot_colors(pb, colors[idx_exp])
+            pb = ax_rap.boxplot(e_rap * 180.0 / np.pi, False, '',
+                                positions=[idx_exp + pos[idx_segment_length]], widths=0.8)
+            _set_boxplot_colors(pb, colors[idx_exp])
+            pb = ax_scale.boxplot(e_scale * 100.0 - 100.0, False, '',
+                                positions=[idx_exp + pos[idx_segment_length]], widths=0.8)
+            _set_boxplot_colors(pb, colors[idx_exp])
+            
+            results[segment_length] = dict()
+            results[segment_length]['translation'] = float(np.median(e_pos))
+            results[segment_length]['translation_rel'] = results[segment_length]['translation'] / segment_length * 100.0
+            results[segment_length]['yaw'] = float(np.median(e_yaw * 180.0 / np.pi))
+            results[segment_length]['roll_pitch'] = float(np.median(e_rap * 180.0 / np.pi))
+            results[segment_length]['scale'] = float(np.median(e_scale * 100.0 - 100.0))
+            
+        # save results_file
+        results_file = os.path.join(data_dir, 'results.yaml')
+        stats = dict()        
+        if os.path.exists(results_file):
+            stats = yaml.load(open(results_file,'r'))
+        stats['relative_errors'] = results
+        with open(results_file,'w') as outfile:
+            outfile.write(yaml.dump(stats, default_flow_style=False))
+                   
+    # create legend
+    ax_pos.legend(dummy_plots_yaw, labels, bbox_to_anchor=(0., 1.02, 1., .102),
+                  loc=3, ncol=3, mode='expand', borderaxespad=0.)
+        
+    def _ax_formatting(ax, dummy_plots):
+       ax.yaxis.grid(ls='--', color='0.7')
+       ax.yaxis.set_major_formatter(FuncFormatter(lambda y, pos: '%.2f'%y))
+       ax.set_xticks(pos + 0.5*n_exp - 0.5)
+       ax.set_xticklabels(segment_lengths)
+       ax.set_xlim(xmin=pos[0] - 1, xmax=pos[-1] + n_exp + 1)
+       for p in dummy_plots:
+           p.set_visible(False)
+    
+    _ax_formatting(ax_pos, dummy_plots_pos)    
+    _ax_formatting(ax_yaw, dummy_plots_yaw)
+    _ax_formatting(ax_rap, dummy_plots_rap)
+    _ax_formatting(ax_scale, dummy_plots_scale)
+    
+    #fig.tight_layout()
+    fig.savefig(os.path.join(data_dirs[0], 'traj_relative_errors_boxplots.pdf'), bbox_inches="tight")
+
+
+def plot_relative_errors_along_trajectory(data_dir, segment_length, circle_size=0.2):
+    ta = TrajectoryAnalysis(result_dir = data_dir)
+    ta.load_data(data_dir=data_dir)
+                     
+    e_pos, e_rap, e_yaw, e_scale, e_idx = \
+        traj_loading.load_relative_errors_from_file(data_dir, segment_length)
+       
+    # plot scale drift
+    fig = plt.figure(figsize=(16, 16))
+    ax = fig.add_subplot(221, aspect='equal', xlabel='x [m]', ylabel='y [m]', 
+                         title='Scale drift over '+str(segment_length)+'m Segment Length')
+    ax.grid(ls='--', color='0.7')
+    ax.plot(ta.p_es[:,0], ta.p_es[:,1], 'b-', label='Estimate')
+    out = plot_utils.circles(ta.p_es[e_idx,0], ta.p_es[e_idx,1], circle_size,
+                             c=np.abs(1.0 - e_scale)*100, ax=ax, edgecolor='none', lw=0)
+    cbar = fig.colorbar(out)
+    cbar.set_label(r"Scale drift [\%]")
+
+    # plot pos drift
+    ax = fig.add_subplot(222, aspect='equal', xlabel='x [m]', ylabel='y [m]', 
+                         title='Position drift over '+str(segment_length)+'m Segment Length')
+    ax.grid(ls='--', color='0.7')
+    ax.plot(ta.p_es[:,0], ta.p_es[:,1], 'b-', label='Estimate')
+    out = plot_utils.circles(ta.p_es[e_idx,0], ta.p_es[e_idx,1], circle_size,
+                             c=e_pos, ax=ax, edgecolor='none', lw=0)
+    cbar = fig.colorbar(out)
+    cbar.set_label(r"Position drift [m]")
+ 
+    # plot yaw drift
+    ax = fig.add_subplot(223, aspect='equal', xlabel='x [m]', ylabel='y [m]', 
+                         title='Yaw drift over '+str(segment_length)+'m Segment Length')
+    ax.grid(ls='--', color='0.7')
+    ax.plot(ta.p_es[:,0], ta.p_es[:,1], 'b-', label='Estimate')
+    out = plot_utils.circles(ta.p_es[e_idx,0], ta.p_es[e_idx,1], circle_size,
+                             c=e_yaw * 180.0 / np.pi, ax=ax, edgecolor='none', lw=0)
+    cbar = fig.colorbar(out)
+    cbar.set_label(r"Yaw drift [deg]")
+
+    # roll-pitch drift
+    ax = fig.add_subplot(224, aspect='equal', xlabel='x [m]', ylabel='y [m]', 
+                         title='Roll and pitch drift over '+str(segment_length)+'m Segment Length')
+    ax.grid(ls='--', color='0.7')
+    ax.plot(ta.p_es[:,0], ta.p_es[:,1], 'b-', label='Estimate')
+    out = plot_utils.circles(ta.p_es[e_idx,0], ta.p_es[e_idx,1], circle_size,
+                             c=e_rap * 180.0 / np.pi, ax=ax, edgecolor='none', lw=0)
+    cbar = fig.colorbar(out)
+    cbar.set_label(r"Roll and pitch drift [deg]")
+    
+    # Save fig
+    fig.savefig(os.path.join(data_dir, 'traj_relative_errors_'+str(segment_length)+'_errors.pdf'))
+    
+    
+def compute_relative_errors(data_dir,
+                            segment_lengths = [10, 50, 100, 200],
+                            skip_frames = 10,
+                            format_gt = 'pose',
+                            format_es = 'pose',
+                            filename_gt = 'traj_gt.csv',
+                            filename_es = 'traj_es.csv',
+                            filename_result_prefix = 'traj_relative_errors',
+                            match_timestamps_offset = 0.0,
+                            match_timestamps_max_difference_sec = 0.02,
+                            use_least_squares_alignment = False,
+                            segment_align_lsq_translation_only = False):
+    logger = logging.getLogger(__name__)
+    for segment_length in segment_lengths:
+        cmd = "rosrun ze_trajectory_analysis kitti_evaluation" \
+            + " --data_dir=" + data_dir \
+            + " --filename_es=" + filename_es \
+            + " --filename_gt=" + filename_gt \
+            + " --format_es=" + format_es \
+            + " --format_gt=" + format_gt \
+            + " --filename_result_prefix=" + filename_result_prefix \
+            + " --offset_sec=" + str(match_timestamps_offset) \
+            + " --max_difference_sec=" + str(match_timestamps_max_difference_sec) \
+            + " --segment_length=" + str(segment_length) \
+            + " --skip_frames=" + str(skip_frames) 
+            
+        if use_least_squares_alignment:
+            print("Use least squares alignment")
+            cmd += " --least_squares_align"
+    
+        if segment_align_lsq_translation_only:
+            print("Ignore orientation for least squares alignment")
+            cmd += " --least_squares_align_translation_only"
+                                           
+        logger.info("Executing command: "+cmd)
+        os.system(cmd)
+        
+if __name__=='__main__':
+    
+    # parse command line
+    parser = argparse.ArgumentParser(description='Compute relative errors')
+    parser.add_argument('--data_dir', default='',
+                        help='folder with the results')
+    parser.add_argument('--format_gt', default='pose',
+                        help='format groundtruth {swe,pose,euroc}')
+    parser.add_argument('--format_es', default='pose',
+                        help='format estimate {swe,pose,euroc}')
+    parser.add_argument('--segment_lengths', default="10, 40, 90, 160", type=str, 
+                        help='segment lengths for relative evaluation')
+    parser.add_argument('--segment_align_lsq', default='False',
+                        help='use least squares alignment of segment')
+    parser.add_argument('--segment_align_lsq_translation_only', default='False',
+                        help='use only translation part in least squares alignment of segment')
+    parser.add_argument('--plot_size', default=0.2,
+                        help='size of circle')
+    parser.add_argument('--skip_frames', default=1,
+                        help='frames skipped between segment evaluation')
+    parser.add_argument('--plot_errors_along_trajectory',
+                        default=False, action='store_true',
+                        help='Plot relative errors along trajectory')
+    options = parser.parse_args()    
+    
+    logging.basicConfig(level=logging.DEBUG)
+    logger = logging.getLogger(__name__)
+    logger.info('Compute relative errors')
+
+    segment_lengths = [int(item) for item in options.segment_lengths.split(',')]
+
+    if options.data_dir:
+        compute_relative_errors(options.data_dir,
+                                segment_lengths,
+                                skip_frames = int(options.skip_frames),
+                                format_gt = options.format_gt,
+                                format_es = options.format_es,
+                                use_least_squares_alignment = options.segment_align_lsq=='True',
+                                segment_align_lsq_translation_only = options.segment_align_lsq_translation_only=='True')
+
+        plot_relative_errors([options.data_dir], segment_lengths)
+        
+        if options.plot_errors_along_trajectory:
+            for segment_length in segment_lengths:
+                plot_relative_errors_along_trajectory( \
+                    options.data_dir, segment_length, float(options.plot_size))
diff --git a/RWR/src/ze_oss/ze_trajectory_analysis/py/ze_trajectory_analysis/templates/report.html b/RWR/src/ze_oss/ze_trajectory_analysis/py/ze_trajectory_analysis/templates/report.html
new file mode 100644
index 0000000000000000000000000000000000000000..b14ace06348cfc80b94721051fa0c203c9e99903
--- /dev/null
+++ b/RWR/src/ze_oss/ze_trajectory_analysis/py/ze_trajectory_analysis/templates/report.html
@@ -0,0 +1,23 @@
+<!DOCTYPE html>
+<html>
+<head lang="en">
+    <meta charset="UTF-8">
+    <title>{{ title }}</title>
+    <style>
+      body {
+        font-size: 8pt;
+        font-family: Times;
+      }
+      td {
+        padding-right:10pt
+      }
+    </style>
+</head>
+<body>
+    <h1>{{ title }}</h1>
+    {% for table in tables %}
+        <h2>{{ table['title'] }}</h2>
+        {{ table['data'] }}
+    {% endfor %}
+</body>
+</html>
diff --git a/RWR/src/ze_oss/ze_trajectory_analysis/py/ze_trajectory_analysis/utils.py b/RWR/src/ze_oss/ze_trajectory_analysis/py/ze_trajectory_analysis/utils.py
new file mode 100755
index 0000000000000000000000000000000000000000..ef666ee3947770a24292962e6b03011a001df5b8
--- /dev/null
+++ b/RWR/src/ze_oss/ze_trajectory_analysis/py/ze_trajectory_analysis/utils.py
@@ -0,0 +1,146 @@
+#!/usr/bin/python
+"""
+Zurich Eye
+"""
+
+import os
+import numpy as np
+import ze_py.transformations as tf
+
+def check_file_exists(filename):
+    """Utility to check if file exists"""
+    if not os.path.exists(filename):
+        raise ValueError('File does not exist: ' + filename)
+    return filename
+
+def check_folder_exists(directory):
+    """Utility to check if folder exists"""
+    if not os.path.isdir(directory):
+        raise ValueError('Folder does not exist: ' + directory)
+    return directory
+    
+def read_nanosecond_timestamps_from_csv_file(filename, col=0, delimiter=','):
+    """
+    Read nanosecond timestamps from textfile.
+    """
+    return np.genfromtxt(filename, usecols=(col), delimiter=delimiter, dtype=np.int64, skip_header=1)
+
+def get_rigid_body_trafo(quat,trans):
+    """
+    Get 4x4 matrix from quaternion and translation.
+    """
+    T = tf.quaternion_matrix(quat)
+    T[0:3,3] = trans
+    return T
+    
+def ominus(a,b):
+    """
+    Compute the relative 3D transformation between a and b.
+    
+    Input:
+    a -- first pose (homogeneous 4x4 matrix)
+    b -- second pose (homogeneous 4x4 matrix)
+    
+    Output:
+    Relative 3D transformation from a to b.
+    """
+    return np.dot(np.linalg.inv(a),b)
+    
+def distances_along_trajectory(traj):
+    """
+    Compute the translational distances along a trajectory. 
+    """
+
+    motion = [ np.linalg.norm(traj[i,:]-traj[i+1,:]) for i in range(len(traj)-1)]
+    distances = [0]
+    sum = 0
+    for t in motion:
+        sum += t
+        distances.append(sum)
+    return distances
+    
+def compute_comparison_indices_length(distances, dist, max_error):
+    max_idx = len(distances)
+    comparisons = []
+    for idx, d in enumerate(distances):
+        j = -1
+        error = max_error
+        for i in range(idx, max_idx):
+            if np.abs(distances[i]-(d+dist)) < error: 
+                j = i
+                error = np.abs(distances[i]-(d+dist))
+        comparisons.append(j)
+    return comparisons
+    
+def compute_comparison_indices_time(t_gt, dist, max_error):
+    max_idx = len(t_gt)
+    comparisons = []
+    for idx, d in enumerate(t_gt):
+        j = -1
+        error = max_error
+        for i in range(idx, max_idx):
+            if np.abs(t_gt[i]-(d+dist)) < error: 
+                j = i
+                error = np.abs(t_gt[i]-(d+dist))
+        comparisons.append(j)
+    return comparisons
+    
+
+def compute_angle(transform):
+    """
+    Compute the rotation angle from a 4x4 homogeneous matrix.
+    """
+    # an invitation to 3-d vision, p 27
+    return np.arccos( min(1,max(-1, (np.trace(transform[0:3,0:3]) - 1)/2) ))*180.0/np.pi
+    
+def compute_relative_error(p_es, q_es, p_gt, q_gt, T_cm, dist, max_error, unit='m', t_gt=[], scale=1.0):
+    
+    # find for every frame another frame that is 'dist' away to compare
+    if unit == 'm':
+        distances = distances_along_trajectory(p_gt)
+        comparisons = compute_comparison_indices_length(distances, dist, max_error)
+    elif unit == 's':
+        comparisons = compute_comparison_indices_time(t_gt, dist, max_error)
+              
+    print('#comparisons = '+str(len(comparisons)))
+    # compute relative error
+    T_mc = np.linalg.inv(T_cm)
+    errors = []
+    for idx, c in enumerate(comparisons):
+        if not c==-1:
+            T_c1 = get_rigid_body_trafo(q_es[idx,:], p_es[idx,:])
+            T_c2 = get_rigid_body_trafo(q_es[c,:], p_es[c,:])
+            T_c1_c2 = np.dot(np.linalg.inv(T_c1), T_c2)
+            T_c1_c2[:3,3] *= scale
+            # rotate error in world frame to get meaningful roll-pitch-yaw errors
+            #w_T_c1_c2 = np.dot(T_c1, np.dot(c1_T_c1_c2, np.linalg.inv(T_c1))) 
+            T_m1 = get_rigid_body_trafo(q_gt[idx,:], p_gt[idx,:])
+            T_m2 = get_rigid_body_trafo(q_gt[c,:], p_gt[c,:])
+            T_m1_m2 = np.dot(np.linalg.inv(T_m1), T_m2)
+            T_m1_m2_in_c1 = np.dot(T_cm, np.dot(T_m1_m2, T_mc))
+            T_error_in_c2 = np.dot(np.linalg.inv(T_c1_c2), T_m1_m2_in_c1)
+            T_c2_rot = np.eye(4)
+            T_c2_rot[0:3,0:3] = T_c2[0:3,0:3]
+            T_error_in_w =  np.dot(T_c2_rot, np.dot(T_error_in_c2, np.linalg.inv(T_c2_rot))) 
+            errors.append(T_error_in_w)
+    
+    error_trans_norm = []
+    error_yaw = []
+    error_gravity = []
+    error_angle = []
+    for e in errors:
+        error_trans_norm.append(np.linalg.norm(e[0:3,3]))
+        rpy_angles = tf.euler_from_matrix(e, 'rzyx')
+        error_angle.append(compute_angle(e))
+        error_yaw.append(abs(rpy_angles[0])*180.0/np.pi)
+        error_gravity.append(np.sqrt(rpy_angles[1]**2+rpy_angles[2]**2)*180.0/np.pi)
+        #error_gravity.append( 0.5*(rpy_angles[1]+rpy_angles[2]) )
+    return errors, error_trans_norm, np.array(error_yaw), np.array(error_gravity), error_angle
+    
+def get_distance_from_start(gt_translation):
+    distances = np.diff(gt_translation[:,0:3],axis=0)
+    distances = np.sqrt(np.sum(np.multiply(distances,distances),1))
+    distances = np.cumsum(distances)
+    distances = np.concatenate(([0], distances))
+    return distances
+    
\ No newline at end of file
diff --git a/RWR/src/ze_oss/ze_trajectory_analysis/setup.py b/RWR/src/ze_oss/ze_trajectory_analysis/setup.py
new file mode 100644
index 0000000000000000000000000000000000000000..07981a410b549629974e7804e0661c2aec11f1fa
--- /dev/null
+++ b/RWR/src/ze_oss/ze_trajectory_analysis/setup.py
@@ -0,0 +1,12 @@
+#!/usr/bin/env python
+
+from distutils.core import setup
+from catkin_pkg.python_setup import generate_distutils_setup
+
+d = generate_distutils_setup(
+    packages=['ze_trajectory_analysis'],
+    package_dir={'': 'py'},
+    install_requires=['rospy', 'yaml'],
+    )
+
+setup(**d)
\ No newline at end of file
diff --git a/RWR/src/ze_oss/ze_trajectory_analysis/src/kitti_evaluation.cpp b/RWR/src/ze_oss/ze_trajectory_analysis/src/kitti_evaluation.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b4398621382fc6d6374dcb09d7626b5b90440da1
--- /dev/null
+++ b/RWR/src/ze_oss/ze_trajectory_analysis/src/kitti_evaluation.cpp
@@ -0,0 +1,164 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include <ze/trajectory_analysis/kitti_evaluation.hpp>
+
+#include <ze/geometry/align_poses.hpp>
+#include <ze/geometry/align_points.hpp>
+
+namespace ze {
+
+RelativeError::RelativeError(
+    size_t first_frame, Vector3 W_t_gt_es, Vector3 W_R_gt_es,
+    real_t segment_length, real_t scale_error, int num_frames_in_between)
+  : first_frame(first_frame)
+  , W_t_gt_es(W_t_gt_es)
+  , W_R_gt_es(W_R_gt_es)
+  , len(segment_length)
+  , scale_error(scale_error)
+  , num_frames(num_frames_in_between)
+{}
+
+std::vector<real_t> trajectoryDistances(
+    const TransformationVector& poses)
+{
+  std::vector<real_t> dist;
+  dist.reserve(poses.size());
+  dist.push_back(0);
+  for (size_t i = 1; i < poses.size(); ++i)
+  {
+    dist.push_back(dist.at(i-1) + (poses.at(i).getPosition() - poses.at(i-1).getPosition()).norm());
+  }
+  return dist;
+}
+
+int32_t lastFrameFromSegmentLength(
+    const std::vector<real_t>& dist,
+    const size_t first_frame,
+    const real_t segment_length)
+{
+  for (size_t i = first_frame; i < dist.size(); i++)
+  {
+    if (dist.at(i) > dist.at(first_frame) + segment_length)
+      return i;
+  }
+  return -1;
+}
+
+std::vector<RelativeError> calcSequenceErrors(
+    const TransformationVector& T_W_A, // groundtruth
+    const TransformationVector& T_W_B,
+    const real_t& segment_length,
+    const size_t skip_num_frames_between_segment_evaluation,
+    const bool use_least_squares_alignment,
+    const double least_squares_align_range,
+    const bool least_squares_align_translation_only)
+{
+  // Pre-compute cumulative distances (from ground truth as reference).
+  std::vector<real_t> dist_gt = trajectoryDistances(T_W_A);
+  std::vector<real_t> dist_es = trajectoryDistances(T_W_B);
+
+  // Compute relative errors for all start positions.
+  std::vector<RelativeError> errors;
+  for (size_t first_frame = 0; first_frame < T_W_A.size();
+       first_frame += skip_num_frames_between_segment_evaluation)
+  {
+    // Find last frame to compare with.
+    int32_t last_frame = lastFrameFromSegmentLength(dist_gt, first_frame, segment_length);
+    if (last_frame == -1)
+    {
+      continue; // continue, if segment is longer than trajectory.
+    }
+
+    // Perform a least-squares alignment of the first part of the trajectories.
+    int n_align_poses = least_squares_align_range * (last_frame - first_frame);
+    Transformation T_A0_B0 = T_W_A[first_frame].inverse() * T_W_B[first_frame];
+    if(use_least_squares_alignment && n_align_poses > 1)
+    {
+      if (least_squares_align_translation_only)
+      {
+        /*
+        Positions p_W_es_align(3, n_align_poses);
+        Positions p_W_gt_align(3, n_align_poses);
+        for (int i = 0; i < n_align_poses; ++i)
+        {
+          p_W_es_align.col(i) = T_W_B.at(first_frame + i).getPosition();
+          p_W_gt_align.col(i) = T_W_A.at(first_frame + i).getPosition();
+
+          PointAligner problem(p_W_gt_align, p_W_es_align);
+          problem.optimize(T_A0_B0);
+        }
+        */
+        //! @todo: Run closed-form solution of the alignment first. As it is now
+        //!        the alignment does not provide good results.
+        LOG(FATAL) << "Point-based alignment not supported yet.";
+      }
+      else
+      {
+        TransformationVector T_W_es_align(
+              T_W_B.begin() + first_frame, T_W_B.begin() + first_frame + n_align_poses);
+        TransformationVector T_W_gt_align(
+              T_W_A.begin() + first_frame, T_W_A.begin() + first_frame + n_align_poses);
+        VLOG(40) << "T_W_es_align size = " << T_W_es_align.size();
+
+        const real_t sigma_pos = 0.05;
+        const real_t sigma_rot = 5.0 / 180 * M_PI;
+        PoseAligner problem(T_W_gt_align, T_W_es_align, sigma_pos, sigma_rot);
+        problem.optimize(T_A0_B0);
+      }
+    }
+
+    // Compute relative rotational and translational errors.
+    Transformation T_W_Ai = T_W_A[last_frame];
+    Transformation T_A0_Ai = T_W_A[first_frame].inverse() * T_W_Ai;
+    Transformation T_Bi_A0 = T_W_B[last_frame].inverse() * T_W_A[first_frame];
+    Transformation T_Bi_Ai = T_Bi_A0 * T_A0_B0 * T_A0_Ai;
+    Transformation T_Ai_Bi = T_Bi_Ai.inverse();
+
+    // The relative error is represented in the frame of reference of the last
+    // frame in the ground-truth trajectory. We want to express it in the world
+    // frame to make statements about the yaw drift (not observable in Visual-
+    // inertial setting) vs. roll and pitch (observable).
+    Vector3 W_t_gtlast_eslast = T_W_Ai.getRotation().rotate(T_Ai_Bi.getPosition());
+    Vector3 W_R_gtlast_eslast = T_W_Ai.getRotation().rotate(T_Ai_Bi.getRotation().log());
+
+    // Scale error is the ratio of the respective segment length
+    real_t scale_error =
+        (dist_es.at(last_frame) - dist_es.at(first_frame))
+        / (dist_gt.at(last_frame) - dist_gt.at(first_frame));
+
+    errors.push_back(RelativeError(first_frame,
+                                   W_t_gtlast_eslast,
+                                   W_R_gtlast_eslast,
+                                   segment_length,
+                                   scale_error,
+                                   last_frame - first_frame + 1));
+  }
+
+  // return error vector
+  return errors;
+}
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_trajectory_analysis/src/kitti_evaluation_node.cpp b/RWR/src/ze_oss/ze_trajectory_analysis/src/kitti_evaluation_node.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..7f574c3df6d6d8fed934064fc368093ca4fec557
--- /dev/null
+++ b/RWR/src/ze_oss/ze_trajectory_analysis/src/kitti_evaluation_node.cpp
@@ -0,0 +1,162 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include <string>
+#include <iostream>
+#include <glog/logging.h>
+#include <gflags/gflags.h>
+
+#include <ze/common/file_utils.hpp>
+#include <ze/common/csv_trajectory.hpp>
+#include <ze/trajectory_analysis/kitti_evaluation.hpp>
+
+DEFINE_string(data_dir, ".", "Path to data");
+DEFINE_string(filename_es, "traj_es.csv", "Filename of estimated trajectory.");
+DEFINE_string(filename_gt, "traj_gt.csv", "Filename of groundtruth trajectory.");
+DEFINE_string(filename_result_prefix, "traj_relative_errors", "Filename prefix of result.");
+DEFINE_string(format_es, "pose", "Format of the estimate {pose, euroc, swe}.");
+DEFINE_string(format_gt, "pose", "Format of the groundtruth {pose, euroc, swe}.");
+DEFINE_double(offset_sec, 0.0, "time offset added to the timestamps of the estimate");
+DEFINE_double(max_difference_sec, 0.02, "maximally allowed time difference for matching entries");
+DEFINE_double(segment_length, 50, "Segment length of relative error evaluation. [meters]");
+DEFINE_double(skip_frames, 10, "Number of frames to skip between evaluation.");
+DEFINE_bool(least_squares_align, false, "Use least squares to align 20% of the segment length.");
+DEFINE_bool(least_squares_align_translation_only, false, "Ignore the orientation for the LSQ alignment.");
+DEFINE_double(least_squares_align_range, 0.2, "Portion of the segment that should be least squares aligned.");
+
+ze::PoseSeries::Ptr loadData(const std::string& format,
+                             const std::string& datapath)
+{
+  if(format == "swe")
+  {
+    VLOG(1) << "Loading 'swe' formatted trajectory from: " << datapath;
+    ze::PoseSeries::Ptr series = std::make_shared<ze::SWEResultSeries>();
+    series->load(datapath);
+    return series;
+  }
+  else if(format == "euroc")
+  {
+    VLOG(1) << "Loading 'euroc' formatted trajectory from: " << datapath;
+    ze::PoseSeries::Ptr series = std::make_shared<ze::EurocResultSeries>();
+    series->load(datapath);
+    return series;
+  }
+  else if(format == "pose")
+  {
+    VLOG(1) << "Loading 'pose' formatted trajectory from: " << datapath;
+    ze::PoseSeries::Ptr series = std::make_shared<ze::PoseSeries>();
+    series->load(datapath);
+    return series;
+  }
+  LOG(FATAL) << "Format " << format << " is not supported.";
+  return nullptr;
+}
+
+
+int main(int argc, char** argv)
+{
+  google::InitGoogleLogging(argv[0]);
+  google::ParseCommandLineFlags(&argc, &argv, true);
+  google::InstallFailureSignalHandler();
+  FLAGS_alsologtostderr = true;
+  FLAGS_colorlogtostderr = true;
+
+  // Load groundtruth.
+  VLOG(1) << "Load groundtruth: " << FLAGS_filename_gt;
+  ze::PoseSeries::Ptr gt_data = loadData(
+        FLAGS_format_gt, ze::joinPath(FLAGS_data_dir, FLAGS_filename_gt));
+
+  // Load estimate data.
+  VLOG(1) << "Load estimate: " << FLAGS_filename_es;
+  ze::PoseSeries::Ptr es_data =
+      loadData(FLAGS_format_es, ze::joinPath(FLAGS_data_dir, FLAGS_filename_es));
+
+  ze::StampedTransformationVector es_stamped_poses =
+      es_data->getStampedTransformationVector();
+
+  // Now loop through all estimate stamps and find closest groundtruth-stamp.
+  ze::TransformationVector es_poses, gt_poses;
+  es_poses.reserve(es_stamped_poses.size());
+  gt_poses.reserve(es_stamped_poses.size());
+  {
+    int64_t offset_nsec = ze::secToNanosec(FLAGS_offset_sec);
+    int64_t max_diff_nsec = ze::secToNanosec(FLAGS_max_difference_sec);
+    int n_skipped = 0;
+    VLOG(1) << "Associated timestamps of " << es_stamped_poses.size() << " poses...";
+    for(const auto& it : es_stamped_poses)
+    {
+      bool success;
+      int64_t gt_stamp;
+      ze::Vector7 gt_pose_data;
+      std::tie(gt_stamp, gt_pose_data, success) =
+          gt_data->getBuffer().getNearestValue(it.first + offset_nsec);
+      CHECK(success);
+      if(std::abs(gt_stamp - it.first + offset_nsec) > max_diff_nsec)
+      {
+        ++n_skipped;
+        continue;
+      }
+
+      es_poses.push_back(it.second);
+      gt_poses.push_back(ze::PoseSeries::getTransformationFromVec7(gt_pose_data));
+    }
+    VLOG(1) << "...done. Found " << es_poses.size() << " matches.";
+  }
+
+  // Kitti evaluation
+  VLOG(1) << "Computing relative errors...";
+  std::vector<ze::RelativeError> errors =
+      ze::calcSequenceErrors(gt_poses, es_poses, FLAGS_segment_length,
+                             FLAGS_skip_frames, FLAGS_least_squares_align,
+                             FLAGS_least_squares_align_range,
+                             FLAGS_least_squares_align_translation_only);
+  VLOG(1) << "...done";
+
+  // Write result to file
+  std::string filename_result =
+      FLAGS_filename_result_prefix + "_"
+      + std::to_string(static_cast<int>(FLAGS_segment_length)) + ".csv";
+  VLOG(1) << "Write result to file: " << ze::joinPath(FLAGS_data_dir, filename_result);
+  std::ofstream fs;
+  ze::openOutputFileStream(ze::joinPath(FLAGS_data_dir, filename_result), &fs);
+  fs << "# First frame index, err-tx, err-ty, err-tz, err-ax, err-ay, err-az, length, num frames, err-scale\n";
+  for(const ze::RelativeError& err : errors)
+  {
+    fs << err.first_frame << ", "
+       << err.W_t_gt_es.x() << ", "
+       << err.W_t_gt_es.y() << ", "
+       << err.W_t_gt_es.z() << ", "
+       << err.W_R_gt_es.x() << ", "
+       << err.W_R_gt_es.y() << ", "
+       << err.W_R_gt_es.z() << ", "
+       << err.len << ", "
+       << err.num_frames << ", "
+       << err.scale_error << "\n";
+  }
+  fs.close();
+  VLOG(1) << "Finished.";
+
+  return 0;
+}
diff --git a/RWR/src/ze_oss/ze_trajectory_analysis/src/match_stamps_node.cpp b/RWR/src/ze_oss/ze_trajectory_analysis/src/match_stamps_node.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f2af41c9fd737f713b399b9b7897b45be5abd0fb
--- /dev/null
+++ b/RWR/src/ze_oss/ze_trajectory_analysis/src/match_stamps_node.cpp
@@ -0,0 +1,114 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include <iostream>
+#include <glog/logging.h>
+#include <gflags/gflags.h>
+#include <ze/common/types.hpp>
+#include <ze/common/buffer.hpp>
+#include <ze/common/file_utils.hpp>
+#include <ze/common/transformation.hpp>
+#include <ze/common/time_conversions.hpp>
+
+DEFINE_string(data_dir, ".", "Path to data");
+DEFINE_string(filename_es, "traj_es.csv", "Filename of estimated trajectory.");
+DEFINE_string(filename_gt, "traj_gt.csv", "Filename of groundtruth trajectory.");
+DEFINE_string(filename_matches, "traj_es_gt_matches.csv", "Filename of matched timestamps");
+DEFINE_uint64(stamp_index_estimate, 0, "Index of timestamp in estimate file.");
+DEFINE_uint64(stamp_index_groundtruth, 0, "Index of timestamp in estimate file.");
+DEFINE_double(offset_sec, 0.0, "time offset added to the timestamps of the estimate");
+DEFINE_double(max_difference_sec, 0.02, "maximally allowed time difference for matching entries");
+
+int main(int argc, char** argv)
+{
+  google::InitGoogleLogging(argv[0]);
+  google::ParseCommandLineFlags(&argc, &argv, true);
+  google::InstallFailureSignalHandler();
+
+  // Load groundtruth.
+  ze::Buffer<ze::real_t,1> gt_stamps;
+  {
+    ze::Vector1 dummy; // We misuse the Buffer class to easily find the closest timestamp.
+    std::ifstream fs;
+    ze::openFileStream(ze::joinPath(FLAGS_data_dir, FLAGS_filename_gt), &fs);
+    std::string line;
+    while(std::getline(fs, line))
+    {
+      if('%' != line.at(0) && '#' != line.at(0) && 't' != line.at(0))
+      {
+        std::vector<std::string> items = ze::splitString(line, ',');
+        CHECK_GE(items.size(), FLAGS_stamp_index_groundtruth + 1);
+        int64_t stamp = std::stoll(items[FLAGS_stamp_index_groundtruth]);
+        gt_stamps.insert(stamp, dummy);
+      }
+    }
+  }
+
+  // Load estimate stamps.
+  std::vector<int64_t> es_stamps;
+  {
+    std::ifstream fs;
+    ze::openFileStream(ze::joinPath(FLAGS_data_dir, FLAGS_filename_es), &fs);
+    std::string line;
+    int64_t offset_nsec = ze::secToNanosec(FLAGS_offset_sec);
+    while(std::getline(fs, line))
+    {
+      if('%' != line.at(0) && '#' != line.at(0) && 't' != line.at(0))
+      {
+        std::vector<std::string> items = ze::splitString(line, ',');
+        CHECK_GE(items.size(), FLAGS_stamp_index_estimate + 1);
+        int64_t stamp = std::stoll(items[FLAGS_stamp_index_estimate]);
+        es_stamps.push_back(stamp + offset_nsec);
+      }
+    }
+  }
+
+  // Now loop through all estimate stamps and find closest groundtruth-stamp.
+  {
+    int64_t max_diff_nsec = ze::secToNanosec(FLAGS_max_difference_sec);
+    int n_skipped = 0;
+    std::ofstream fs;
+    ze::openOutputFileStream(ze::joinPath(FLAGS_data_dir, FLAGS_filename_matches), &fs);
+    fs << "# es-stamp [nsec], gt-stamp [nsec]\n";
+    for(const int64_t& es_stamp : es_stamps)
+    {
+      bool success;
+      int64_t gt_stamp;
+      std::tie(gt_stamp, std::ignore, success) = gt_stamps.getNearestValue(es_stamp);
+      CHECK(success);
+      if(std::abs(gt_stamp - es_stamp) > max_diff_nsec)
+      {
+        ++n_skipped;
+        continue;
+      }
+      fs << es_stamp << ", " << gt_stamp << "\n";
+    }
+
+    VLOG(1) << "Wrote " << es_stamps.size() - n_skipped << " matched poses to file"
+            << ". Skipped " << n_skipped << " because of too large time difference.";
+  }
+
+  return 0;
+}
diff --git a/RWR/src/ze_oss/ze_vi_simulation/CMakeLists.txt b/RWR/src/ze_oss/ze_vi_simulation/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..cc3d1001b3250f79ce2eba5bcaf8808bfc93206e
--- /dev/null
+++ b/RWR/src/ze_oss/ze_vi_simulation/CMakeLists.txt
@@ -0,0 +1,54 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(ze_vi_simulation)
+
+find_package(catkin_simple REQUIRED)
+find_package(OpenCV REQUIRED)
+catkin_simple(ALL_DEPS_REQUIRED)
+
+include(ze_setup)
+
+#############
+# LIBRARIES #
+#############
+set(HEADERS
+    include/ze/vi_simulation/camera_simulator.hpp
+    include/ze/vi_simulation/camera_simulator_types.hpp
+    include/ze/vi_simulation/imu_bias_simulator.hpp
+    include/ze/vi_simulation/imu_simulator.hpp
+    include/ze/vi_simulation/trajectory_simulator.hpp
+    include/ze/vi_simulation/evaluation_tools.hpp
+    include/ze/vi_simulation/vi_simulator.hpp
+    )
+
+set(SOURCES
+    src/camera_simulator.cpp
+    src/imu_bias_simulator.cpp
+    src/vi_simulator.cpp
+    )
+
+cs_add_library(${PROJECT_NAME} ${SOURCES} ${HEADERS})
+
+##########
+# GTESTS #
+##########
+
+catkin_add_gtest(test_camera_simulator test/test_camera_simulator.cpp)
+target_link_libraries(test_camera_simulator ${PROJECT_NAME} ${OpenCV_LIBRARIES})
+
+catkin_add_gtest(test_imu_bias_simulator test/test_imu_bias_simulator.cpp)
+target_link_libraries(test_imu_bias_simulator ${PROJECT_NAME} ${OpenCV_LIBRARIES})
+
+catkin_add_gtest(test_imu_simulator test/test_imu_simulator.cpp)
+target_link_libraries(test_imu_simulator ${PROJECT_NAME} ${OpenCV_LIBRARIES})
+
+catkin_add_gtest(test_trajectory_simulator test/test_trajectory_simulator.cpp)
+target_link_libraries(test_trajectory_simulator ${PROJECT_NAME} ${OpenCV_LIBRARIES})
+
+catkin_add_gtest(test_vi_simulator test/test_vi_simulator.cpp)
+target_link_libraries(test_vi_simulator ${PROJECT_NAME} ${OpenCV_LIBRARIES})
+
+##########
+# EXPORT #
+##########
+cs_install()
+cs_export()
diff --git a/RWR/src/ze_oss/ze_vi_simulation/include/ze/vi_simulation/camera_simulator.hpp b/RWR/src/ze_oss/ze_vi_simulation/include/ze/vi_simulation/camera_simulator.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..4a9ea103e0415375bfafcbccd7206c465afc7460
--- /dev/null
+++ b/RWR/src/ze_oss/ze_vi_simulation/include/ze/vi_simulation/camera_simulator.hpp
@@ -0,0 +1,118 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#pragma once
+
+#include <memory>
+#include <unordered_map>
+#include <ze/common/macros.hpp>
+#include <ze/common/timer_collection.hpp>
+#include <ze/common/transformation.hpp>
+#include <ze/common/types.hpp>
+#include <ze/vi_simulation/camera_simulator_types.hpp>
+
+namespace ze {
+
+// fwd.
+class CameraRig;
+class TrajectorySimulator;
+class Visualizer;
+
+// -----------------------------------------------------------------------------
+struct CameraSimulatorOptions
+{
+  uint32_t num_keypoints_per_frame { 50  };
+  real_t keypoint_noise_sigma { 1.0 };
+  uint32_t max_num_landmarks_ { 10000 };
+  real_t min_depth_m { 2.0 };
+  real_t max_depth_m { 7.0 };
+};
+
+// -----------------------------------------------------------------------------
+//! Simulate feature observations while moving along a trajectory.
+//! @todo(cfo) Model extra nuisances:
+//!            - false associations / outliers
+//!            - variable number of features along trajectory
+class CameraSimulator
+{
+public:
+  ZE_POINTER_TYPEDEFS(CameraSimulator);
+
+  CameraSimulator() = delete;
+
+  CameraSimulator(
+      const std::shared_ptr<TrajectorySimulator>& trajectory,
+      const std::shared_ptr<CameraRig>& camera_rig,
+      const CameraSimulatorOptions& options)
+    : trajectory_(trajectory)
+    , rig_(camera_rig)
+    , options_(options)
+  {}
+
+  void setVisualizer(const std::shared_ptr<Visualizer>& visualizer);
+
+  void initializeMap();
+
+  void visualize(
+      real_t dt = 0.2,
+      real_t marker_size_trajectory = 0.2,
+      real_t marker_size_landmarks = 0.2);
+
+  CameraMeasurementsVector getMeasurements(
+      real_t time);
+
+  CameraMeasurementsVector getMeasurementsCorrupted(
+      real_t time);
+
+  void reset();
+
+  inline const TrajectorySimulator& trajectory() const { return *trajectory_; }
+
+  DECLARE_TIMER(SimTimer, timer_,
+                visible_landmarks, get_measurements);
+
+private:
+  CameraMeasurements visibleLandmarks(
+      const uint32_t cam_idx,
+      const Transformation& T_W_B,
+      const uint32_t lm_min_idx,
+      const uint32_t lm_max_idx);
+
+  std::shared_ptr<TrajectorySimulator> trajectory_;
+  std::shared_ptr<CameraRig> rig_;
+  CameraSimulatorOptions options_;
+
+  std::shared_ptr<Visualizer> viz_;
+
+  Positions landmarks_W_;
+  Bearings normals_W_;
+
+  int32_t track_id_counter_ = 0;
+  std::unordered_map<int32_t, int32_t> global_lm_id_to_track_id_map_;
+};
+
+
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_vi_simulation/include/ze/vi_simulation/camera_simulator_types.hpp b/RWR/src/ze_oss/ze_vi_simulation/include/ze/vi_simulation/camera_simulator_types.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..51e841aee40fe18b64677e6ae8df5bc3ea5988f7
--- /dev/null
+++ b/RWR/src/ze_oss/ze_vi_simulation/include/ze/vi_simulation/camera_simulator_types.hpp
@@ -0,0 +1,47 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#pragma once
+
+#include <ze/common/types.hpp>
+
+namespace ze {
+
+struct CameraMeasurements
+{
+  //! Each column is a keypoint observation.
+  Keypoints keypoints_;
+
+  //! Global landmark index of each observed feature. The size of the vector is
+  //! the same as the number of columns in the keypoints block.
+  std::vector<int32_t> global_landmark_ids_;
+
+  //! Temporary track index of a landmark. If the landmark is
+  //! re-observed after a loop, it will be assigned a different id.
+  std::vector<int32_t> local_track_ids_;
+};
+using CameraMeasurementsVector = std::vector<CameraMeasurements>;
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_vi_simulation/include/ze/vi_simulation/evaluation_tools.hpp b/RWR/src/ze_oss/ze_vi_simulation/include/ze/vi_simulation/evaluation_tools.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..f580a3e47667a94fc5e4764bf85c6ff66d38a704
--- /dev/null
+++ b/RWR/src/ze_oss/ze_vi_simulation/include/ze/vi_simulation/evaluation_tools.hpp
@@ -0,0 +1,223 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#pragma once
+
+#include <ze/vi_simulation/imu_bias_simulator.hpp>
+#include <ze/matplotlib/matplotlibcpp.hpp>
+#include <ze/splines/bspline_pose_minimal.hpp>
+
+//! A couple of useful functions to evaluation simulation results with IMUs.
+namespace ze {
+
+// -----------------------------------------------------------------------------
+//! Takes the accel / gyro bias noise density in the continuous-case and
+//! a constant vector valued bias otherwise.
+ImuBiasSimulator::Ptr generateImuBias(real_t start,
+                             real_t end,
+                             const std::string& type,
+                             Vector3 imu_acc_bias,
+                             Vector3 imu_gyr_bias)
+{
+  //! continuous bias model
+  if (type == "continuous")
+  {
+
+    return std::make_shared<ContinuousBiasSimulator>(
+          imu_acc_bias,
+          imu_gyr_bias,
+          start,
+           end,
+          100); // This is an arbitrary value.
+  }
+  //! a simple constant bias
+  else
+  {
+    return std::make_shared<ConstantBiasSimulator>(imu_acc_bias, imu_gyr_bias);
+  }
+}
+
+//-----------------------------------------------------------------------------
+void plotOrientation(
+    const std::vector<real_t>& times,
+    const Eigen::Matrix<real_t, 3, Eigen::Dynamic>& points,
+    const std::string& label,
+    const Eigen::Matrix<real_t, 3, Eigen::Dynamic>* ref_points = nullptr)
+{
+  plt::figure("orientation");
+  plt::subplot(3, 1, 1);
+  plt::title("Orientation");
+  plt::labelPlot(label, times, points.row(0));
+  if(ref_points)
+  {
+    plt::labelPlot("reference", times, ref_points->row(0));
+  }
+  plt::legend();
+
+  plt::subplot(3, 1, 2);
+  plt::labelPlot(label, times, points.row(1));
+  if(ref_points)
+  {
+    plt::labelPlot("reference", times, ref_points->row(1));
+  }
+  plt::subplot(3, 1, 3);
+  plt::labelPlot(label, times, points.row(2));
+  if(ref_points)
+  {
+    plt::labelPlot("reference", times, ref_points->row(2));
+  }
+  plt::show(false);
+}
+
+//-----------------------------------------------------------------------------
+void plotOrientation(
+    const std::vector<real_t>& times,
+    const QuaternionVector& orientation,
+    const std::string& label,
+    const BSplinePoseMinimalRotationVector::Ptr reference = nullptr)
+{
+  CHECK_EQ(times.size(), orientation.size());
+
+  Eigen::Matrix<real_t, 3, Eigen::Dynamic> points(3, orientation.size());
+  Eigen::Matrix<real_t, 3, Eigen::Dynamic> ref_points(3, orientation.size());
+
+  for (size_t i = 0; i < orientation.size(); ++i)
+  {
+    ze::sm::RotationVector rv(Matrix3(orientation[i].getRotationMatrix()));
+    points.col(i) = rv.getParameters();
+    // Hanlde sign flips in the rotation vector.
+    if (i > 0 && (points.col(i-1) - points.col(i)).norm() >
+        (points.col(i-1) + points.col(i)).norm())
+    {
+      points.col(i) = -points.col(i);
+    }
+    if (reference)
+    {
+      ref_points.col(i) = reference->eval(times[i]).tail<3>();
+    }
+  }
+
+  if (reference)
+  {
+    plotOrientation(times, points, label, &ref_points);
+  }
+  else
+  {
+    plotOrientation(times, points, label);
+  }
+}
+
+//-----------------------------------------------------------------------------
+void plotOrientation(
+    const std::vector<real_t>& times,
+    const std::vector<Matrix3>& orientation,
+    const std::string& label,
+    const BSplinePoseMinimalRotationVector::Ptr& trajectory = nullptr)
+{
+  CHECK_EQ(times.size(), orientation.size());
+
+  Eigen::Matrix<real_t, 3, Eigen::Dynamic> points(3, orientation.size());
+  Eigen::Matrix<real_t, 3, Eigen::Dynamic> ref_points(3, orientation.size());
+
+  for (size_t i = 0; i < orientation.size(); ++i)
+  {
+    ze::sm::RotationVector rv(orientation[i]);
+    points.col(i) = rv.getParameters();
+    if (trajectory)
+    {
+      ref_points.col(i) = trajectory->eval(times[i]).tail<3>();
+    }
+  }
+
+  if (trajectory)
+  {
+    plotOrientation(times, points, label, &ref_points);
+  }
+  else
+  {
+    plotOrientation(times, points, label);
+  }
+}
+
+//-----------------------------------------------------------------------------
+void plotOrientationError(
+    const std::vector<real_t>& times,
+    const std::vector<Matrix3>& est,
+    const std::string& label,
+    const BSplinePoseMinimalRotationVector::Ptr trajectory)
+{
+  CHECK_EQ(times.size(), est.size());
+
+  Eigen::Matrix<real_t, 1, Eigen::Dynamic> err(1, est.size());
+
+  for (size_t i = 0; i < est.size(); ++i)
+  {
+    Quaternion q1(est[i]);
+    Quaternion q2(trajectory->orientation(times[i]));
+    err(i) = q1.getDisparityAngle(q2);
+  }
+  plt::figure("orientation_offset");
+  plt::title("Orientation Offset");
+  plt::labelPlot(label, times, err.row(0));
+  plt::legend();
+
+  plt::show(false);
+}
+
+//-----------------------------------------------------------------------------
+void plotImuMeasurements(
+    const std::vector<real_t>& times,
+    const ImuAccGyrContainer& measurements)
+{
+  CHECK_EQ(static_cast<int>(times.size()), measurements.cols());
+
+  plt::figure("imu_measurements");
+  plt::subplot(3, 1, 1);
+  plt::title("Imu Measurements (Accel)");
+  plt::plot(times, measurements.row(0));
+
+  plt::subplot(3, 1, 2);
+  plt::plot(times, measurements.row(1));
+
+  plt::subplot(3, 1, 3);
+  plt::plot(times, measurements.row(2));
+
+  plt::show(false);
+
+  plt::figure();
+  plt::subplot(3, 1, 1);
+  plt::title("Imu Measurements (Gyro)");
+  plt::plot(times, measurements.row(3));
+
+  plt::subplot(3, 1, 2);
+  plt::plot(times, measurements.row(4));
+
+  plt::subplot(3, 1, 3);
+  plt::plot(times, measurements.row(5));
+
+  plt::show(false);
+}
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_vi_simulation/include/ze/vi_simulation/imu_bias_simulator.hpp b/RWR/src/ze_oss/ze_vi_simulation/include/ze/vi_simulation/imu_bias_simulator.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..721c52c4744d40a8bf95676f7b7bc2b1712c4b67
--- /dev/null
+++ b/RWR/src/ze_oss/ze_vi_simulation/include/ze/vi_simulation/imu_bias_simulator.hpp
@@ -0,0 +1,139 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#pragma once
+
+#include <ze/common/types.hpp>
+#include <ze/common/random_matrix.hpp>
+#include <ze/splines/bspline.hpp>
+
+namespace ze {
+
+//! The base class for Imu Bias simulations.
+class ImuBiasSimulator
+{
+public:
+  ZE_POINTER_TYPEDEFS(ImuBiasSimulator);
+
+  //! Access the accelerometer bias at given timestamp.
+  virtual const Vector3 accelerometer(real_t t) const = 0;
+
+  //! Access the gyroscope bias at given timestamp.
+  virtual const Vector3 gyroscope(real_t t) const = 0;
+
+  //! Regenerate the bias.
+  virtual void reset() = 0;
+};
+
+//! A simple constant bias defined at all timestamps.
+class ConstantBiasSimulator : public ImuBiasSimulator
+{
+public:
+  ConstantBiasSimulator() = default;
+
+  ConstantBiasSimulator(const Vector3& bias_acc, const Vector3& bias_gyro) :
+      bias_acc_(bias_acc), bias_gyro_(bias_gyro) {
+  }
+
+  //! Get accelerometer bias at time t.
+  const Vector3 accelerometer(real_t t) const override
+  {
+    return bias_acc_;
+  }
+
+  //! Get gyroscope bias at time t.
+  const Vector3 gyroscope(real_t t) const override
+  {
+    return bias_gyro_;
+  }
+
+  void reset() override
+  {}
+
+private:
+  Vector3 bias_acc_ = Vector3::Zero();
+  Vector3 bias_gyro_ = Vector3::Zero();
+};
+
+//! A continous-time bias curve seeded from a discrete random walk.
+class ContinuousBiasSimulator : public ImuBiasSimulator
+{
+public:
+  //! Given the process noise, start/end times and number of samples to take
+  //! initializes a spline from a discrete random walk.
+  ContinuousBiasSimulator(
+      const Vector3& gyr_bias_noise_density,
+      const Vector3& acc_bias_noise_density,
+      real_t start_time,
+      real_t end_time,
+      size_t samples,
+      size_t spline_order = 3,
+      size_t spline_segments = 0,
+      size_t spline_smoothing_lambda = 1e-5);
+
+  //! Get accelerometer bias at time t.
+  const Vector3 accelerometer(real_t t) const
+  {
+    CHECK_GE(t, start_);
+    CHECK_LE(t, end_);
+    return bs_.eval(t).head<3>();
+  }
+
+  //! Get gyroscope bias at time t.
+  const Vector3 gyroscope(real_t t) const
+  {
+    CHECK_GE(t, start_);
+    CHECK_LE(t, end_);
+    return bs_.eval(t).tail<3>();
+  }
+
+  void reset()
+  {
+    initialize();
+  }
+
+private:
+  void initialize();
+
+  //! The noise density of the gyro / accelerometer brownian motion.
+  Vector3 gyr_bias_noise_density_;
+  Vector3 acc_bias_noise_density_;
+
+  //! Start and End-Time of the modelled continous bias.
+  real_t start_;
+  real_t end_;
+
+  size_t samples_;
+  size_t spline_order_;
+  size_t spline_segments_;
+  real_t spline_smoothing_lambda_;
+
+  //! The first three elements are the accelerometer bias, last 3 elements are
+  //! the gyrocope bias.
+  BSpline bs_;
+
+};
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_vi_simulation/include/ze/vi_simulation/imu_simulator.hpp b/RWR/src/ze_oss/ze_vi_simulation/include/ze/vi_simulation/imu_simulator.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..6556807c3779ab6edcd3f188f3fd28a8ec72f074
--- /dev/null
+++ b/RWR/src/ze_oss/ze_vi_simulation/include/ze/vi_simulation/imu_simulator.hpp
@@ -0,0 +1,117 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#pragma once
+
+// The below code and structure is heavily inspired by gtsam's
+// navigation/Scenario.h and ScenarioRunner.h
+
+#include <ze/vi_simulation/trajectory_simulator.hpp>
+#include <ze/vi_simulation/imu_bias_simulator.hpp>
+
+namespace ze {
+
+//! Given the trajectory defined by a Scenario, the runner generates
+//! the corresponding corrupted and actual imu measurements.
+//! The parameter naming follows the convention of a yaml-serialized
+//! imu model.
+class ImuSimulator
+{
+public:
+  ZE_POINTER_TYPEDEFS(ImuSimulator);
+
+  ImuSimulator(
+      const TrajectorySimulator::Ptr& trajectory,
+      const ImuBiasSimulator::Ptr& bias,
+      RandomVectorSampler<3>::Ptr accelerometer_noise,
+      RandomVectorSampler<3>::Ptr gyro_noise,
+      real_t accelerometer_noise_bandwidth_hz,
+      real_t gyroscope_noise_bandwidth_hz,
+      real_t gravity_magnitude)
+    : trajectory_(trajectory)
+    , bias_(bias)
+    , accelerometer_noise_(accelerometer_noise)
+    , gyro_noise_(gyro_noise)
+    , accelerometer_noise_bandwidth_hz_sqrt_(std::sqrt(accelerometer_noise_bandwidth_hz))
+    , gyro_noise_bandwidth_hz_sqrt_(std::sqrt(gyroscope_noise_bandwidth_hz))
+    , gravity_(Vector3(0, 0, gravity_magnitude))
+  {}
+
+  //! The gravity vector is fixed along Z axis.
+  const Vector3& gravity() const
+  {
+    return gravity_;
+  }
+
+  //! Get the angular velocity in the body frame.
+  Vector3 angularVelocityActual(real_t t) const
+  {
+    return trajectory_->angularVelocity_B(t);
+  }
+
+  //! An accelerometer measures the specific force (incl. gravity).
+  Vector3 specificForceActual(real_t t) const
+  {
+    Quaternion Rbw(trajectory_->R_W_B(t).inverse());
+    return trajectory_->acceleration_B(t) + Rbw.rotate(gravity());
+  }
+
+  //! The angular velocity corrupted by noise and bias.
+  Vector3 angularVelocityCorrupted(real_t t) const
+  {
+    return angularVelocityActual(t) + bias_->gyroscope(t) +
+        gyro_noise_->sample() * gyro_noise_bandwidth_hz_sqrt_;
+  }
+
+  //! The specific force corrupted by noise and bias.
+  Vector3 specificForceCorrupted(real_t t) const
+  {
+    return specificForceActual(t) + bias_->accelerometer(t) +
+        accelerometer_noise_->sample() *
+        accelerometer_noise_bandwidth_hz_sqrt_;
+  }
+
+  //! Gyro and accel bias.
+  const ImuBiasSimulator::Ptr& bias() const
+  {
+    return bias_;
+  }
+
+private:
+  const TrajectorySimulator::Ptr trajectory_;
+  const ImuBiasSimulator::Ptr bias_;
+
+  mutable RandomVectorSampler<3>::Ptr accelerometer_noise_;
+  mutable RandomVectorSampler<3>::Ptr gyro_noise_;
+
+  //! The noise bandwidth of accelerometer and gyroscope (sqrt(hz))
+  const real_t accelerometer_noise_bandwidth_hz_sqrt_;
+  const real_t gyro_noise_bandwidth_hz_sqrt_;
+
+  //! The gravity vector in the world frame (negative Z-axis).
+  const Vector3 gravity_;
+};
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_vi_simulation/include/ze/vi_simulation/trajectory_simulator.hpp b/RWR/src/ze_oss/ze_vi_simulation/include/ze/vi_simulation/trajectory_simulator.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..f682f0013f074c07a4372480e7760ffb570d589b
--- /dev/null
+++ b/RWR/src/ze_oss/ze_vi_simulation/include/ze/vi_simulation/trajectory_simulator.hpp
@@ -0,0 +1,130 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#pragma once
+
+// The below code and structure is heavily inspired by gtsam's
+// navigation/Scenario.h and ScenarioRunner.h
+
+#include <ze/common/macros.hpp>
+#include <ze/splines/bspline_pose_minimal.hpp>
+#include <ze/common/transformation.hpp>
+
+namespace ze {
+
+//! Represents a test scenario that describes a trajectory.
+class TrajectorySimulator
+{
+public:
+  ZE_POINTER_TYPEDEFS(TrajectorySimulator);
+
+  //! Get the pose at a given time.
+  virtual Transformation T_W_B(real_t t) const = 0;
+
+  //! Get the rotational velocity in the body frame.
+  virtual Vector3 angularVelocity_B(real_t t) const = 0;
+
+  //! Get the velocity in the world frame.
+  virtual Vector3 velocity_W(real_t t) const = 0;
+
+  //! Get the acceleration in the world frame (without gravity).
+  virtual Vector3 acceleration_W(real_t t) const = 0;
+
+  //! Start time of the scenario
+  virtual real_t start() const = 0;
+
+  //! End time of the scenario
+  virtual real_t end() const = 0;
+
+  //! Get the orientation in the world frame.
+  Quaternion R_W_B(real_t t) const
+  {
+    return T_W_B(t).getRotation();
+  }
+
+  //! The linear velocity in the body frame.
+  Vector3 velocity_B(real_t t) const
+  {
+    const Quaternion Rwb = R_W_B(t);
+    return Rwb.inverse().rotate(velocity_W(t));
+  }
+
+  //! The linear acceleration in the body frame (without gravity).
+  Vector3 acceleration_B(real_t t) const
+  {
+    const Quaternion Rwb = R_W_B(t);
+    return Rwb.inverse().rotate(acceleration_W(t));
+  }
+};
+
+//! A scenario that is based upon a bspline fitted trajectory.
+class SplineTrajectorySimulator : public TrajectorySimulator
+{
+public:
+  SplineTrajectorySimulator(const std::shared_ptr<BSplinePoseMinimalRotationVector>& bs)
+    : bs_(bs)
+  {}
+
+  //! Get pose at time t.
+  virtual Transformation T_W_B(real_t t) const override
+  {
+    return Transformation(bs_->transformation(t));
+  }
+
+  //! Get gyro measurement at time t (B_w_WB).
+  virtual Vector3 angularVelocity_B(real_t t) const override
+  {
+    return bs_->angularVelocityBodyFrame(t);
+  }
+
+  //! Get velocity in world coorinates at time t.
+  virtual  Vector3 velocity_W(real_t t) const override
+  {
+    return bs_->linearVelocity(t);
+  }
+
+  //! Get acceleration in world coordinates at time t (without gravity).
+  virtual Vector3 acceleration_W(real_t t) const override
+  {
+    return bs_->linearAcceleration(t);
+  }
+
+  //! Get start-time of trajectory.
+  virtual real_t start() const override
+  {
+    return bs_->t_min();
+  }
+
+  //! Get end time of trajectory.
+  virtual real_t end() const override
+  {
+    return bs_->t_max();
+  }
+
+private:
+  const std::shared_ptr<BSplinePoseMinimalRotationVector> bs_;
+};
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_vi_simulation/include/ze/vi_simulation/vi_simulator.hpp b/RWR/src/ze_oss/ze_vi_simulation/include/ze/vi_simulation/vi_simulator.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..c83f00ec5e12fc27b5a4b7e37272304a56ebee3e
--- /dev/null
+++ b/RWR/src/ze_oss/ze_vi_simulation/include/ze/vi_simulation/vi_simulator.hpp
@@ -0,0 +1,122 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#pragma once
+
+#include <memory>
+#include <ze/common/macros.hpp>
+#include <ze/common/transformation.hpp>
+#include <ze/vi_simulation/camera_simulator_types.hpp>
+
+namespace ze {
+
+// fwd
+class CameraRig;
+class CameraSimulator;
+struct CameraSimulatorOptions;
+class TrajectorySimulator;
+class ImuSimulator;
+class Visualizer;
+
+// -----------------------------------------------------------------------------
+struct ViSensorData
+{
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  //! Nanosecond timestamp.
+  int64_t timestamp;
+
+  //! Vector of keypoint measurements.
+  CameraMeasurementsVector cam_measurements;
+
+  //! Vector of imu timestamps since last camera measurement.
+  ImuStamps imu_stamps;
+
+  //! Inertial measurements since last camera measurement.
+  ImuAccGyrContainer imu_measurements;
+
+  //! Groundtruth states.
+  struct Groundtruth
+  {
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+    Transformation T_W_Bk;
+    Vector3 acc_bias;
+    Vector3 gyr_bias;
+    Vector3 linear_velocity_W;
+    Vector3 angular_velocity_B;
+  };
+  Groundtruth groundtruth;
+};
+
+// -----------------------------------------------------------------------------
+class ViSimulator
+{
+public:
+  ZE_POINTER_TYPEDEFS(ViSimulator);
+
+  ViSimulator(
+      const std::shared_ptr<TrajectorySimulator>& trajectory,
+      const std::shared_ptr<CameraRig>& camera_rig,
+      const CameraSimulatorOptions& camera_sim_options,
+      const real_t gyr_bias_noise_sigma = 0.0000266,
+      const real_t acc_bias_noise_sigma = 0.000433,
+      const real_t gyr_noise_sigma = 0.000186,
+      const real_t acc_noise_sigma = 0.00186,
+      const uint32_t cam_framerate_hz = 20,
+      const uint32_t imu_bandwidth_hz = 200,
+      const real_t gravity_magnitude = 9.81);
+
+  void initialize();
+
+  std::pair<ViSensorData, bool> getMeasurement();
+
+  void setVisualizer(const std::shared_ptr<Visualizer>& visualizer);
+
+  void visualize(
+      real_t dt = 0.2,
+      real_t marker_size_trajectory = 0.2,
+      real_t marker_size_landmarks = 0.2);
+
+private:
+  // Modules
+  std::shared_ptr<TrajectorySimulator> trajectory_;
+  std::shared_ptr<ImuSimulator> imu_;
+  std::shared_ptr<CameraSimulator> camera_;
+  std::shared_ptr<Visualizer> viz_;
+
+  // Sampling state
+  int64_t cam_dt_ns_;
+  int64_t imu_dt_ns_;
+  int64_t last_sample_stamp_ns_;
+  Transformation T_W_Bk_;
+};
+
+// -----------------------------------------------------------------------------
+
+//! Outdoor car scenario with stereo camera.
+ViSimulator::Ptr createViSimulationScenario1();
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_vi_simulation/package.xml b/RWR/src/ze_oss/ze_vi_simulation/package.xml
new file mode 100644
index 0000000000000000000000000000000000000000..6aecabbd390e1e4cd21f822a548fdb4580cd8896
--- /dev/null
+++ b/RWR/src/ze_oss/ze_vi_simulation/package.xml
@@ -0,0 +1,22 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>ze_vi_simulation</name>
+  <version>0.0.1</version>
+  <description>
+    Tools to simulate IMUs
+  </description>
+
+  <maintainer email="luc.oth@WyssZurich.ch">Luc Oth</maintainer>
+  <license>ZE</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <buildtool_depend>catkin_simple</buildtool_depend>
+  <depend>ze_cameras</depend>
+  <depend>ze_cmake</depend>
+  <depend>ze_common</depend>
+  <depend>ze_splines</depend>
+  <depend>ze_visualization</depend>
+  <depend>minkindr</depend>
+
+  <export></export>
+</package>
diff --git a/RWR/src/ze_oss/ze_vi_simulation/src/camera_simulator.cpp b/RWR/src/ze_oss/ze_vi_simulation/src/camera_simulator.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a79d62273acb87d8a8b1cda51cbe4cd7468469b3
--- /dev/null
+++ b/RWR/src/ze_oss/ze_vi_simulation/src/camera_simulator.cpp
@@ -0,0 +1,223 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include <ze/vi_simulation/camera_simulator.hpp>
+
+#include <ze/cameras/camera_rig.hpp>
+#include <ze/cameras/camera_utils.hpp>
+#include <ze/common/random_matrix.hpp>
+#include <ze/vi_simulation/trajectory_simulator.hpp>
+#include <ze/visualization/viz_interface.hpp>
+
+namespace ze {
+
+// -----------------------------------------------------------------------------
+void CameraSimulator::initializeMap()
+{
+  uint32_t num_frames = options_.max_num_landmarks_
+                         / options_.num_keypoints_per_frame;
+  real_t time = trajectory_->start();
+  real_t dt = (trajectory_->end() - time) / num_frames;
+  int num_landmarks_per_frame = options_.num_keypoints_per_frame / rig_->size();
+  uint32_t num_landmarks = 0u;
+  landmarks_W_.resize(Eigen::NoChange, options_.max_num_landmarks_);
+
+  for (uint32_t i = 0u; i < num_frames; ++i)
+  {
+    Transformation T_W_B = trajectory_->T_W_B(time);
+
+    for (uint32_t cam_idx = 0u; cam_idx < rig_->size(); ++cam_idx)
+    {
+      // Check how many landmarks are already visible:
+      CameraMeasurements measurements = visibleLandmarks(cam_idx, T_W_B, 0u, num_landmarks);
+      int num_visible = measurements.keypoints_.cols();
+      if (num_visible >= num_landmarks_per_frame)
+      {
+        continue;
+      }
+
+      // Initialize new random visible landmarks.
+      int32_t num_new_landmarks = std::max(0, num_landmarks_per_frame - num_visible);
+      CHECK_GE(num_new_landmarks, 0);
+
+      Positions p_C;
+      std::tie(std::ignore, std::ignore, p_C) =
+          generateRandomVisible3dPoints(
+            rig_->at(cam_idx), num_new_landmarks,
+            10u, options_.min_depth_m, options_.max_depth_m);
+
+      DEBUG_CHECK_LE(static_cast<int>(num_landmarks + num_new_landmarks),
+                     landmarks_W_.cols());
+
+      landmarks_W_.middleCols(num_landmarks, num_new_landmarks)
+          =  (T_W_B * rig_->T_B_C(cam_idx)).transformVectorized(p_C);
+
+      num_landmarks += num_new_landmarks;
+    }
+    time += dt;
+  }
+
+  VLOG(1) << "Initialized map with " << num_landmarks << " visible landmarks.";
+  landmarks_W_.conservativeResize(Eigen::NoChange, num_landmarks);
+}
+
+// -----------------------------------------------------------------------------
+CameraMeasurements CameraSimulator::visibleLandmarks(
+    const uint32_t cam_idx,
+    const Transformation& T_W_B,
+    const uint32_t lm_min_idx,
+    const uint32_t lm_max_idx)
+{
+  auto t = timer_[SimTimer::visible_landmarks].timeScope();
+
+  const uint32_t num_landmarks = lm_max_idx - lm_min_idx;
+  if (num_landmarks == 0)
+  {
+    return CameraMeasurements();
+  }
+
+  const Size2u image_size = rig_->at(cam_idx).size();
+  const auto lm_W = landmarks_W_.middleCols(lm_min_idx, num_landmarks);
+  const auto lm_C = (T_W_B * rig_->T_B_C(cam_idx)).inverse().transformVectorized(lm_W);
+  Keypoints px = rig_->at(cam_idx).projectVectorized(lm_C);
+  std::vector<uint32_t> visible_indices;
+  for (uint32_t i = 0u; i < num_landmarks; ++i)
+  {
+    if (lm_C(2,i) < options_.min_depth_m ||
+        lm_C(2,i) > options_.max_depth_m)
+    {
+      // Landmark is either behind or too far from the camera.
+      continue;
+    }
+
+    if (isVisible(image_size, px.col(i)))
+    {
+      visible_indices.push_back(i);
+    }
+  }
+
+  // Copy visible indices into Camera Measurements struct:
+  CameraMeasurements m;
+  m.keypoints_.resize(Eigen::NoChange, visible_indices.size());
+  m.global_landmark_ids_.resize(visible_indices.size());
+  for (size_t i = 0; i < visible_indices.size(); ++i)
+  {
+    m.keypoints_.col(i) = px.col(visible_indices[i]);
+    m.global_landmark_ids_[i] = visible_indices[i];
+  }
+
+  return m;
+}
+
+// -----------------------------------------------------------------------------
+CameraMeasurementsVector CameraSimulator::getMeasurements(real_t time)
+{
+  CHECK_GT(landmarks_W_.cols(), 0) << "Map has not been initialized.";
+  CHECK_GE(time, trajectory_->start());
+  CHECK_LT(time, trajectory_->end());
+
+  auto t = timer_[SimTimer::get_measurements].timeScope();
+
+  Transformation T_W_B = trajectory_->T_W_B(time);
+  std::unordered_map<int32_t, int32_t> new_global_lm_id_to_track_id_map;
+  CameraMeasurementsVector measurements;
+
+  for (uint32_t cam_idx = 0u; cam_idx < rig_->size(); ++cam_idx)
+  {
+    CameraMeasurements m = visibleLandmarks(cam_idx, T_W_B, 0u, landmarks_W_.cols());
+    m.local_track_ids_.resize(m.keypoints_.cols());
+    for (int32_t i = 0; i < m.keypoints_.cols(); ++i)
+    {
+      const int32_t lm_id = m.global_landmark_ids_[i];
+      int32_t track_id = -1;
+      auto res = global_lm_id_to_track_id_map_.find(lm_id);
+      if (res == global_lm_id_to_track_id_map_.end())
+      {
+        // This is a new track:
+        track_id = track_id_counter_;
+        ++track_id_counter_;
+      }
+      else
+      {
+        // This is an existing track:
+        track_id = res->second;
+      }
+      m.local_track_ids_[i] = track_id;
+
+      // Update our list of tracks:
+      new_global_lm_id_to_track_id_map[lm_id] = track_id;
+    }
+    measurements.push_back(m);
+  }
+
+  // Update our list of active tracks:
+  global_lm_id_to_track_id_map_ = new_global_lm_id_to_track_id_map;
+
+  return measurements;
+}
+
+// -----------------------------------------------------------------------------
+CameraMeasurementsVector CameraSimulator::getMeasurementsCorrupted(real_t time)
+{
+  CameraMeasurementsVector measurements = getMeasurements(time);
+  for (CameraMeasurements& m : measurements)
+  {
+    m.keypoints_ += randomMatrixNormalDistributed(2, m.keypoints_.cols(), false,
+                                                  0.0, options_.keypoint_noise_sigma);
+  }
+  return measurements;
+}
+
+// -----------------------------------------------------------------------------
+void CameraSimulator::reset()
+{
+  global_lm_id_to_track_id_map_.clear();
+}
+
+// -----------------------------------------------------------------------------
+void CameraSimulator::setVisualizer(const std::shared_ptr<Visualizer>& visualizer)
+{
+  viz_ = visualizer;
+}
+
+// -----------------------------------------------------------------------------
+void CameraSimulator::visualize(
+    real_t dt,
+    real_t marker_size_trajectory,
+    real_t marker_size_landmarks)
+{
+  CHECK_GT(landmarks_W_.cols(), 0) << "Map has not been initialized.";
+  CHECK(viz_) << "No visualizer has been registered.";
+  std::vector<Position> trajectory;
+  for (real_t time = trajectory_->start(), time_end = trajectory_->end();
+       time < time_end; time += dt)
+  {
+    trajectory.push_back(trajectory_->T_W_B(time).getPosition());
+  }
+  viz_->drawTrajectory("simulation_trajectory", 0, trajectory, Colors::Green, marker_size_trajectory);
+  viz_->drawPoints("simulated_map", 0u, landmarks_W_, Colors::Orange, marker_size_landmarks);
+}
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_vi_simulation/src/imu_bias_simulator.cpp b/RWR/src/ze_oss/ze_vi_simulation/src/imu_bias_simulator.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..2ac2ea6732f6825e20e917550b7af8c71c1005a7
--- /dev/null
+++ b/RWR/src/ze_oss/ze_vi_simulation/src/imu_bias_simulator.cpp
@@ -0,0 +1,88 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include <ze/vi_simulation/imu_bias_simulator.hpp>
+
+namespace ze {
+
+//------------------------------------------------------------------------------
+ContinuousBiasSimulator::ContinuousBiasSimulator(
+    const Vector3& gyr_bias_noise_density,
+    const Vector3& acc_bias_noise_density,
+    real_t start_time,
+    real_t end_time,
+    size_t samples,
+    size_t spline_order,
+    size_t spline_segments,
+    size_t spline_smoothing_lambda)
+  : gyr_bias_noise_density_(gyr_bias_noise_density)
+  , acc_bias_noise_density_(acc_bias_noise_density)
+  , start_(start_time)
+  , end_(end_time)
+  , samples_(samples)
+  , spline_order_(spline_order)
+  , spline_segments_(spline_segments)
+  , spline_smoothing_lambda_(spline_smoothing_lambda)
+  , bs_(3)
+{
+  if (spline_segments_ == 0)
+  {
+    // this is usually a good setting
+    spline_segments_ = samples / 2;
+  }
+  initialize();
+}
+
+//------------------------------------------------------------------------------
+void ContinuousBiasSimulator::initialize()
+{
+  Vector6 noise;
+  noise.head<3>() = acc_bias_noise_density_;
+  noise.tail<3>() = gyr_bias_noise_density_;
+  // merge acc and bias noise
+  RandomVectorSampler<6>::Ptr sampler = RandomVectorSampler<6>::sigmas(noise);
+
+  // sampling interval
+  real_t dt = (end_ - start_) / samples_;
+  real_t dt_sqrt = sqrt(dt);
+  CHECK_LE(0, dt);
+
+  // simulate the white noise process
+  MatrixX points(6, samples_ + 1);
+  VectorX times(samples_ + 1);
+  points.col(0) = Vector6::Zero();
+  times(0) = start_;
+  for (size_t i = 1; i <= samples_; ++i)
+  {
+    times(i) = start_ + dt * i;
+    points.col(i) = points.col(i-1) +
+        dt_sqrt * sampler->sample();
+  }
+
+  // initialize spline
+  bs_.initSpline3(times, points, spline_segments_, spline_smoothing_lambda_);
+}
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_vi_simulation/src/vi_simulator.cpp b/RWR/src/ze_oss/ze_vi_simulation/src/vi_simulator.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..eb5877cb2f6c7a5164376f10b549a7551cbad580
--- /dev/null
+++ b/RWR/src/ze_oss/ze_vi_simulation/src/vi_simulator.cpp
@@ -0,0 +1,202 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include <ze/vi_simulation/vi_simulator.hpp>
+
+#include <ze/cameras/camera_rig.hpp>
+#include <ze/common/test_utils.hpp>
+#include <ze/common/csv_trajectory.hpp>
+#include <ze/common/path_utils.hpp>
+#include <ze/vi_simulation/trajectory_simulator.hpp>
+#include <ze/vi_simulation/camera_simulator.hpp>
+#include <ze/vi_simulation/imu_simulator.hpp>
+#include <ze/visualization/viz_interface.hpp>
+
+namespace ze {
+
+// -----------------------------------------------------------------------------
+ViSimulator::ViSimulator(
+    const TrajectorySimulator::Ptr& trajectory,
+    const CameraRig::Ptr& camera_rig,
+    const CameraSimulatorOptions& camera_sim_options,
+    const real_t gyr_bias_noise_sigma,
+    const real_t acc_bias_noise_sigma,
+    const real_t gyr_noise_sigma,
+    const real_t acc_noise_sigma,
+    const uint32_t cam_framerate_hz,
+    const uint32_t imu_bandwidth_hz,
+    const real_t gravity_magnitude)
+  : trajectory_(trajectory)
+  , cam_dt_ns_(secToNanosec(1.0 / cam_framerate_hz))
+  , imu_dt_ns_(secToNanosec(1.0 / imu_bandwidth_hz))
+  , last_sample_stamp_ns_(secToNanosec(trajectory->start()))
+{
+  CHECK(imu_bandwidth_hz % cam_framerate_hz == 0);
+
+  ImuBiasSimulator::Ptr bias;
+  try
+  {
+    VLOG(1) << "Initialize bias ...";
+    bias = std::make_shared<ContinuousBiasSimulator>(
+             Vector3::Constant(gyr_bias_noise_sigma),
+             Vector3::Constant(acc_bias_noise_sigma),
+             trajectory->start(),
+             trajectory->end(),
+             100); // Results in malloc: (trajectory->end() - trajectory->start()) * imu_bandwidth_hz);
+    VLOG(1) << "done.";
+  }
+  catch (const std::bad_alloc& e)
+  {
+    LOG(FATAL) << "Could not create bias because number of samples is too high."
+               << " Allocation failed: " << e.what();
+  }
+
+  VLOG(1) << "Initialize IMU ...";
+  imu_ = std::make_shared<ImuSimulator>(
+           trajectory,
+           bias,
+           RandomVectorSampler<3>::sigmas(Vector3::Constant(acc_noise_sigma)),
+           RandomVectorSampler<3>::sigmas(Vector3::Constant(gyr_noise_sigma)),
+           imu_bandwidth_hz,
+           imu_bandwidth_hz,
+           gravity_magnitude);
+  VLOG(1) << "done.";
+
+  camera_ = std::make_shared<CameraSimulator>(
+              trajectory,
+              camera_rig,
+              camera_sim_options);
+}
+
+// -----------------------------------------------------------------------------
+void ViSimulator::initialize()
+{
+  VLOG(1) << "Initialize map ...";
+  camera_->initializeMap();
+  VLOG(1) << "done.";
+}
+
+// -----------------------------------------------------------------------------
+std::pair<ViSensorData, bool> ViSimulator::getMeasurement()
+{
+  int64_t new_img_stamp_ns = last_sample_stamp_ns_ + cam_dt_ns_;
+
+  real_t time_s = nanosecToSecTrunc(new_img_stamp_ns);
+  ViSensorData data;
+
+  if (time_s > trajectory_->end())
+  {
+    LOG(WARNING) << "Reached end of trajectory!";
+    return std::make_pair(data, false);
+  }
+
+  data.timestamp = new_img_stamp_ns;
+
+  // Get camera measurements:
+  data.cam_measurements = camera_->getMeasurementsCorrupted(time_s);
+
+  // Get groundtruth:
+  data.groundtruth.T_W_Bk = trajectory_->T_W_B(time_s);
+  data.groundtruth.linear_velocity_W = trajectory_->velocity_W(time_s);
+  data.groundtruth.angular_velocity_B = trajectory_->angularVelocity_B(time_s);
+  data.groundtruth.acc_bias = imu_->bias()->accelerometer(time_s);
+  data.groundtruth.gyr_bias = imu_->bias()->gyroscope(time_s);
+  T_W_Bk_ = data.groundtruth.T_W_Bk;
+
+  // Get inertial measurements:
+  uint64_t imu_stamp_ns = last_sample_stamp_ns_;
+  uint32_t num_imu_measurements = cam_dt_ns_ / imu_dt_ns_ + 1u;
+
+  data.imu_measurements.resize(Eigen::NoChange, num_imu_measurements);
+  data.imu_stamps.resize(num_imu_measurements);
+  for (uint32_t i = 0; i < num_imu_measurements; ++i)
+  {
+    data.imu_stamps(i) = imu_stamp_ns;
+    data.imu_measurements.block<3,1>(0, i) =
+        imu_->specificForceCorrupted(nanosecToSecTrunc(imu_stamp_ns));
+    data.imu_measurements.block<3,1>(3, i) =
+        imu_->angularVelocityCorrupted(nanosecToSecTrunc(imu_stamp_ns));
+    imu_stamp_ns += imu_dt_ns_;
+  }
+  DEBUG_CHECK_EQ(data.imu_stamps(data.imu_stamps.size()-1), new_img_stamp_ns);
+
+  // Prepare next iteration:
+  last_sample_stamp_ns_ = new_img_stamp_ns;
+
+  return std::make_pair(data, true);
+}
+
+// -----------------------------------------------------------------------------
+void ViSimulator::setVisualizer(const std::shared_ptr<Visualizer>& visualizer)
+{
+  viz_ = visualizer;
+  camera_->setVisualizer(viz_);
+}
+
+// -----------------------------------------------------------------------------
+void ViSimulator::visualize(
+    real_t dt,
+    real_t marker_size_trajectory,
+    real_t marker_size_landmarks)
+{
+  camera_->visualize(dt, marker_size_trajectory, marker_size_landmarks);
+  viz_->drawCoordinateFrame("simulated_camera", 0u, T_W_Bk_, marker_size_trajectory * 5);
+}
+
+// -----------------------------------------------------------------------------
+ViSimulator::Ptr createViSimulationScenario1()
+{
+  // Create trajectory:
+  PoseSeries pose_series;
+  pose_series.load(joinPath(getTestDataDir("ze_applanix_gt_data"), "traj_es.csv"));
+  StampedTransformationVector poses = pose_series.getStampedTransformationVector();
+
+  // Set start time of trajectory to zero.
+  const int64_t offset = poses.at(0).first;
+  for (StampedTransformation& it : poses)
+  {
+    it.first -= offset;
+  }
+
+  std::shared_ptr<BSplinePoseMinimalRotationVector> bs =
+      std::make_shared<BSplinePoseMinimalRotationVector>(3);
+  bs->initPoseSplinePoses(poses, 100, 0.5);
+  TrajectorySimulator::Ptr trajectory = std::make_shared<SplineTrajectorySimulator>(bs);
+
+  // Create camera simulation:
+  CameraRig::Ptr rig = cameraRigFromYaml(joinPath(getTestDataDir("camera_models"),
+                                                  "camera_rig_3.yaml"));
+  // Create Vi-Simulator:
+  CameraSimulatorOptions cam_sim_options;
+  cam_sim_options.min_depth_m = 4.0;
+  cam_sim_options.max_depth_m = 10.0;
+  cam_sim_options.max_num_landmarks_ = 20000;
+  ViSimulator::Ptr vi_sim =
+      std::make_shared<ViSimulator>(trajectory, rig, cam_sim_options);
+  vi_sim->initialize();
+  return vi_sim;
+}
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_vi_simulation/test/test_camera_simulator.cpp b/RWR/src/ze_oss/ze_vi_simulation/test/test_camera_simulator.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..45d20d562b7e53f2e0de6aec8c4f77a57fc35ebc
--- /dev/null
+++ b/RWR/src/ze_oss/ze_vi_simulation/test/test_camera_simulator.cpp
@@ -0,0 +1,103 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include <ze/vi_simulation/camera_simulator.hpp>
+#include <ze/vi_simulation/trajectory_simulator.hpp>
+#include <ze/cameras/camera_rig.hpp>
+#include <ze/common/csv_trajectory.hpp>
+#include <ze/common/test_entrypoint.hpp>
+#include <ze/common/test_utils.hpp>
+#include <ze/common/types.hpp>
+#include <ze/common/path_utils.hpp>
+#include <ze/common/random_matrix.hpp>
+#include <ze/splines/bspline_pose_minimal.hpp>
+#include <ze/visualization/viz_ros.hpp>
+#ifdef ZE_USE_OPENCV
+#include <opencv2/imgproc/imgproc.hpp>
+#include <opencv2/highgui/highgui.hpp>
+#endif
+
+TEST(CameraSimulator, testSplineScenario)
+{
+  using namespace ze;
+
+  // Create trajectory:
+  PoseSeries pose_series;
+  pose_series.load(joinPath(getTestDataDir("ze_applanix_gt_data"), "traj_es.csv"));
+  StampedTransformationVector poses = pose_series.getStampedTransformationVector();
+
+  std::shared_ptr<BSplinePoseMinimalRotationVector> bs =
+      std::make_shared<BSplinePoseMinimalRotationVector>(3);
+  bs->initPoseSplinePoses(poses, 100, 0.5);
+  TrajectorySimulator::Ptr trajectory = std::make_shared<SplineTrajectorySimulator>(bs);
+
+  // Create camera:
+  CameraRig::Ptr rig = cameraRigFromYaml(joinPath(getTestDataDir("camera_models"),
+                                                  "camera_rig_3.yaml"));
+
+  // Create visualizer:
+  Visualizer::Ptr visualizer = std::make_shared<VisualizerRos>();
+
+  // Create camera simulator:
+  CameraSimulatorOptions options;
+  options.min_depth_m = 4.0;
+  options.max_depth_m = 10.0;
+  options.max_num_landmarks_ = 20000;
+  CameraSimulator cam_sim(trajectory, rig, options);
+  cam_sim.setVisualizer(visualizer);
+  cam_sim.initializeMap();
+  for (int i = 0; i < 100; ++i)
+  {
+    cam_sim.visualize(1.0, 4.0, 0.3);
+  }
+
+#ifdef ZE_USE_OPENCV
+  cv::Mat img_0(rig->at(0).height(), rig->at(0).width(), CV_8UC1, cv::Scalar(0));
+#endif
+
+  // Loop through trajectory and visualize feature tracks:
+  for (int j = 0; j < 1000; ++j)
+  {
+    real_t time = cam_sim.trajectory().start() + j * 1.0/20.0;
+    const CameraMeasurementsVector& m_vec = cam_sim.getMeasurements(time);
+#ifdef ZE_USE_OPENCV
+    if (false)
+    {
+      const CameraMeasurements& m = m_vec[0];
+      for (int i = 0; i < m.keypoints_.cols(); ++i)
+      {
+        cv::circle(img_0, cv::Point(m.keypoints_(0,i), m.keypoints_(1,i)), 3,
+                   cv::Scalar(m.local_track_ids_[i] % 255), 3);
+      }
+      cv::imshow("img_0", img_0);
+      cv::waitKey(1);
+    }
+#endif
+  }
+
+  VLOG(1) << "Timing results: \n" << cam_sim.timer_;
+}
+
+ZE_UNITTEST_ENTRYPOINT
diff --git a/RWR/src/ze_oss/ze_vi_simulation/test/test_imu_bias_simulator.cpp b/RWR/src/ze_oss/ze_vi_simulation/test/test_imu_bias_simulator.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..9cacfb2b19705f6a32d35c742262b9bce41f187a
--- /dev/null
+++ b/RWR/src/ze_oss/ze_vi_simulation/test/test_imu_bias_simulator.cpp
@@ -0,0 +1,52 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include <ze/vi_simulation/imu_bias_simulator.hpp>
+#include <ze/common/test_entrypoint.hpp>
+
+TEST(ImuBiasTest, testConstantImuBias)
+{
+  using namespace ze;
+
+  Vector3 acc_bias(1, 2, 3);
+  Vector3 gyr_bias(4, 5, 6);
+  ConstantBiasSimulator bias(acc_bias, gyr_bias);
+  EXPECT_TRUE(EIGEN_MATRIX_NEAR(acc_bias, bias.accelerometer(0), 1e-8));
+  EXPECT_TRUE(EIGEN_MATRIX_NEAR(gyr_bias, bias.gyroscope(0), 1e-8));
+  EXPECT_TRUE(EIGEN_MATRIX_NEAR(acc_bias, bias.accelerometer(10), 1e-8));
+  EXPECT_TRUE(EIGEN_MATRIX_NEAR(gyr_bias, bias.gyroscope(10), 1e-8));
+}
+
+TEST(ImuBiasTest, testContinuousBias)
+{
+  using namespace ze;
+  ContinuousBiasSimulator bias(Vector3(1e-4, 1e-4, 1e-4),
+                      Vector3(1e-3, 1e-3, 1e-3),
+                      10, 20, 200);
+  bias.accelerometer(10);
+  bias.gyroscope(20);
+}
+
+ZE_UNITTEST_ENTRYPOINT
diff --git a/RWR/src/ze_oss/ze_vi_simulation/test/test_imu_simulator.cpp b/RWR/src/ze_oss/ze_vi_simulation/test/test_imu_simulator.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..3b1e70e38005b245faba4910842a7c2c9ced9c5c
--- /dev/null
+++ b/RWR/src/ze_oss/ze_vi_simulation/test/test_imu_simulator.cpp
@@ -0,0 +1,124 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include <ze/vi_simulation/imu_bias_simulator.hpp>
+#include <ze/vi_simulation/imu_simulator.hpp>
+#include <ze/vi_simulation/trajectory_simulator.hpp>
+#include <ze/common/test_entrypoint.hpp>
+#include <ze/splines/bspline_pose_minimal.hpp>
+#include <ze/common/types.hpp>
+#include <ze/common/random_matrix.hpp>
+
+TEST(TrajectorySimulator, testSplineScenario)
+{
+  using namespace ze;
+
+  std::shared_ptr<BSplinePoseMinimalRotationVector> bs =
+      std::make_shared<BSplinePoseMinimalRotationVector>(3);
+
+
+  bs->initPoseSpline(10.0, 20.0,
+                     bs->curveValueToTransformation(Vector6::Random()),
+                     bs->curveValueToTransformation(Vector6::Random()));
+
+  SplineTrajectorySimulator::Ptr scenario =
+      std::make_shared<SplineTrajectorySimulator>(bs);
+
+  // dependencies:
+  ImuBiasSimulator::Ptr bias(std::make_shared<ConstantBiasSimulator>());
+  RandomVectorSampler<3>::Ptr acc_noise =
+      RandomVectorSampler<3>::sigmas(Vector3(1e-5, 1e-5, 1e-5));
+  RandomVectorSampler<3>::Ptr gyr_noise =
+      RandomVectorSampler<3>::sigmas(Vector3(1e-5, 1e-5, 1e-5));
+  real_t gravity_magnitude = 9.81;
+
+  // test runner
+  ImuSimulator imu_simulator(
+        scenario, bias, acc_noise, gyr_noise,
+        100, 100, gravity_magnitude);
+
+  for (real_t t = 10.0; t < 20.0; t += 0.1)
+  {
+    // the probability is really high that this test passes... but not guaranteed
+    EXPECT_TRUE(EIGEN_MATRIX_NEAR(
+                  imu_simulator.angularVelocityCorrupted(t),
+                  imu_simulator.angularVelocityActual(t),
+                  0.3));
+    EXPECT_TRUE(EIGEN_MATRIX_NEAR(
+                  imu_simulator.specificForceActual(t),
+                  imu_simulator.specificForceCorrupted(t),
+                  0.3));
+  }
+}
+
+TEST(TrajectorySimulator, testConsistency)
+{
+  using namespace ze;
+
+  std::shared_ptr<BSplinePoseMinimalRotationVector> bs =
+      std::make_shared<BSplinePoseMinimalRotationVector>(3);
+
+  Vector6 v1; v1 << 0, 0, 0, 1, 1, 1.;
+  Vector6 v2; v2 << 0, 0, 0, 1, 1, 1.;
+
+  bs->initPoseSpline(10.0, 20.0,
+                     bs->curveValueToTransformation(v1),
+                     bs->curveValueToTransformation(v2));
+
+  SplineTrajectorySimulator::Ptr scenario =
+      std::make_shared<SplineTrajectorySimulator>(bs);
+
+  // dependencies:
+  ImuBiasSimulator::Ptr bias(std::make_shared<ConstantBiasSimulator>());
+  RandomVectorSampler<3>::Ptr acc_noise =
+      RandomVectorSampler<3>::sigmas(Vector3(0, 0, 0));
+  RandomVectorSampler<3>::Ptr gyr_noise =
+      RandomVectorSampler<3>::sigmas(Vector3(0, 0, 0));
+  real_t gravity_magnitude = 9.81;
+
+  // test runner
+  ImuSimulator imu_simulator(
+        scenario, bias, acc_noise, gyr_noise, 100, 100, gravity_magnitude);
+
+  EXPECT_TRUE(EIGEN_MATRIX_NEAR(
+                imu_simulator.specificForceActual(11),
+                Vector3(9.38534, -1.7953, 2.219960), 1e-4));
+
+  Matrix3 R_B_I;
+  R_B_I.col(2) = imu_simulator.specificForceActual(11).normalized();
+  R_B_I.col(0) = - (R_B_I.col(2).cross(Vector3::UnitY())).normalized();
+  R_B_I.col(1) = R_B_I.col(2).cross(R_B_I.col(0));
+  R_B_I = R_B_I.transpose().eval();
+
+  EXPECT_TRUE(EIGEN_MATRIX_NEAR(
+                R_B_I.eulerAngles(2, 1, 0),
+                Vector3(0.6585, -1.27549, -0.68003), 1e-4));
+  EXPECT_TRUE(EIGEN_MATRIX_NEAR(
+                bs->orientation(15).eulerAngles(2, 1, 0),
+                Vector3(2.46156, -1.86611, 2.46156), 1e-4));
+
+}
+
+ZE_UNITTEST_ENTRYPOINT
diff --git a/RWR/src/ze_oss/ze_vi_simulation/test/test_trajectory_simulator.cpp b/RWR/src/ze_oss/ze_vi_simulation/test/test_trajectory_simulator.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..2b93916e14c55104968bee39bc8ed3c2be0ae046
--- /dev/null
+++ b/RWR/src/ze_oss/ze_vi_simulation/test/test_trajectory_simulator.cpp
@@ -0,0 +1,61 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include <ze/vi_simulation/imu_simulator.hpp>
+#include <ze/common/test_entrypoint.hpp>
+#include <ze/splines/bspline_pose_minimal.hpp>
+#include <ze/common/types.hpp>
+
+TEST(ImuSimulator, testSplineScenario)
+{
+  using namespace ze;
+
+  std::shared_ptr<BSplinePoseMinimalRotationVector> bs =
+      std::make_shared<BSplinePoseMinimalRotationVector>(3);
+
+  bs->initPoseSpline(10.0, 20.0,
+                     bs->curveValueToTransformation(Vector6::Random()),
+                     bs->curveValueToTransformation(Vector6::Random()));
+
+  SplineTrajectorySimulator scenario(bs);
+
+  EXPECT_TRUE(EIGEN_MATRIX_NEAR(bs->transformation(11),
+                                scenario.T_W_B(11).getTransformationMatrix(),
+                                1e-8));
+  EXPECT_TRUE(EIGEN_MATRIX_NEAR(bs->angularVelocityBodyFrame(11),
+                                scenario.angularVelocity_B(11),
+                                1e-8));
+  EXPECT_TRUE(EIGEN_MATRIX_NEAR(bs->linearVelocity(11),
+                                scenario.velocity_W(11),
+                                1e-8));
+  EXPECT_TRUE(EIGEN_MATRIX_NEAR(bs->linearAcceleration(11),
+                                scenario.acceleration_W(11),
+                                1e-8));
+  EXPECT_TRUE(EIGEN_MATRIX_NEAR(bs->orientation(11),
+                                scenario.R_W_B(11).getRotationMatrix(),
+                                1e-8));
+}
+
+ZE_UNITTEST_ENTRYPOINT
diff --git a/RWR/src/ze_oss/ze_vi_simulation/test/test_vi_simulator.cpp b/RWR/src/ze_oss/ze_vi_simulation/test/test_vi_simulator.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..9a770debda4cf176fa239a29728b78f99a41e809
--- /dev/null
+++ b/RWR/src/ze_oss/ze_vi_simulation/test/test_vi_simulator.cpp
@@ -0,0 +1,58 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include <ze/common/test_entrypoint.hpp>
+#include <ze/common/timer_statistics.hpp>
+#include <ze/vi_simulation/camera_simulator.hpp>
+#include <ze/vi_simulation/vi_simulator.hpp>
+#include <ze/visualization/viz_ros.hpp>
+
+TEST(CameraSimulator, testSplineScenario)
+{
+  using namespace ze;
+
+  Visualizer::Ptr viz = std::make_shared<VisualizerRos>();
+  ViSimulator::Ptr sim = createViSimulationScenario1();
+  sim->setVisualizer(viz);
+
+  ViSensorData data;
+  bool success;
+  std::tie(data, success) = sim->getMeasurement();
+  int64_t last_cam_stamp = data.timestamp;
+  TimerStatistics timer;
+  for (int i = 0; i < 2000; ++i)
+  {
+    auto t = timer.timeScope();
+    std::tie(data, success) = sim->getMeasurement();
+    EXPECT_TRUE(success);
+    EXPECT_EQ(data.imu_stamps(0), last_cam_stamp);
+    EXPECT_EQ(data.imu_stamps(data.imu_stamps.size()-1), data.timestamp);
+    last_cam_stamp = data.timestamp;
+    sim->visualize(1.0, 4.0, 0.3);
+  }
+  VLOG(1) << "Average time per frame = " << timer.mean() << " milliseconds.";
+}
+
+ZE_UNITTEST_ENTRYPOINT
diff --git a/RWR/src/ze_oss/ze_visualization/CMakeLists.txt b/RWR/src/ze_oss/ze_visualization/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..4846f539bd2074a1c71ad6b1b3a2ff711cb97d05
--- /dev/null
+++ b/RWR/src/ze_oss/ze_visualization/CMakeLists.txt
@@ -0,0 +1,37 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(ze_visualization)
+
+find_package(catkin_simple REQUIRED)
+find_package(OpenCV REQUIRED)
+catkin_simple(ALL_DEPS_REQUIRED)
+
+include(ze_setup)
+
+#############
+# LIBRARIES #
+#############
+set(HEADERS
+  include/ze/visualization/viz_common.hpp
+  include/ze/visualization/viz_interface.hpp
+  include/ze/visualization/viz_ros.hpp
+  include/ze/visualization/viz_ros_utils.hpp
+  )
+
+set(SOURCES
+  src/viz_common.cpp 
+  src/viz_ros.cpp
+  )
+
+cs_add_library(${PROJECT_NAME} ${SOURCES} ${HEADERS})
+
+###############
+# EXECUTABLES #
+###############
+cs_add_executable(viz_example_node src/viz_example_node.cpp)
+target_link_libraries(viz_example_node ${PROJECT_NAME} ${OpenCV_LIBRARIES})
+
+##########
+# EXPORT #
+##########
+cs_install()
+cs_export()
diff --git a/RWR/src/ze_oss/ze_visualization/include/ze/visualization/viz_common.hpp b/RWR/src/ze_oss/ze_visualization/include/ze/visualization/viz_common.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..bd9dd4c3ab7179fa8d45c96d17477c557f287eb4
--- /dev/null
+++ b/RWR/src/ze_oss/ze_visualization/include/ze/visualization/viz_common.hpp
@@ -0,0 +1,63 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#pragma once
+
+#include <ze/common/types.hpp>
+
+namespace ze {
+
+struct Color
+{
+  real_t r = 0.0f;
+  real_t g = 0.0f;
+  real_t b = 0.0f;
+  real_t a = 1.0f;
+
+  constexpr Color(real_t r, real_t g, real_t b)
+    : r(r), g(g), b(b)
+  {}
+
+  constexpr Color(real_t r, real_t g, real_t b, real_t a)
+    : r(r), g(g), b(b), a(a)
+  {}
+};
+
+struct Colors
+{
+  static constexpr Color Red {1.0, 0.0, 0.0};
+  static constexpr Color Green {0.0, 1.0, 0.0};
+  static constexpr Color Blue {0.0, 0.0, 1.0};
+  static constexpr Color DarkRed {0.5, 0.0, 0.0};
+  static constexpr Color DarkGreen {0.0, 0.5, 0.0};
+  static constexpr Color DarkBlue {0.0, 0.0, 0.5};
+  static constexpr Color White {1.0, 1.0, 1.0};
+  static constexpr Color LightGray {0.8, 0.8, 0.8};
+  static constexpr Color Yellow {1.0, 1.0, 0.0};
+  static constexpr Color Magenta {1.0, 0.0, 1.0};
+  static constexpr Color Orange {1.0, 0.5, 0.0};
+};
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_visualization/include/ze/visualization/viz_interface.hpp b/RWR/src/ze_oss/ze_visualization/include/ze/visualization/viz_interface.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..c6264d22d8e078f0936e9bdfc78a06181aea2ac3
--- /dev/null
+++ b/RWR/src/ze_oss/ze_visualization/include/ze/visualization/viz_interface.hpp
@@ -0,0 +1,109 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#pragma once
+
+#include <string>
+#include <tuple>
+
+#include <ze/common/macros.hpp>
+#include <ze/common/types.hpp>
+#include <ze/common/transformation.hpp>
+#include <ze/visualization/viz_common.hpp>
+
+namespace ze {
+
+using LineMarkers = std::vector<std::pair<Position, Position>, Eigen::aligned_allocator<std::pair<Position, Position>>>;
+
+//! Interface class for all visualizations.
+class Visualizer
+{
+public:
+  ZE_POINTER_TYPEDEFS(Visualizer);
+
+  Visualizer() = default;
+  virtual ~Visualizer() = default;
+
+  // ---------------------------------------------------------------------------
+  // Draw single elements
+
+  virtual void drawPoint(
+      const std::string& topic,
+      const size_t id,
+      const Position& point,
+      const Color& color,
+      const real_t size = 0.02) = 0;
+
+  virtual void drawLine(
+      const std::string& topic,
+      const size_t id,
+      const Position& line_from,
+      const Position& line_to,
+      const Color& color,
+      const real_t size = 0.02) = 0;
+
+  virtual void drawCoordinateFrame(
+      const std::string& topic,
+      const size_t id,
+      const Transformation& pose, // T_W_B
+      const real_t size = 0.02) = 0;
+
+  virtual void drawRobot(
+      const std::string& name,
+      const Transformation& T_W_B) = 0;
+
+  // ---------------------------------------------------------------------------
+  // Draw multiple elements
+
+  virtual void drawPoints(
+      const std::string& topic,
+      const size_t id,
+      const Positions& points,
+      const Color& color,
+      const real_t size = 0.02) = 0;
+
+  virtual void drawLines(
+      const std::string& topic,
+      const size_t id,
+      const LineMarkers& lines,
+      const Color& color,
+      const real_t size = 0.02) = 0;
+
+  virtual void drawCoordinateFrames(
+      const std::string& topic,
+      const size_t id,
+      const TransformationVector& poses,
+      const real_t size = 0.02) = 0;
+
+  virtual void drawTrajectory(
+      const std::string& topic,
+      const size_t id,
+      const std::vector<Position>& points,
+      const Color& color,
+      const real_t size = 0.02) = 0;
+
+};
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_visualization/include/ze/visualization/viz_ros.hpp b/RWR/src/ze_oss/ze_visualization/include/ze/visualization/viz_ros.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..c6e506458d69979f22d99c8d0e3aafe8f0f3ca9a
--- /dev/null
+++ b/RWR/src/ze_oss/ze_visualization/include/ze/visualization/viz_ros.hpp
@@ -0,0 +1,120 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#pragma once
+
+#include <memory>
+#include <ze/visualization/viz_interface.hpp>
+
+// fwd
+namespace ros {
+class Publisher;
+class NodeHandle;
+}
+
+namespace tf {
+class TransformBroadcaster;
+}
+
+namespace ze {
+
+//! @todo(cfo): Deal with multi-threaded publishing.
+class VisualizerRos : public Visualizer
+{
+public:
+
+  VisualizerRos();
+  VisualizerRos(const std::string& frame);
+
+  virtual ~VisualizerRos() = default;
+
+  // ---------------------------------------------------------------------------
+  // Draw single elements
+
+  virtual void drawPoint(
+      const std::string& ns,
+      const size_t id,
+      const Position& point,
+      const Color& color,
+      const real_t size = 0.02) override;
+
+  virtual void drawLine(
+      const std::string& ns,
+      const size_t id,
+      const Position& line_from,
+      const Position& line_to,
+      const Color& color,
+      const real_t size = 0.02) override;
+
+  virtual void drawCoordinateFrame(
+      const std::string& ns,
+      const size_t id,
+      const Transformation& pose, // T_W_B
+      const real_t size = 0.2) override;
+
+  virtual void drawRobot(
+      const std::string& name,
+      const Transformation& T_W_B) override;
+
+  // ---------------------------------------------------------------------------
+  // Draw multiple elements
+
+  virtual void drawPoints(
+      const std::string& ns,
+      const size_t id,
+      const Positions& points,
+      const Color& color,
+      const real_t size = 0.02) override;
+
+  virtual void drawLines(
+      const std::string& ns,
+      const size_t id,
+      const LineMarkers& lines,
+      const Color& color,
+      const real_t size = 0.02) override;
+
+  virtual void drawCoordinateFrames(
+      const std::string& ns,
+      const size_t id,
+      const TransformationVector& poses,
+      const real_t size = 0.2) override;
+
+  virtual void drawTrajectory(
+      const std::string& topic,
+      const size_t id,
+      const std::vector<Position>& points,
+      const Color& color,
+      const real_t size = 0.02) override;
+
+private:
+  std::shared_ptr<ros::NodeHandle> nh_;
+  std::shared_ptr<ros::Publisher> pub_marker_;
+  std::shared_ptr<tf::TransformBroadcaster> tf_broadcaster_;
+  std::string world_frame = "map";    //!< World-frame
+  double viz_scale_ = 1.0;            //!< Scale marker size
+};
+
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_visualization/include/ze/visualization/viz_ros_utils.hpp b/RWR/src/ze_oss/ze_visualization/include/ze/visualization/viz_ros_utils.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..e9a64942ba83a8dd1912df1c30bacefd654cbe4d
--- /dev/null
+++ b/RWR/src/ze_oss/ze_visualization/include/ze/visualization/viz_ros_utils.hpp
@@ -0,0 +1,71 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#pragma once
+
+#include <visualization_msgs/Marker.h>
+#include <ze/common/types.hpp>
+#include <ze/common/transformation.hpp>
+
+namespace ze {
+
+inline std_msgs::ColorRGBA getRosColor(const Color& color)
+{
+  std_msgs::ColorRGBA c;
+  c.r = color.r;
+  c.g = color.g;
+  c.b = color.b;
+  c.a = color.a;
+  return c;
+}
+
+inline geometry_msgs::Point getRosPoint(const Eigen::Ref<const Position>& point)
+{
+  geometry_msgs::Point p;
+  p.x = point(0);
+  p.y = point(1);
+  p.z = point(2);
+  return p;
+}
+
+inline geometry_msgs::Quaternion getRosQuaternion(const Quaternion& rot)
+{
+  geometry_msgs::Quaternion q;
+  q.x = rot.toImplementation().x();
+  q.y = rot.toImplementation().y();
+  q.z = rot.toImplementation().z();
+  q.w = rot.toImplementation().w();
+  return q;
+}
+
+inline geometry_msgs::Pose getRosPose(const Transformation& pose)
+{
+  geometry_msgs::Pose T;
+  T.position = getRosPoint(pose.getPosition());
+  T.orientation = getRosQuaternion(pose.getRotation());
+  return T;
+}
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_visualization/package.xml b/RWR/src/ze_oss/ze_visualization/package.xml
new file mode 100644
index 0000000000000000000000000000000000000000..b1e7a55606d9d73b6c8278e42e30e61b87dc1f44
--- /dev/null
+++ b/RWR/src/ze_oss/ze_visualization/package.xml
@@ -0,0 +1,27 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>ze_visualization</name>
+  <version>0.1.4</version>
+  <description>The ze_visualization package</description>
+
+  <maintainer email="christian.forster@WyssZurich.ch">Christian Forster</maintainer>
+  <license>ZE</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <buildtool_depend>catkin_simple</buildtool_depend>
+
+  <depend>eigen_catkin</depend>
+  <depend>glog_catkin</depend>
+  <depend>imp_bridge_opencv</depend>
+  <depend>minkindr</depend>
+  <depend>ze_cmake</depend>
+  <depend>ze_common</depend>
+  <depend>ze_ros</depend>
+  
+  <depend>roscpp</depend>
+  <depend>tf</depend>
+  <depend>visualization_msgs</depend>
+  <depend>image_transport</depend>
+  
+  <test_depend>gtest</test_depend>
+</package>
diff --git a/RWR/src/ze_oss/ze_visualization/src/viz_common.cpp b/RWR/src/ze_oss/ze_visualization/src/viz_common.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..759b9923115e0495c2cafa5ff51431a7ae8a958b
--- /dev/null
+++ b/RWR/src/ze_oss/ze_visualization/src/viz_common.cpp
@@ -0,0 +1,42 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include <ze/visualization/viz_common.hpp>
+
+namespace ze {
+
+constexpr Color Colors::Red;
+constexpr Color Colors::Green;
+constexpr Color Colors::Blue;
+constexpr Color Colors::DarkRed;
+constexpr Color Colors::DarkGreen;
+constexpr Color Colors::DarkBlue;
+constexpr Color Colors::White;
+constexpr Color Colors::LightGray;
+constexpr Color Colors::Yellow;
+constexpr Color Colors::Magenta;
+constexpr Color Colors::Orange;
+
+} // namespace ze
diff --git a/RWR/src/ze_oss/ze_visualization/src/viz_example_node.cpp b/RWR/src/ze_oss/ze_visualization/src/viz_example_node.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..01d22d758acf4feba6db1c1578377a092f6ebb3f
--- /dev/null
+++ b/RWR/src/ze_oss/ze_visualization/src/viz_example_node.cpp
@@ -0,0 +1,73 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include <ros/ros.h>
+#include <ze/common/logging.hpp>
+#include <gflags/gflags.h>
+
+#include <ze/visualization/viz_ros.hpp>
+
+int main(int argc, char** argv)
+{
+  google::InitGoogleLogging(argv[0]);
+  google::ParseCommandLineFlags(&argc, &argv, true);
+
+  // Visualizer internally initializes ROS and creates a node handle.
+  ze::VisualizerRos visualizer;
+
+  for(int i = 0; i < 100; ++i)
+  {
+    visualizer.drawPoint(
+          "point", i, ze::Vector3(0, 0, i), ze::Colors::DarkBlue);
+
+    visualizer.drawLine(
+          "line", i, ze::Vector3(i, 0, 0), ze::Vector3(i, 0.4, 0), ze::Colors::DarkRed);
+
+    visualizer.drawCoordinateFrame(
+          "frame", i, ze::Transformation(ze::Quaternion(), ze::Vector3(i, i, i)*0.2));
+
+    ze::Positions points(3, 4);
+    points << 0, 1, 2, 3,
+              1, 1, 1, 1,
+              i, i, i, i;
+    visualizer.drawPoints("points", 0, points, ze::Colors::Blue);
+
+    ze::LineMarkers lines;
+    lines.push_back(std::make_pair(ze::Position(i, 0, 1), ze::Position(i, 0, 0)));
+    lines.push_back(std::make_pair(ze::Position(i, 1, 1), ze::Position(i, 1, 0)));
+    lines.push_back(std::make_pair(ze::Position(i, 2, 1), ze::Position(i, 2, 0)));
+    lines.push_back(std::make_pair(ze::Position(i, 3, 1), ze::Position(i, 3, 0)));
+    visualizer.drawLines("lines", 0, lines, ze::Colors::Green);
+
+    ze::TransformationVector poses;
+    poses.push_back(ze::Transformation(ze::Quaternion(ze::Vector3(0.1, 0, 0)), ze::Position(i, 0, 0)));
+    poses.push_back(ze::Transformation(ze::Quaternion(ze::Vector3(0.2, 0, 0)), ze::Position(i, 1, 0)));
+    poses.push_back(ze::Transformation(ze::Quaternion(ze::Vector3(0.3, 0, 0)), ze::Position(i, 2, 0)));
+    poses.push_back(ze::Transformation(ze::Quaternion(ze::Vector3(0.4, 0, 0)), ze::Position(i, 3, 0)));
+    visualizer.drawCoordinateFrames("poses", 0, poses);
+
+    sleep(1);
+  }
+}
diff --git a/RWR/src/ze_oss/ze_visualization/src/viz_ros.cpp b/RWR/src/ze_oss/ze_visualization/src/viz_ros.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f8de7196e16430fb7a6eb21bbe633974659541b5
--- /dev/null
+++ b/RWR/src/ze_oss/ze_visualization/src/viz_ros.cpp
@@ -0,0 +1,293 @@
+// Copyright (c) 2015-2016, ETH Zurich, Wyss Zurich, Zurich Eye
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the ETH Zurich, Wyss Zurich, Zurich Eye nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL ETH Zurich, Wyss Zurich, Zurich Eye BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include <ze/visualization/viz_ros.hpp>
+
+#include <ros/ros.h>
+#include <tf/transform_broadcaster.h>
+#include <visualization_msgs/Marker.h>
+#include <visualization_msgs/MarkerArray.h>
+
+#include <ze/ros/tf_bridge.hpp>
+#include <ze/visualization/viz_ros_utils.hpp>
+
+namespace ze {
+
+VisualizerRos::VisualizerRos()
+{
+  // Inititialize ROS if it was not initialized before.
+  if(!ros::isInitialized())
+  {
+    VLOG(1) << "Initializting ROS";
+    int argc = 0;
+    ros::init(argc, nullptr, std::string("ze_visualization"));
+  }
+
+  // Create node and subscribe.
+  nh_.reset(new ros::NodeHandle("~"));
+  pub_marker_.reset(new ros::Publisher(nh_->advertise<visualization_msgs::Marker>("markers", 100)));
+  tf_broadcaster_.reset(new tf::TransformBroadcaster());
+}
+
+VisualizerRos::VisualizerRos(const std::string& frame)
+  : VisualizerRos::VisualizerRos()
+{
+  world_frame = frame;
+}
+
+void VisualizerRos::drawPoint(
+    const std::string& ns,
+    const size_t id,
+    const Position& point,
+    const Color& color,
+    const real_t size)
+{
+  if(pub_marker_->getNumSubscribers() == 0)
+    return;
+
+  visualization_msgs::Marker m;
+  m.header.frame_id = world_frame;
+  m.header.stamp = ros::Time::now();
+  m.ns = ns;
+  m.id = id;
+  m.type = visualization_msgs::Marker::CUBE;
+  m.action = 0; // add/modify
+  real_t marker_scale = size * viz_scale_;
+  m.scale.x = marker_scale;
+  m.scale.y = marker_scale;
+  m.scale.z = marker_scale;
+  m.color = getRosColor(color);
+  m.pose.position = getRosPoint(point);
+  pub_marker_->publish(m);
+}
+
+void VisualizerRos::drawLine(
+    const std::string& ns,
+    const size_t id,
+    const Position& line_from,
+    const Position& line_to,
+    const Color& color,
+    const real_t size)
+{
+  if(pub_marker_->getNumSubscribers() == 0)
+    return;
+
+  visualization_msgs::Marker m;
+  m.header.frame_id = world_frame;
+  m.header.stamp = ros::Time::now();
+  m.ns = ns;
+  m.id = id;
+  m.type = visualization_msgs::Marker::LINE_STRIP;
+  m.action = 0; // 0 = add/modify
+  m.scale.x = size * viz_scale_;
+  m.scale.y = size * viz_scale_;
+  m.scale.z = size * viz_scale_;
+  m.color = getRosColor(color);
+  m.points.reserve(2);
+  m.points.push_back(getRosPoint(line_from));
+  m.points.push_back(getRosPoint(line_to));
+  pub_marker_->publish(m);
+}
+
+void VisualizerRos::drawCoordinateFrame(
+    const std::string& ns,
+    const size_t id,
+    const Transformation& pose, // T_W_B
+    const real_t size)
+{
+  if(pub_marker_->getNumSubscribers() == 0)
+    return;
+
+  const Vector3& p = pose.getPosition();
+
+  visualization_msgs::Marker m;
+  m.header.frame_id = world_frame;
+  m.header.stamp = ros::Time::now();
+  m.ns = ns;
+  m.id = id;
+  m.type = visualization_msgs::Marker::LINE_LIST;
+  m.action = 0; // 0 = add/modify
+  m.scale.x = size * viz_scale_ * 0.05;
+  m.colors.reserve(6);
+  m.points.reserve(6);
+  real_t length = size * viz_scale_;
+  m.points.push_back(getRosPoint(Vector3::Zero()));
+  m.colors.push_back(getRosColor(Colors::Red));
+  m.points.push_back(getRosPoint(Vector3::UnitX() * length));
+  m.colors.push_back(getRosColor(Colors::Red));
+  m.points.push_back(getRosPoint(Vector3::Zero()));
+  m.colors.push_back(getRosColor(Colors::Green));
+  m.points.push_back(getRosPoint(Vector3::UnitY() * length));
+  m.colors.push_back(getRosColor(Colors::Green));
+  m.points.push_back(getRosPoint(Vector3::Zero()));
+  m.colors.push_back(getRosColor(Colors::Blue));
+  m.points.push_back(getRosPoint(Vector3::UnitZ() * length));
+  m.colors.push_back(getRosColor(Colors::Blue));
+  m.pose = getRosPose(pose);
+  pub_marker_->publish(m);
+}
+
+void VisualizerRos::drawRobot(
+    const std::string& name,
+    const Transformation& T_W_B)
+{
+  tf::StampedTransform tf;
+  tf.stamp_ = ros::Time::now();
+  tf.frame_id_ = world_frame;
+  tf.child_frame_id_ = name;
+  tf.setData(transformationToTF(T_W_B));
+  tf_broadcaster_->sendTransform(tf);
+}
+
+void VisualizerRos::drawPoints(
+    const std::string& ns,
+    const size_t id,
+    const Positions& points,
+    const Color& color,
+    const real_t size)
+{
+  if(pub_marker_->getNumSubscribers() == 0)
+    return;
+
+  visualization_msgs::Marker m;
+  m.header.frame_id = world_frame;
+  m.header.stamp = ros::Time::now();
+  m.ns = ns;
+  m.id = id;
+  m.type = visualization_msgs::Marker::POINTS;
+  m.action = 0; // add/modify
+  real_t marker_scale = size * viz_scale_;
+  m.scale.x = marker_scale;
+  m.scale.y = marker_scale;
+  m.scale.z = marker_scale;
+  m.color = getRosColor(color);
+  m.points.reserve(points.cols());
+  for(int i = 0; i < points.cols(); ++i)
+  {
+    if(points.col(i).norm() > 1e4)
+    {
+      continue;
+    }
+    m.points.push_back(getRosPoint(points.col(i)));
+  }
+  pub_marker_->publish(m);
+}
+
+void VisualizerRos::drawLines(
+    const std::string& ns,
+    const size_t id,
+    const LineMarkers& lines,
+    const Color& color,
+    const real_t size)
+{
+  if(pub_marker_->getNumSubscribers() == 0)
+    return;
+
+  visualization_msgs::Marker m;
+  m.header.frame_id = world_frame;
+  m.header.stamp = ros::Time::now();
+  m.ns = ns;
+  m.id = id;
+  m.type = visualization_msgs::Marker::LINE_LIST;
+  m.action = 0; // 0 = add/modify
+  m.scale.x = size * viz_scale_;
+  m.color = getRosColor(color);
+  m.points.reserve(lines.size() * 2);
+  for(size_t i = 0; i < lines.size(); ++i)
+  {
+    m.points.push_back(getRosPoint(lines[i].first));
+    m.points.push_back(getRosPoint(lines[i].second));
+  }
+  pub_marker_->publish(m);
+}
+
+void VisualizerRos::drawCoordinateFrames(
+    const std::string& ns,
+    const size_t id,
+    const TransformationVector& poses,
+    const real_t size)
+{
+  if(pub_marker_->getNumSubscribers() == 0)
+    return;
+
+  visualization_msgs::Marker m;
+  m.header.frame_id = world_frame;
+  m.header.stamp = ros::Time::now();
+  m.ns = ns;
+  m.id = id;
+  m.type = visualization_msgs::Marker::LINE_LIST;
+  m.action = 0; // 0 = add/modify
+  m.scale.x = size * viz_scale_ * 0.05;
+  m.colors.reserve(poses.size() * 3);
+  m.points.reserve(poses.size() * 3);
+  real_t length = size * viz_scale_;
+  for(const Transformation& T : poses)
+  {
+    const Vector3& p = T.getPosition();
+    const Matrix3 R = T.getRotationMatrix();
+    m.points.push_back(getRosPoint(p));
+    m.colors.push_back(getRosColor(Colors::Red));
+    m.points.push_back(getRosPoint(p + R.col(0) * length));
+    m.colors.push_back(getRosColor(Colors::Red));
+    m.points.push_back(getRosPoint(p));
+    m.colors.push_back(getRosColor(Colors::Green));
+    m.points.push_back(getRosPoint(p + R.col(1) * length));
+    m.colors.push_back(getRosColor(Colors::Green));
+    m.points.push_back(getRosPoint(p));
+    m.colors.push_back(getRosColor(Colors::Blue));
+    m.points.push_back(getRosPoint(p + R.col(2) * length));
+    m.colors.push_back(getRosColor(Colors::Blue));
+  }
+  pub_marker_->publish(m);
+}
+
+void VisualizerRos::drawTrajectory(
+    const std::string& topic,
+    const size_t id,
+    const std::vector<Position>& points,
+    const Color& color,
+    const real_t size)
+{
+  if(pub_marker_->getNumSubscribers() == 0)
+    return;
+
+  visualization_msgs::Marker m;
+  m.header.frame_id = world_frame;
+  m.header.stamp = ros::Time::now();
+  m.ns = topic;
+  m.id = id;
+  m.type = visualization_msgs::Marker::LINE_STRIP;
+  m.action = 0; // 0 = add/modify
+  m.scale.x = size * viz_scale_ * 0.05;
+  m.color = getRosColor(color);
+  m.points.reserve(points.size());
+  for (size_t i = 0u; i < points.size(); ++i)
+  {
+    m.points.push_back(getRosPoint(points[i]));
+  }
+  pub_marker_->publish(m);
+}
+
+} // namespace ze
diff --git a/gitmodules b/gitmodules
new file mode 100644
index 0000000000000000000000000000000000000000..b694432f226cde4152c7464c67c460ef071103de
--- /dev/null
+++ b/gitmodules
@@ -0,0 +1,27 @@
+[submodule "project_ws/src/catkin_simple"]
+	path = project_ws/src/catkin_simple
+	url = git@github.com:catkin/catkin_simple.git
+[submodule "project_ws/src/gflags_catkin"]
+	path = project_ws/src/gflags_catkin
+	url = git@github.com:ethz-asl/gflags_catkin.git
+[submodule "project_ws/src/eigen_catkin"]
+	path = project_ws/src/eigen_catkin
+	url = git@github.com:ethz-asl/eigen_catkin.git
+[submodule "project_ws/src/eigen_checks"]
+	path = project_ws/src/eigen_checks
+	url = git@github.com:ethz-asl/eigen_checks.git
+[submodule "project_ws/src/minkindr"]
+	path = project_ws/src/minkindr
+	url = git@github.com:ethz-asl/minkindr.git
+[submodule "project_ws/src/catkin_boost_python_buildtool"]
+	path = project_ws/src/catkin_boost_python_buildtool
+	url = git@github.com:ethz-asl/catkin_boost_python_buildtool.git
+[submodule "project_ws/src/yaml_cpp_catkin"]
+	path = project_ws/src/yaml_cpp_catkin
+	url = git@github.com:ethz-asl/yaml_cpp_catkin.git
+[submodule "project_ws/src/numpy_eigen"]
+	path = project_ws/src/numpy_eigen
+	url = git@github.com:ethz-asl/numpy_eigen.git
+[submodule "project_ws/src/pangolin_catkin"]
+	path = project_ws/src/pangolin_catkin
+	url = git@github.com:uzh-rpg/pangolin_catkin.git
diff --git a/lab_ws/src/CMakeLists.txt b/lab_ws/src/CMakeLists.txt
new file mode 120000
index 0000000000000000000000000000000000000000..20168168606294577155b02b7fd1000947532faf
--- /dev/null
+++ b/lab_ws/src/CMakeLists.txt
@@ -0,0 +1 @@
+/opt/ros/noetic/share/catkin/cmake/toplevel.cmake
\ No newline at end of file
diff --git a/labs/catkin_ws/src/l2e4/CMakeLists.txt b/lab_ws/src/l2e4/CMakeLists.txt
similarity index 100%
rename from labs/catkin_ws/src/l2e4/CMakeLists.txt
rename to lab_ws/src/l2e4/CMakeLists.txt
diff --git a/labs/catkin_ws/src/l2e4/turtlesim_spawn.launch b/lab_ws/src/l2e4/launch/turtlesim_spawn.launch
similarity index 100%
rename from labs/catkin_ws/src/l2e4/turtlesim_spawn.launch
rename to lab_ws/src/l2e4/launch/turtlesim_spawn.launch
diff --git a/lab_ws/src/l2e4/package.xml b/lab_ws/src/l2e4/package.xml
new file mode 100644
index 0000000000000000000000000000000000000000..4b8c5e860e02994bc21f638473a01efe981c0ece
--- /dev/null
+++ b/lab_ws/src/l2e4/package.xml
@@ -0,0 +1,68 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>l2e4</name>
+  <version>0.0.0</version>
+  <description>The l2e4 package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="chris@todo.todo">chris</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/l2e4</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>rospy</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_export_depend>roscpp</build_export_depend>
+  <build_export_depend>rospy</build_export_depend>
+  <build_export_depend>std_msgs</build_export_depend>
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>rospy</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>
diff --git a/labs/catkin_ws/src/l2e4/teleport_turtle3.py b/lab_ws/src/l2e4/scripts/teleport_turtle3.py
similarity index 100%
rename from labs/catkin_ws/src/l2e4/teleport_turtle3.py
rename to lab_ws/src/l2e4/scripts/teleport_turtle3.py
diff --git a/labs/catkin_ws/src/l2e4/scripts/turtle1_pub.py b/lab_ws/src/l2e4/scripts/turtle1_pub.py
similarity index 100%
rename from labs/catkin_ws/src/l2e4/scripts/turtle1_pub.py
rename to lab_ws/src/l2e4/scripts/turtle1_pub.py
diff --git a/lab_ws/src/l2e4/src/teleport_turtle3.py b/lab_ws/src/l2e4/src/teleport_turtle3.py
new file mode 100644
index 0000000000000000000000000000000000000000..5d90683cc455823456cb5c3947ae52ef2d0eb06b
--- /dev/null
+++ b/lab_ws/src/l2e4/src/teleport_turtle3.py
@@ -0,0 +1,7 @@
+from turtlesim.srv import TeleportAbsolute, Spawn
+
+spawn_client = rospy.ServiceProxy('/spawn', Spawn)
+spawn_client(x_pos, y_pos, theta, "turtle3")
+
+teleport_client = rospy.ServiceProxy('/turtle3/teleport_absolute', TeleportAbsolute)
+teleport_client(x_pos, y_pos, theta)
\ No newline at end of file
diff --git a/labs/catkin_ws/src/l2e4/turtle1_pub.cpp b/lab_ws/src/l2e4/src/turtle1_pub.cpp
similarity index 100%
rename from labs/catkin_ws/src/l2e4/turtle1_pub.cpp
rename to lab_ws/src/l2e4/src/turtle1_pub.cpp
diff --git a/lab_ws/src/l2e4/teleport_turtle3.py b/lab_ws/src/l2e4/teleport_turtle3.py
new file mode 100644
index 0000000000000000000000000000000000000000..5d90683cc455823456cb5c3947ae52ef2d0eb06b
--- /dev/null
+++ b/lab_ws/src/l2e4/teleport_turtle3.py
@@ -0,0 +1,7 @@
+from turtlesim.srv import TeleportAbsolute, Spawn
+
+spawn_client = rospy.ServiceProxy('/spawn', Spawn)
+spawn_client(x_pos, y_pos, theta, "turtle3")
+
+teleport_client = rospy.ServiceProxy('/turtle3/teleport_absolute', TeleportAbsolute)
+teleport_client(x_pos, y_pos, theta)
\ No newline at end of file
diff --git a/lab_ws/src/l2e4/turtle1_pub.cpp b/lab_ws/src/l2e4/turtle1_pub.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..7b1ebfed3f96d595130b9f23175ad23a9496aa4b
--- /dev/null
+++ b/lab_ws/src/l2e4/turtle1_pub.cpp
@@ -0,0 +1,55 @@
+#include <ros/ros.h>
+#include <turtlesim/Pose.h>
+#include <turtlesim/Spawn.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <geometry_msgs/Twist.h>
+#include <tf/transform_datatypes.h>
+
+//#define INVERT(x) x * -1
+ros::Publisher turtle2_cmd;
+
+void velocityCallback(const geometry_msgs::Twist& msg)
+{
+    geometry_msgs::Twist t2_cmd_vel;
+    //turtle1_cmd = nh1.subscribe("turtle1/cmd_vel", 1000, &velocityCallback);
+    t2_cmd_vel.linear.x  = msg.linear.x * -1;
+    t2_cmd_vel.linear.y  = msg.linear.y * -1;
+    t2_cmd_vel.angular.x = msg.angular.x * -1;
+    t2_cmd_vel.angular.y = msg.angular.y * -1;
+    t2_cmd_vel.angular.z = msg.angular.z * -1;
+
+    
+    ROS_INFO_STREAM("output:linear.x " << t2_cmd_vel.linear.x << " linear.y " << t2_cmd_vel.linear.y );
+    turtle2_cmd.publish(t2_cmd_vel);
+}
+
+int main(int argc, char** argv)
+{
+    ros::init(argc, argv, "command_pub_sub");
+    ros::NodeHandle nh1;
+    ros::NodeHandle nh2;
+    turtle2_cmd = nh2.advertise<geometry_msgs::Twist>("turtle2/cmd_vel", 1000);
+    ros::Subscriber turtle1_cmd = nh1.subscribe("turtle1/cmd_vel", 1000, &velocityCallback);
+    //ros::Publisher turtle1_cmd = nh1.advertise<geometry_msgs::Twist>("turtle1/cmd_vel", 1000);
+    
+    ros::Rate loop_rate(10);
+    //int count(0);
+
+    /*while(ros::ok())
+    {
+        geometry_msgs::Twist t2_cmd_vel;
+        //turtle1_cmd = nh1.subscribe("turtle1/cmd_vel", 1000, &velocityCallback);
+        t2_cmd_vel.linear.x  = t2_cmd_vel.linear.x * -1;
+        t2_cmd_vel.linear.y  = t2_cmd_vel.linear.y * -1;
+        t2_cmd_vel.angular.x = t2_cmd_vel.angular.x * -1;
+        t2_cmd_vel.angular.y = t2_cmd_vel.angular.y * -1;
+        velocityCallback(t2_cmd_vel, turtle2_cmd);
+        //turtle2_cmd.publish(t2_cmd_vel);
+        ros::spinOnce();
+        loop_rate.sleep();
+    }*/
+
+    ros::spin();
+
+    return 0;
+}
\ No newline at end of file
diff --git a/lab_ws/src/l2e4/turtle2_sub.cpp b/lab_ws/src/l2e4/turtle2_sub.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..061ff7ed4d748831694aeca2d0b935090218020e
--- /dev/null
+++ b/lab_ws/src/l2e4/turtle2_sub.cpp
@@ -0,0 +1,21 @@
+#include <ros/ros.h>
+#include <turtlesim/Pose.h>
+#include <turtlesim/Spawn.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <geometry_msgs/Twist.h>
+#include <tf/transform_datatypes.h>
+
+void turtle1_command_callback(const turtlesim::Twist &msg)
+{
+    ROS_INFO_STREAM(msg * -1);
+}
+
+int main(int argc, char ** argv)
+{
+    ros::init(argc, argv, "turtle2_subscriber");
+    ros::NodeHandle nh;
+    ros::Subscriber sub = nh.subscribe("turtle2/cmd_vel", 10, turtle1_command_callback);
+    ros::spin();
+
+    return 0;
+}
diff --git a/lab_ws/src/l2e4/turtlesim_spawn.launch b/lab_ws/src/l2e4/turtlesim_spawn.launch
new file mode 100644
index 0000000000000000000000000000000000000000..897f805a25e2148b52a3838fc3ac2ba53bed795b
--- /dev/null
+++ b/lab_ws/src/l2e4/turtlesim_spawn.launch
@@ -0,0 +1,19 @@
+<launch>
+    <node
+        pkg="turtlesim"
+        type="turtlesim_node"
+        name="turtlesim"
+        respawn="true"
+    />
+    <node 
+        pkg="rosservice"
+        type="rosservice"
+        name="spawn"
+        args="call spawn --wait 5.5 5.5 0 'turtle2'"
+    />
+    <node
+        pkg="l2e4"
+        type="turtle1_pub"
+        name="turtle1_pub"
+    />
+</launch>
\ No newline at end of file
diff --git a/labs/catkin_ws/src/lab2/CMakeLists.txt b/lab_ws/src/lab2/CMakeLists.txt
similarity index 100%
rename from labs/catkin_ws/src/lab2/CMakeLists.txt
rename to lab_ws/src/lab2/CMakeLists.txt
diff --git a/labs/catkin_ws/src/lab2/wrap_rviz.h b/lab_ws/src/lab2/include/wrap_rviz.h
similarity index 100%
rename from labs/catkin_ws/src/lab2/wrap_rviz.h
rename to lab_ws/src/lab2/include/wrap_rviz.h
diff --git a/labs/catkin_ws/src/lab2/package.xml b/lab_ws/src/lab2/package.xml
similarity index 100%
rename from labs/catkin_ws/src/lab2/package.xml
rename to lab_ws/src/lab2/package.xml
diff --git a/labs/catkin_ws/src/lab2/pose_listener.cpp b/lab_ws/src/lab2/pose_listener.cpp
similarity index 100%
rename from labs/catkin_ws/src/lab2/pose_listener.cpp
rename to lab_ws/src/lab2/pose_listener.cpp
diff --git a/labs/catkin_ws/src/lab2/pose_rviz.cpp b/lab_ws/src/lab2/pose_rviz.cpp
similarity index 100%
rename from labs/catkin_ws/src/lab2/pose_rviz.cpp
rename to lab_ws/src/lab2/pose_rviz.cpp
diff --git a/labs/catkin_ws/src/lab2/scripts/pose_listener.py b/lab_ws/src/lab2/scripts/pose_listener.py
similarity index 100%
rename from labs/catkin_ws/src/lab2/scripts/pose_listener.py
rename to lab_ws/src/lab2/scripts/pose_listener.py
diff --git a/lab_ws/src/lab2/src/pose_listener.cpp b/lab_ws/src/lab2/src/pose_listener.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..8af20e53a657e0eaaeeeb195b96c15d729ef3772
--- /dev/null
+++ b/lab_ws/src/lab2/src/pose_listener.cpp
@@ -0,0 +1,15 @@
+#include <ros/ros.h>
+#include <turtlesim/Pose.h>
+
+void turtle_pose_callback(const turtlesim::Pose& msg){
+    ROS_INFO("X=%f",msg.x);
+    ROS_INFO_STREAM("Y="<<msg.y);
+    ROS_INFO_STREAM("T="<<msg.theta);
+}
+
+int main(int argc, char **argv){
+    ros::init(argc, argv, "pose_subscriber");
+    ros::NodeHandle nh;
+    ros::Subscriber sub = nh.subscribe("turtle1/pose",10, &turtle_pose_callback);
+    ros::spin();
+}
\ No newline at end of file
diff --git a/lab_ws/src/lab2/src/pose_rviz.cpp b/lab_ws/src/lab2/src/pose_rviz.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b224d520d402732e1b0d1a1f6a22dd9465fdc53a
--- /dev/null
+++ b/lab_ws/src/lab2/src/pose_rviz.cpp
@@ -0,0 +1,30 @@
+#include "geometry_msgs/PoseStamped.h"
+#include <tf/tf.h>
+#include <turtlesim/Pose.h>
+
+#include "wrap_rviz.h"
+
+void pub_rviz_wrapper::turtle_pose_callback(const turtlesim::Pose& msg){
+    geometry_msgs::PoseStamped pose_rviz;
+    pose_rviz.header.frame_id="map";
+    pose_rviz.header.stamp=ros::Time::now();
+    pose_rviz.header.seq=0;
+    pose_rviz.pose.orientation =
+    tf::createQuaternionMsgFromYaw(msg.theta);
+    pose_rviz.pose.position.x = msg.x;
+    pose_rviz.pose.position.y = msg.y;
+
+    this->pub.publish(pose_rviz);
+}
+
+int main(int argc, char** argv)
+{
+    ros::init(argc, argv, "pose_subscriber");
+    pub_rviz_wrapper wrap;
+    ros::NodeHandle nh;
+    //ros::Subscriber sub = nh.subscribe("turtle1/pose",10, &pub_rviz_wrapper::turtle_pose_callback);
+    ros::Subscriber sub = nh.subscribe("turtle1/pose", 10, &pub_rviz_wrapper::turtle_pose_callback, &wrap);
+    //pub = nh.advertise("turtle1/pose_rviz", 10, &pub_rviz_wrapper::turtle_pose_callback, &wrap);
+    wrap.pub = nh.advertise<geometry_msgs::PoseStamped>("turtle1/pose_rviz", 10);
+    ros::spin();
+}
\ No newline at end of file
diff --git a/labs/catkin_ws/src/lab2/turtle_map.cpp b/lab_ws/src/lab2/src/turtle_map.cpp
similarity index 100%
rename from labs/catkin_ws/src/lab2/turtle_map.cpp
rename to lab_ws/src/lab2/src/turtle_map.cpp
diff --git a/lab_ws/src/lab2/turtle_map.cpp b/lab_ws/src/lab2/turtle_map.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e6945d5f6a5ee57956f4544224195dbd8f75eacd
--- /dev/null
+++ b/lab_ws/src/lab2/turtle_map.cpp
@@ -0,0 +1,26 @@
+#include <ros/ros.h>
+#include <turtlesim/Pose.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <turtlesim/Pose.h>
+#include <tf/transform_datatypes.h>
+
+int main(int argc, char **argv) {
+    ros::init(argc, argv, "pose_publisher");
+    ros::NodeHandle nh;
+    ros::Publisher pose_pub = nh.advertise<geometry_msgs::PoseStamped>("pose_output", 1000);
+    ros::Rate loop_rate(10);
+    int count = 0;
+    while (ros::ok()) {
+        geometry_msgs::PoseStamped msg;
+        msg.header.frame_id="map";
+        msg.header.stamp=ros::Time::now();
+        msg.header.seq=count++;
+        msg.pose.orientation = tf::createQuaternionMsgFromYaw(0);
+        msg.pose.position.x = 1;
+        msg.pose.position.y = 1;
+        pose_pub.publish(msg);
+        ros::spinOnce();
+        loop_rate.sleep();
+    }
+    return 0;
+}
\ No newline at end of file
diff --git a/lab_ws/src/lab2/wrap_rviz.h b/lab_ws/src/lab2/wrap_rviz.h
new file mode 100644
index 0000000000000000000000000000000000000000..c7ac88685d18bffbce0629eae4d7e69367bbc766
--- /dev/null
+++ b/lab_ws/src/lab2/wrap_rviz.h
@@ -0,0 +1,14 @@
+#ifndef WRAP_RVIZ_H
+#define WRAP_RVIZ_H
+
+#include <ros/ros.h>
+#include <turtlesim/Pose.h>
+
+class pub_rviz_wrapper
+{
+    public:
+        ros::Publisher pub;
+        void turtle_pose_callback(const turtlesim::Pose& msg);
+};
+
+#endif //WRAP_RVIZ_H
\ No newline at end of file
diff --git a/labs/catkin_ws/src/lab5/CMakeLists.txt b/lab_ws/src/lab5/CMakeLists.txt
similarity index 100%
rename from labs/catkin_ws/src/lab5/CMakeLists.txt
rename to lab_ws/src/lab5/CMakeLists.txt
diff --git a/labs/catkin_ws/src/lab5/amcl.launch b/lab_ws/src/lab5/amcl.launch
similarity index 100%
rename from labs/catkin_ws/src/lab5/amcl.launch
rename to lab_ws/src/lab5/amcl.launch
diff --git a/labs/catkin_ws/src/lab5/ex1.launch b/lab_ws/src/lab5/ex1.launch
similarity index 100%
rename from labs/catkin_ws/src/lab5/ex1.launch
rename to lab_ws/src/lab5/ex1.launch
diff --git a/labs/catkin_ws/src/lab5/ex2.launch b/lab_ws/src/lab5/ex2.launch
similarity index 100%
rename from labs/catkin_ws/src/lab5/ex2.launch
rename to lab_ws/src/lab5/ex2.launch
diff --git a/labs/catkin_ws/src/lab5/ex3.launch b/lab_ws/src/lab5/ex3.launch
similarity index 100%
rename from labs/catkin_ws/src/lab5/ex3.launch
rename to lab_ws/src/lab5/ex3.launch
diff --git a/labs/catkin_ws/src/lab5/ex4.launch b/lab_ws/src/lab5/ex4.launch
similarity index 100%
rename from labs/catkin_ws/src/lab5/ex4.launch
rename to lab_ws/src/lab5/ex4.launch
diff --git a/labs/catkin_ws/src/lab5/goal.cpp b/lab_ws/src/lab5/goal.cpp
similarity index 100%
rename from labs/catkin_ws/src/lab5/goal.cpp
rename to lab_ws/src/lab5/goal.cpp
diff --git a/lab_ws/src/lab5/lab5_world b/lab_ws/src/lab5/lab5_world
new file mode 100644
index 0000000000000000000000000000000000000000..80bee831e8784f0f3c025467fe829e112df4294e
--- /dev/null
+++ b/lab_ws/src/lab5/lab5_world
@@ -0,0 +1,289 @@
+<sdf version='1.7'>
+  <world name='default'>
+    <light name='sun' type='directional'>
+      <cast_shadows>1</cast_shadows>
+      <pose>0 0 10 0 -0 0</pose>
+      <diffuse>0.8 0.8 0.8 1</diffuse>
+      <specular>0.2 0.2 0.2 1</specular>
+      <attenuation>
+        <range>1000</range>
+        <constant>0.9</constant>
+        <linear>0.01</linear>
+        <quadratic>0.001</quadratic>
+      </attenuation>
+      <direction>-0.5 0.1 -0.9</direction>
+      <spot>
+        <inner_angle>0</inner_angle>
+        <outer_angle>0</outer_angle>
+        <falloff>0</falloff>
+      </spot>
+    </light>
+    <model name='ground_plane'>
+      <static>1</static>
+      <link name='link'>
+        <collision name='collision'>
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>100 100</size>
+            </plane>
+          </geometry>
+          <surface>
+            <contact>
+              <collide_bitmask>65535</collide_bitmask>
+              <ode/>
+            </contact>
+            <friction>
+              <ode>
+                <mu>100</mu>
+                <mu2>50</mu2>
+              </ode>
+              <torsional>
+                <ode/>
+              </torsional>
+            </friction>
+            <bounce/>
+          </surface>
+          <max_contacts>10</max_contacts>
+        </collision>
+        <visual name='visual'>
+          <cast_shadows>0</cast_shadows>
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>100 100</size>
+            </plane>
+          </geometry>
+          <material>
+            <script>
+              <uri>file://media/materials/scripts/gazebo.material</uri>
+              <name>Gazebo/Grey</name>
+            </script>
+          </material>
+        </visual>
+        <self_collide>0</self_collide>
+        <enable_wind>0</enable_wind>
+        <kinematic>0</kinematic>
+      </link>
+    </model>
+    <gravity>0 0 -9.8</gravity>
+    <magnetic_field>6e-06 2.3e-05 -4.2e-05</magnetic_field>
+    <atmosphere type='adiabatic'/>
+    <physics type='ode'>
+      <max_step_size>0.001</max_step_size>
+      <real_time_factor>1</real_time_factor>
+      <real_time_update_rate>1000</real_time_update_rate>
+    </physics>
+    <scene>
+      <ambient>0.4 0.4 0.4 1</ambient>
+      <background>0.7 0.7 0.7 1</background>
+      <shadows>1</shadows>
+    </scene>
+    <wind/>
+    <spherical_coordinates>
+      <surface_model>EARTH_WGS84</surface_model>
+      <latitude_deg>0</latitude_deg>
+      <longitude_deg>0</longitude_deg>
+      <elevation>0</elevation>
+      <heading_deg>0</heading_deg>
+    </spherical_coordinates>
+    <light name='sun' type='directional'>
+      <cast_shadows>1</cast_shadows>
+      <pose>-5.50359 0.337381 10 0 -0 0</pose>
+      <diffuse>0.8 0.8 0.8 1</diffuse>
+      <specular>0.2 0.2 0.2 1</specular>
+      <attenuation>
+        <range>1000</range>
+        <constant>0.9</constant>
+        <linear>0.01</linear>
+        <quadratic>0.001</quadratic>
+      </attenuation>
+      <direction>-0.5 0.1 -0.9</direction>
+    </light>
+    <light name='sun' type='directional'>
+      <cast_shadows>1</cast_shadows>
+      <pose>1.7638 3.28968 10 0 -0 0</pose>
+      <diffuse>0.8 0.8 0.8 1</diffuse>
+      <specular>0.2 0.2 0.2 1</specular>
+      <attenuation>
+        <range>1000</range>
+        <constant>0.9</constant>
+        <linear>0.01</linear>
+        <quadratic>0.001</quadratic>
+      </attenuation>
+      <direction>-0.5 0.1 -0.9</direction>
+    </light>
+    <model name='wheel_valve'>
+      <link name='handle'>
+        <pose>0 0 0.1 0 -0 1.57</pose>
+        <inertial>
+          <mass>7.9917</mass>
+          <pose>0.05 0 0 0 -0 0</pose>
+          <inertia>
+            <ixx>0.0689</ixx>
+            <ixy>0</ixy>
+            <ixz>0</ixz>
+            <iyy>0.0353</iyy>
+            <iyz>0</iyz>
+            <izz>0.0353</izz>
+          </inertia>
+        </inertial>
+        <collision name='collision'>
+          <geometry>
+            <mesh>
+              <uri>model://drc_practice_wheel_valve/meshes/valve_wheel.dae</uri>
+            </mesh>
+          </geometry>
+          <surface>
+            <bounce>
+              <restitution_coefficient>0.01</restitution_coefficient>
+              <threshold>5</threshold>
+            </bounce>
+            <friction>
+              <ode>
+                <mu>5</mu>
+                <mu2>5</mu2>
+              </ode>
+              <torsional>
+                <ode/>
+              </torsional>
+            </friction>
+            <contact>
+              <ode>
+                <soft_cfm>0.01</soft_cfm>
+                <kp>1e+06</kp>
+                <kd>100000</kd>
+                <max_vel>0.01</max_vel>
+                <min_depth>0.001</min_depth>
+              </ode>
+            </contact>
+          </surface>
+          <max_contacts>10</max_contacts>
+        </collision>
+        <visual name='visual'>
+          <geometry>
+            <mesh>
+              <uri>model://drc_practice_wheel_valve/meshes/valve_wheel.dae</uri>
+            </mesh>
+          </geometry>
+        </visual>
+        <self_collide>0</self_collide>
+        <enable_wind>0</enable_wind>
+        <kinematic>0</kinematic>
+      </link>
+      <joint name='joint' type='revolute'>
+        <parent>world</parent>
+        <child>handle</child>
+        <axis>
+          <xyz expressed_in='__model__'>0 1 0</xyz>
+          <limit>
+            <lower>-12.56</lower>
+            <upper>12.56</upper>
+          </limit>
+          <dynamics>
+            <damping>1000</damping>
+            <friction>100</friction>
+            <spring_reference>0</spring_reference>
+            <spring_stiffness>0</spring_stiffness>
+          </dynamics>
+        </axis>
+        <physics>
+          <ode>
+            <implicit_spring_damper>1</implicit_spring_damper>
+            <limit>
+              <cfm>0</cfm>
+              <erp>0.2</erp>
+            </limit>
+          </ode>
+        </physics>
+      </joint>
+      <pose>6.9713 5.86506 0 0 -0 0</pose>
+    </model>
+    <model name='hatchback'>
+      <static>1</static>
+      <link name='link'>
+        <collision name='collision'>
+          <pose>0 0 0 0 -0 1.5708</pose>
+          <geometry>
+            <mesh>
+              <scale>0.0254 0.0254 0.0254</scale>
+              <uri>model://hatchback/meshes/hatchback.obj</uri>
+            </mesh>
+          </geometry>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <contact>
+              <ode/>
+            </contact>
+            <bounce/>
+            <friction>
+              <torsional>
+                <ode/>
+              </torsional>
+              <ode/>
+            </friction>
+          </surface>
+        </collision>
+        <visual name='visual'>
+          <pose>0 0 0 0 -0 1.5708</pose>
+          <geometry>
+            <mesh>
+              <scale>0.0254 0.0254 0.0254</scale>
+              <uri>model://hatchback/meshes/hatchback.obj</uri>
+            </mesh>
+          </geometry>
+        </visual>
+        <self_collide>0</self_collide>
+        <enable_wind>0</enable_wind>
+        <kinematic>0</kinematic>
+      </link>
+      <pose>5.31569 6.54006 0 0 -0 0</pose>
+    </model>
+    <state world_name='default'>
+      <sim_time>335 437000000</sim_time>
+      <real_time>371 455946666</real_time>
+      <wall_time>1634195339 637531651</wall_time>
+      <iterations>335437</iterations>
+      <model name='ground_plane'>
+        <pose>0 0 0 0 -0 0</pose>
+        <scale>1 1 1</scale>
+        <link name='link'>
+          <pose>0 0 0 0 -0 0</pose>
+          <velocity>0 0 0 0 -0 0</velocity>
+          <acceleration>0 0 0 0 -0 0</acceleration>
+          <wrench>0 0 0 0 -0 0</wrench>
+        </link>
+      </model>
+      <model name='hatchback'>
+        <pose>5.31569 6.54006 0 0 -0 0</pose>
+        <scale>1 1 1</scale>
+        <link name='link'>
+          <pose>5.31569 6.54006 0 0 -0 0</pose>
+          <velocity>0 0 0 0 -0 0</velocity>
+          <acceleration>0 0 0 0 -0 0</acceleration>
+          <wrench>0 0 0 0 -0 0</wrench>
+        </link>
+      </model>
+      <model name='wheel_valve'>
+        <pose>6.97113 5.86522 0.003135 -0.004483 0.002838 -0.000345</pose>
+        <scale>1 1 1</scale>
+        <link name='handle'>
+          <pose>6.97141 5.86566 0.103134 0.002834 0.004486 1.56967</pose>
+          <velocity>0.015608 -0.015086 -0.041832 0.161707 0.124423 0.083303</velocity>
+          <acceleration>4.9e-05 -7e-05 -0.000154 0.001902 0.00017 -0.018816</acceleration>
+          <wrench>0.00039 -0.000556 -0.001231 0 -0 0</wrench>
+        </link>
+      </model>
+      <light name='sun'>
+        <pose>0 0 10 0 -0 0</pose>
+      </light>
+    </state>
+    <gui fullscreen='0'>
+      <camera name='user_camera'>
+        <pose>10.2495 15.0284 16.0916 0 0.652802 -2.05429</pose>
+        <view_controller>orbit</view_controller>
+        <projection_type>perspective</projection_type>
+      </camera>
+    </gui>
+  </world>
+</sdf>
diff --git a/lab_ws/src/lab5/launch/amcl.launch b/lab_ws/src/lab5/launch/amcl.launch
new file mode 100644
index 0000000000000000000000000000000000000000..5bc3c5cd5510561af70dd847fc001ea091cbeed3
--- /dev/null
+++ b/lab_ws/src/lab5/launch/amcl.launch
@@ -0,0 +1,46 @@
+<launch>
+  <!-- Arguments -->
+  <arg name="scan_topic"     default="scan"/>
+  <arg name="initial_pose_x" default="0.0"/>
+  <arg name="initial_pose_y" default="0.0"/>
+  <arg name="initial_pose_a" default="0.0"/>
+
+  <!-- AMCL -->
+  <node pkg="amcl" type="amcl" name="amcl">
+
+    <param name="min_particles"             value="500"/>
+    <param name="max_particles"             value="3000"/>
+    <param name="kld_err"                   value="0.02"/>
+    <param name="update_min_d"              value="0.20"/>
+    <param name="update_min_a"              value="0.20"/>
+    <param name="resample_interval"         value="1"/>
+    <param name="transform_tolerance"       value="0.5"/>
+    <param name="recovery_alpha_slow"       value="0.00"/>
+    <param name="recovery_alpha_fast"       value="0.00"/>
+    <param name="initial_pose_x"            value="$(arg initial_pose_x)"/>
+    <param name="initial_pose_y"            value="$(arg initial_pose_y)"/>
+    <param name="initial_pose_a"            value="$(arg initial_pose_a)"/>
+    <param name="gui_publish_rate"          value="50.0"/>
+
+    <remap from="scan"                      to="$(arg scan_topic)"/>
+    <param name="laser_max_range"           value="3.5"/>
+    <param name="laser_max_beams"           value="180"/>
+    <param name="laser_z_hit"               value="0.5"/>
+    <param name="laser_z_short"             value="0.05"/>
+    <param name="laser_z_max"               value="0.05"/>
+    <param name="laser_z_rand"              value="0.5"/>
+    <param name="laser_sigma_hit"           value="0.2"/>
+    <param name="laser_lambda_short"        value="0.1"/>
+    <param name="laser_likelihood_max_dist" value="2.0"/>
+    <param name="laser_model_type"          value="likelihood_field"/>
+
+    <param name="odom_model_type"           value="diff"/>
+    <param name="odom_alpha1"               value="0.1"/>
+    <param name="odom_alpha2"               value="0.1"/>
+    <param name="odom_alpha3"               value="0.1"/>
+    <param name="odom_alpha4"               value="0.1"/>
+    <param name="odom_frame_id"             value="odom"/>
+    <param name="base_frame_id"             value="base_footprint"/>
+
+  </node>
+</launch>
\ No newline at end of file
diff --git a/lab_ws/src/lab5/launch/ex1.launch b/lab_ws/src/lab5/launch/ex1.launch
new file mode 100644
index 0000000000000000000000000000000000000000..9bc3524efe65aae1f2b757f48079e8555ca3893f
--- /dev/null
+++ b/lab_ws/src/lab5/launch/ex1.launch
@@ -0,0 +1,26 @@
+
+<launch>
+  <arg name="model" default="$(env TURTLEBOT3_MODEL)" doc="model type [burger, waffle, waffle_pi]"/>
+  <arg name="x_pos" default="-2.0"/>
+  <arg name="y_pos" default="-0.5"/>
+  <arg name="z_pos" default="0.0"/>
+  <arg name="world_file" default="$(find lab5)/worlds/lab5_world.world"/>
+
+  <include file="$(find gazebo_ros)/launch/empty_world.launch">
+    <arg name="world_name" value="$(find turtlebot3_gazebo)/worlds/turtlebot3_world.world"/>
+    <arg name="paused" value="false"/>
+    <arg name="use_sim_time" value="true"/>
+    <arg name="gui" value="true"/>
+    <arg name="headless" value="false"/>
+    <arg name="debug" value="false"/>
+  </include>
+
+  <param name="robot_description" command="$(find xacro)/xacro --inorder $(find turtlebot3_description)/urdf/turtlebot3_$(arg model).urdf.xacro" />
+
+  <node pkg="gazebo_ros" type="spawn_model" name="spawn_urdf"  args="-urdf -model turtlebot3_$(arg model) -x $(arg x_pos) -y $(arg y_pos) -z $(arg z_pos) -param robot_description" />
+
+    <!-- Robot State Publisher -->
+  <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher">
+  <param name="publish_frequency" type="double" value="30.0" />
+  </node>
+</launch>
diff --git a/lab_ws/src/lab5/launch/ex2.launch b/lab_ws/src/lab5/launch/ex2.launch
new file mode 100644
index 0000000000000000000000000000000000000000..95c8477406e5d848d36f347ddc6bb38485d7d643
--- /dev/null
+++ b/lab_ws/src/lab5/launch/ex2.launch
@@ -0,0 +1,42 @@
+
+<launch>
+  <arg name="model" default="$(env TURTLEBOT3_MODEL)" doc="model type [burger, waffle, waffle_pi]"/>
+  <arg name="x_pos" default="-2.0"/>
+  <arg name="y_pos" default="-0.5"/>
+  <arg name="z_pos" default="0.0"/>
+  <arg name="world_file" default="$(find lab5)/worlds/ex2.world"/>
+  <arg name="map_file" default="$(find lab5)/maps/ex2_map.yaml"/>
+  <arg name="amcl" default="$(find lab5)/amcl.launch"/>
+  <arg name="open_rviz" default="true"/>
+  <arg name="state_pub" default="true"/>
+  <arg name="move_forward_only" default="false"/>
+
+
+  <include file="$(find gazebo_ros)/launch/empty_world.launch">
+    <arg name="world_name" value="$(find turtlebot3_gazebo)/worlds/turtlebot3_world.world"/>
+    <arg name="paused" value="false"/>
+    <arg name="use_sim_time" value="true"/>
+    <arg name="gui" value="true"/>
+    <arg name="headless" value="false"/>
+    <arg name="debug" value="false"/>
+  </include>
+
+  <include file="$(find turtlebot3_navigation)/launch/move_base.launch">
+    <arg name="model" value="$(arg model)" />
+    <arg name="move_forward_only" value="$(arg move_forward_only)"/>
+  </include>
+
+  <group if="$(arg open_rviz)">
+    <node pkg="rviz" type="rviz" name="rviz" required="true"
+    args="-d $(find turtlebot3_navigation)/rviz/turtlebot3_navigation.rviz"/>
+  </group>
+
+  <include file="$(find lab5)/amcl.launch"/>
+
+  <param name="robot_description" command="$(find xacro)/xacro --inorder $(find turtlebot3_description)/urdf/turtlebot3_$(arg model).urdf.xacro" />
+
+  <node pkg="map_server" name="map_server" type="map_server" args="$(arg map_file)"/>
+  
+  <node pkg="gazebo_ros" type="spawn_model" name="spawn_urdf"  args="-urdf -model turtlebot3_$(arg model) -x $(arg x_pos) -y $(arg y_pos) -z $(arg z_pos) -param robot_description" />
+
+</launch>
\ No newline at end of file
diff --git a/lab_ws/src/lab5/launch/ex3.launch b/lab_ws/src/lab5/launch/ex3.launch
new file mode 100644
index 0000000000000000000000000000000000000000..95c8477406e5d848d36f347ddc6bb38485d7d643
--- /dev/null
+++ b/lab_ws/src/lab5/launch/ex3.launch
@@ -0,0 +1,42 @@
+
+<launch>
+  <arg name="model" default="$(env TURTLEBOT3_MODEL)" doc="model type [burger, waffle, waffle_pi]"/>
+  <arg name="x_pos" default="-2.0"/>
+  <arg name="y_pos" default="-0.5"/>
+  <arg name="z_pos" default="0.0"/>
+  <arg name="world_file" default="$(find lab5)/worlds/ex2.world"/>
+  <arg name="map_file" default="$(find lab5)/maps/ex2_map.yaml"/>
+  <arg name="amcl" default="$(find lab5)/amcl.launch"/>
+  <arg name="open_rviz" default="true"/>
+  <arg name="state_pub" default="true"/>
+  <arg name="move_forward_only" default="false"/>
+
+
+  <include file="$(find gazebo_ros)/launch/empty_world.launch">
+    <arg name="world_name" value="$(find turtlebot3_gazebo)/worlds/turtlebot3_world.world"/>
+    <arg name="paused" value="false"/>
+    <arg name="use_sim_time" value="true"/>
+    <arg name="gui" value="true"/>
+    <arg name="headless" value="false"/>
+    <arg name="debug" value="false"/>
+  </include>
+
+  <include file="$(find turtlebot3_navigation)/launch/move_base.launch">
+    <arg name="model" value="$(arg model)" />
+    <arg name="move_forward_only" value="$(arg move_forward_only)"/>
+  </include>
+
+  <group if="$(arg open_rviz)">
+    <node pkg="rviz" type="rviz" name="rviz" required="true"
+    args="-d $(find turtlebot3_navigation)/rviz/turtlebot3_navigation.rviz"/>
+  </group>
+
+  <include file="$(find lab5)/amcl.launch"/>
+
+  <param name="robot_description" command="$(find xacro)/xacro --inorder $(find turtlebot3_description)/urdf/turtlebot3_$(arg model).urdf.xacro" />
+
+  <node pkg="map_server" name="map_server" type="map_server" args="$(arg map_file)"/>
+  
+  <node pkg="gazebo_ros" type="spawn_model" name="spawn_urdf"  args="-urdf -model turtlebot3_$(arg model) -x $(arg x_pos) -y $(arg y_pos) -z $(arg z_pos) -param robot_description" />
+
+</launch>
\ No newline at end of file
diff --git a/lab_ws/src/lab5/launch/ex4.launch b/lab_ws/src/lab5/launch/ex4.launch
new file mode 100644
index 0000000000000000000000000000000000000000..b4b0d56f1cbd54911e3f0fb86b5968da49ff1181
--- /dev/null
+++ b/lab_ws/src/lab5/launch/ex4.launch
@@ -0,0 +1,42 @@
+
+<launch>
+  <arg name="model" default="$(env TURTLEBOT3_MODEL)" doc="model type [burger, waffle, waffle_pi]"/>
+  <arg name="x_pos" default="-2.0"/>
+  <arg name="y_pos" default="-0.5"/>
+  <arg name="z_pos" default="0.0"/>
+  <arg name="world_file" default="$(find lab5)/worlds/ex2.world"/>
+  <arg name="map_file" default="$(find lab5)/maps/ex2_map.yaml"/>
+  <arg name="amcl" default="$(find lab5)/amcl.launch"/>
+  <arg name="open_rviz" default="true"/>
+  <arg name="state_pub" default="true"/>
+  <arg name="move_forward_only" default="false"/>
+
+
+  <include file="$(find gazebo_ros)/launch/empty_world.launch">
+    <arg name="world_name" value="$(find turtlebot3_gazebo)/worlds/turtlebot3_world.world"/>
+    <arg name="paused" value="false"/>
+    <arg name="use_sim_time" value="true"/>
+    <arg name="gui" value="true"/>
+    <arg name="headless" value="false"/>
+    <arg name="debug" value="false"/>
+  </include>
+
+  <include file="$(find turtlebot3_navigation)/launch/move_base.launch">
+    <arg name="model" value="$(arg model)" />
+    <arg name="move_forward_only" value="$(arg move_forward_only)"/>
+  </include>
+
+  <group if="$(arg open_rviz)">
+    <node pkg="rviz" type="rviz" name="rviz" required="true"
+    args="-d $(find turtlebot3_navigation)/rviz/turtlebot3_navigation.rviz"/>
+  </group>
+
+  <include file="$(find lab5)/amcl.launch"/>
+
+  <param name="robot_description" command="$(find xacro)/xacro --inorder $(find lab5)/urdf/turtlebot3_burger.urdf.xacro" />
+
+  <node pkg="map_server" name="map_server" type="map_server" args="$(arg map_file)"/>
+  
+  <node pkg="gazebo_ros" type="spawn_model" name="spawn_urdf"  args="-urdf -model turtlebot3_$(arg model) -x $(arg x_pos) -y $(arg y_pos) -z $(arg z_pos) -param robot_description" />
+
+</launch>
\ No newline at end of file
diff --git a/labs/catkin_ws/src/lab5/move_base.launch b/lab_ws/src/lab5/launch/move_base.launch
similarity index 100%
rename from labs/catkin_ws/src/lab5/move_base.launch
rename to lab_ws/src/lab5/launch/move_base.launch
diff --git a/labs/catkin_ws/src/lab5/maps/ex2_map.pgm b/lab_ws/src/lab5/maps/ex2_map.pgm
similarity index 100%
rename from labs/catkin_ws/src/lab5/maps/ex2_map.pgm
rename to lab_ws/src/lab5/maps/ex2_map.pgm
diff --git a/labs/catkin_ws/src/lab5/maps/ex2_map.yaml b/lab_ws/src/lab5/maps/ex2_map.yaml
similarity index 100%
rename from labs/catkin_ws/src/lab5/maps/ex2_map.yaml
rename to lab_ws/src/lab5/maps/ex2_map.yaml
diff --git a/lab_ws/src/lab5/move_base.launch b/lab_ws/src/lab5/move_base.launch
new file mode 100644
index 0000000000000000000000000000000000000000..c232f84a00a18477fa99c089619cc031ef90a877
--- /dev/null
+++ b/lab_ws/src/lab5/move_base.launch
@@ -0,0 +1,21 @@
+<launch>
+	<!-- Arguments -->
+	<arg name="model" default="$(env TURTLEBOT3_MODEL)" doc="model type [burger, waffle, waffle_pi]"/>
+	<arg name="cmd_vel_topic" default="/cmd_vel" />
+	<arg name="odom_topic" default="odom" />
+	<arg name="move_forward_only" default="false"/>
+	
+	<!-- move_base -->
+	<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
+		<param name="base_local_planner" value="dwa_local_planner/DWAPlannerROS" />
+		<rosparam file="$(find turtlebot3_navigation)/param/costmap_common_params_$(arg model).yaml" command="load" ns="global_costmap" />
+		<rosparam file="$(find turtlebot3_navigation)/param/costmap_common_params_$(arg model).yaml" command="load" ns="local_costmap" />
+		<rosparam file="$(find turtlebot3_navigation)/param/local_costmap_params.yaml" command="load" />
+		<rosparam file="$(find turtlebot3_navigation)/param/global_costmap_params.yaml" command="load" />
+		<rosparam file="$(find turtlebot3_navigation)/param/move_base_params.yaml" command="load" />
+		<rosparam file="$(find turtlebot3_navigation)/param/dwa_local_planner_params_$(arg model).yaml" command="load" />
+		<remap from="cmd_vel" to="$(arg cmd_vel_topic)"/>
+		<remap from="odom" to="$(arg odom_topic)"/>
+		<param name="DWAPlannerROS/min_vel_x" value="0.0" if="$(arg move_forward_only)" />
+	</node>
+</launch>
\ No newline at end of file
diff --git a/labs/catkin_ws/src/lab5/package.xml b/lab_ws/src/lab5/package.xml
similarity index 100%
rename from labs/catkin_ws/src/lab5/package.xml
rename to lab_ws/src/lab5/package.xml
diff --git a/lab_ws/src/lab5/src/goal.cpp b/lab_ws/src/lab5/src/goal.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a0c63247efc70b8735165060b630a53b52999295
--- /dev/null
+++ b/lab_ws/src/lab5/src/goal.cpp
@@ -0,0 +1,38 @@
+#include <ros/ros.h>
+#include <geometry_msgs/PoseStamped.h>
+
+int main(int argc, char ** argv)
+{
+    ros::init(argc, argv, "gloal_publisher");
+    ros::NodeHandle nh;
+
+    ros::Publisher pose_pub = nh.advertise<geometry_msgs::PoseStamped>("/move_base_simple/goal", 1000);
+    ros::Rate loop_rate(10);
+    int count = 0;
+
+    while(ros::ok())
+    {
+        geometry_msgs::PoseStamped msg;
+        msg.header.frame_id="map";
+        msg.header.stamp=ros::Time::now();
+        msg.header.seq=count++;
+
+        msg.pose.orientation.x = 0.0;
+        msg.pose.orientation.y = 0.0;
+        msg.pose.orientation.z = 1.0;
+
+        //msg.pose.orientation = 
+        msg.pose.position.x = 1.0;
+        msg.pose.position.y = 0.0;
+        msg.pose.position.z = 0.0;
+
+        pose_pub.publish(msg);
+
+        ros::spinOnce();
+        loop_rate.sleep();
+    }
+    //working cmd on terminal
+    //rostopic pub /move_base_simple/goal geometry_msgs/PoseStamped '{ header: { frame_id:  "map"}, pose: { position: { x: 0.2, y: 0 }, orientation: { x: 0, y: 0, z: 0, w: 1 } } }'
+
+    return 0;
+}
\ No newline at end of file
diff --git a/lab_ws/src/lab5/turtlebot3_burger_camera.urdf.xacro b/lab_ws/src/lab5/turtlebot3_burger_camera.urdf.xacro
new file mode 100644
index 0000000000000000000000000000000000000000..285d81021d20cff1b920df115999c3a37393e45f
--- /dev/null
+++ b/lab_ws/src/lab5/turtlebot3_burger_camera.urdf.xacro
@@ -0,0 +1,77 @@
+<?xml version="1.0"?>
+<robot name="turtlebot3_burger_cam" xmlns:xacro="http://ros.org/wiki/xacro">
+
+  <xacro:property name="camera_link" value="0.05" /> <!-- Size of square 'camera' box -->
+
+  <joint name="camera_joint" type="fixed">
+    <axis xyz="0 1 0" />
+    <origin xyz="0.0 0.0 0.1" rpy="0 0 0"/>
+    <parent link="base_scan"/>
+    <child link="camera_link"/>
+  </joint>
+
+  <!-- Camera -->
+  <link name="camera_link">
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+    <box size="${camera_link} ${camera_link} ${camera_link}"/>
+      </geometry>
+    </collision>
+
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+    <box size="${camera_link} ${camera_link} ${camera_link}"/>
+      </geometry>
+      <material name="red"/>
+    </visual>
+
+    <inertial>
+      <mass value="1e-5" />
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6" />
+    </inertial>
+  </link>
+
+  <!-- camera -->
+  <gazebo reference="camera_link">
+    <sensor type="camera" name="camera1">
+      <update_rate>30.0</update_rate>
+      <camera name="head">
+        <horizontal_fov>1.3962634</horizontal_fov>
+        <image>
+          <width>800</width>
+          <height>800</height>
+          <format>R8G8B8</format>
+        </image>
+        <clip>
+          <near>0.02</near>
+          <far>300</far>
+        </clip>
+        <noise>
+          <type>gaussian</type>
+          <!-- Noise is sampled independently per pixel on each frame.
+               That pixel's noise value is added to each of its color
+               channels, which at that point lie in the range [0,1]. -->
+          <mean>0.0</mean>
+          <stddev>0.007</stddev>
+        </noise>
+      </camera>
+      <plugin name="camera_controller" filename="libgazebo_ros_camera.so">
+        <alwaysOn>true</alwaysOn>
+        <updateRate>0.0</updateRate>
+        <cameraName>turtlebot3/camera</cameraName>
+        <imageTopicName>image_raw</imageTopicName>
+        <cameraInfoTopicName>camera_info</cameraInfoTopicName>
+        <frameName>camera_link</frameName>
+        <hackBaseline>0.07</hackBaseline>
+        <distortionK1>0.0</distortionK1>
+        <distortionK2>0.0</distortionK2>
+        <distortionK3>0.0</distortionK3>
+        <distortionT1>0.0</distortionT1>
+        <distortionT2>0.0</distortionT2>
+      </plugin>
+    </sensor>
+  </gazebo>
+</robot>
diff --git a/labs/catkin_ws/src/lab5/urdf/turtlebot3_burger.gazebo.xacro b/lab_ws/src/lab5/urdf/turtlebot3_burger.gazebo.xacro
similarity index 100%
rename from labs/catkin_ws/src/lab5/urdf/turtlebot3_burger.gazebo.xacro
rename to lab_ws/src/lab5/urdf/turtlebot3_burger.gazebo.xacro
diff --git a/labs/catkin_ws/src/lab5/urdf/turtlebot3_burger.urdf.xacro b/lab_ws/src/lab5/urdf/turtlebot3_burger.urdf.xacro
similarity index 100%
rename from labs/catkin_ws/src/lab5/urdf/turtlebot3_burger.urdf.xacro
rename to lab_ws/src/lab5/urdf/turtlebot3_burger.urdf.xacro
diff --git a/labs/catkin_ws/src/lab5/urdf/turtlebot3_burger_camera.urdf.xacro b/lab_ws/src/lab5/urdf/turtlebot3_burger_camera.urdf.xacro
similarity index 100%
rename from labs/catkin_ws/src/lab5/urdf/turtlebot3_burger_camera.urdf.xacro
rename to lab_ws/src/lab5/urdf/turtlebot3_burger_camera.urdf.xacro
diff --git a/labs/catkin_ws/src/lab5/worlds/ex1_world b/lab_ws/src/lab5/worlds/ex1_world
similarity index 100%
rename from labs/catkin_ws/src/lab5/worlds/ex1_world
rename to lab_ws/src/lab5/worlds/ex1_world
diff --git a/labs/catkin_ws/src/lab5/worlds/ex2.world b/lab_ws/src/lab5/worlds/ex2.world
similarity index 100%
rename from labs/catkin_ws/src/lab5/worlds/ex2.world
rename to lab_ws/src/lab5/worlds/ex2.world
diff --git a/labs/catkin_ws/src/lab5/worlds/lab5_world.world b/lab_ws/src/lab5/worlds/lab5_world.world
similarity index 100%
rename from labs/catkin_ws/src/lab5/worlds/lab5_world.world
rename to lab_ws/src/lab5/worlds/lab5_world.world
diff --git a/labs/catkin_ws/src/lab2/CMakeLists.txt~ b/labs/catkin_ws/src/lab2/CMakeLists.txt~
deleted file mode 100644
index f4d0e64a645307b5102eb534176bed2c05a10dbe..0000000000000000000000000000000000000000
--- a/labs/catkin_ws/src/lab2/CMakeLists.txt~
+++ /dev/null
@@ -1,206 +0,0 @@
-cmake_minimum_required(VERSION 3.0.2)
-project(lab2)
-
-## Compile as C++11, supported in ROS Kinetic and newer
-add_compile_options(-std=c++11)
-
-## Find catkin macros and libraries
-## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
-## is used, also find other catkin packages
-find_package(catkin REQUIRED COMPONENTS
-  roscpp
-  rospy
-  std_msgs
-)
-
-## System dependencies are found with CMake's conventions
-# find_package(Boost REQUIRED COMPONENTS system)
-
-
-## Uncomment this if the package has a setup.py. This macro ensures
-## modules and global scripts declared therein get installed
-## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
-# catkin_python_setup()
-
-################################################
-## Declare ROS messages, services and actions ##
-################################################
-
-## To declare and build messages, services or actions from within this
-## package, follow these steps:
-## * Let MSG_DEP_SET be the set of packages whose message types you use in
-##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
-## * In the file package.xml:
-##   * add a build_depend tag for "message_generation"
-##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
-##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
-##     but can be declared for certainty nonetheless:
-##     * add a exec_depend tag for "message_runtime"
-## * In this file (CMakeLists.txt):
-##   * add "message_generation" and every package in MSG_DEP_SET to
-##     find_package(catkin REQUIRED COMPONENTS ...)
-##   * add "message_runtime" and every package in MSG_DEP_SET to
-##     catkin_package(CATKIN_DEPENDS ...)
-##   * uncomment the add_*_files sections below as needed
-##     and list every .msg/.srv/.action file to be processed
-##   * uncomment the generate_messages entry below
-##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
-
-## Generate messages in the 'msg' folder
-# add_message_files(
-#   FILES
-#   Message1.msg
-#   Message2.msg
-# )
-
-## Generate services in the 'srv' folder
-# add_service_files(
-#   FILES
-#   Service1.srv
-#   Service2.srv
-# )
-
-## Generate actions in the 'action' folder
-# add_action_files(
-#   FILES
-#   Action1.action
-#   Action2.action
-# )
-
-## Generate added messages and services with any dependencies listed here
-# generate_messages(
-#   DEPENDENCIES
-#   std_msgs
-# )
-
-################################################
-## Declare ROS dynamic reconfigure parameters ##
-################################################
-
-## To declare and build dynamic reconfigure parameters within this
-## package, follow these steps:
-## * In the file package.xml:
-##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
-## * In this file (CMakeLists.txt):
-##   * add "dynamic_reconfigure" to
-##     find_package(catkin REQUIRED COMPONENTS ...)
-##   * uncomment the "generate_dynamic_reconfigure_options" section below
-##     and list every .cfg file to be processed
-
-## Generate dynamic reconfigure parameters in the 'cfg' folder
-# generate_dynamic_reconfigure_options(
-#   cfg/DynReconf1.cfg
-#   cfg/DynReconf2.cfg
-# )
-
-###################################
-## catkin specific configuration ##
-###################################
-## The catkin_package macro generates cmake config files for your package
-## Declare things to be passed to dependent projects
-## INCLUDE_DIRS: uncomment this if your package contains header files
-## LIBRARIES: libraries you create in this project that dependent projects also need
-## CATKIN_DEPENDS: catkin_packages dependent projects also need
-## DEPENDS: system dependencies of this project that dependent projects also need
-catkin_package(
-#  INCLUDE_DIRS include
-#  LIBRARIES lab2
-#  CATKIN_DEPENDS roscpp rospy std_msgs
-#  DEPENDS system_lib
-)
-
-###########
-## Build ##
-###########
-
-## Specify additional locations of header files
-## Your package locations should be listed before other locations
-include_directories(
-# include
-  ${catkin_INCLUDE_DIRS}
-)
-
-## Declare a C++ library
-# add_library(${PROJECT_NAME}
-#   src/${PROJECT_NAME}/lab2.cpp
-# )
-
-## Add cmake target dependencies of the library
-## as an example, code may need to be generated before libraries
-## either from message generation or dynamic reconfigure
-# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-
-## Declare a C++ executable
-## With catkin_make all packages are built within a single CMake context
-## The recommended prefix ensures that target names across packages don't collide
-add_executable(pose_listener /home/chris/3rdYear/RWR/Robot-With-Reflexes/labs/catkin_ws/src/lab2/pose_listener.cpp)
-#add_executable(turtle_map /home/chris/3rdYear/RWR/Robot-With-Reflexes/labs/catkin_ws/src/lab2/turtle_map.cpp)
-## Rename C++ executable without prefix
-## The above recommended prefix causes long target names, the following renames the
-## target back to the shorter version for ease of user use
-## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
-# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
-
-## Add cmake target dependencies of the executable
-## same as for the library above
-add_dependencies(pose_listener ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-#add_dependencies(turtle_map ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-## Specify libraries to link a library or executable target against
-target_link_libraries(pose_listener
-  ${catkin_LIBRARIES}
-)
-
-#############
-## Install ##
-#############
-
-# all install targets should use catkin DESTINATION variables
-# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
-
-## Mark executable scripts (Python etc.) for installation
-## in contrast to setup.py, you can choose the destination
-# catkin_install_python(PROGRAMS
-#   scripts/my_python_script
-#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-# )
-
-## Mark executables for installation
-## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
-# install(TARGETS ${PROJECT_NAME}_node
-#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-# )
-
-## Mark libraries for installation
-## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
-# install(TARGETS ${PROJECT_NAME}
-#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-#   RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
-# )
-
-## Mark cpp header files for installation
-# install(DIRECTORY include/${PROJECT_NAME}/
-#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
-#   FILES_MATCHING PATTERN "*.h"
-#   PATTERN ".svn" EXCLUDE
-# )
-
-## Mark other files for installation (e.g. launch and bag files, etc.)
-# install(FILES
-#   # myfile1
-#   # myfile2
-#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-# )
-
-#############
-## Testing ##
-#############
-
-## Add gtest based cpp test target and link libraries
-# catkin_add_gtest(${PROJECT_NAME}-test test/test_lab2.cpp)
-# if(TARGET ${PROJECT_NAME}-test)
-#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
-# endif()
-
-## Add folders to be run by python nosetests
-# catkin_add_nosetests(test)
diff --git a/sim_ws/src/eigen_catkin b/sim_ws/src/eigen_catkin
deleted file mode 160000
index 3323b388540fa95ec9da6f9cd887f70ead055edb..0000000000000000000000000000000000000000
--- a/sim_ws/src/eigen_catkin
+++ /dev/null
@@ -1 +0,0 @@
-Subproject commit 3323b388540fa95ec9da6f9cd887f70ead055edb
diff --git a/sim_ws/src/eigen_checks b/sim_ws/src/eigen_checks
deleted file mode 160000
index 22a6247a3df11bc285d43d1a030f4e874a413997..0000000000000000000000000000000000000000
--- a/sim_ws/src/eigen_checks
+++ /dev/null
@@ -1 +0,0 @@
-Subproject commit 22a6247a3df11bc285d43d1a030f4e874a413997
diff --git a/sim_ws/src/gflags_catkin b/sim_ws/src/gflags_catkin
deleted file mode 160000
index fc38fc525f7d48881aebb27a7b9978453556bbd4..0000000000000000000000000000000000000000
--- a/sim_ws/src/gflags_catkin
+++ /dev/null
@@ -1 +0,0 @@
-Subproject commit fc38fc525f7d48881aebb27a7b9978453556bbd4
diff --git a/spotMicroCode/catkin_ws/src/spotMicro/CMakeLists.txt b/spotMicroCode/catkin_ws/src/spotMicro/CMakeLists.txt
deleted file mode 120000
index 581e61db89fce59006b1ceb2d208d9f3e5fbcb5e..0000000000000000000000000000000000000000
--- a/spotMicroCode/catkin_ws/src/spotMicro/CMakeLists.txt
+++ /dev/null
@@ -1 +0,0 @@
-/opt/ros/kinetic/share/catkin/cmake/toplevel.cmake
\ No newline at end of file