diff --git a/svo/CMakeLists.txt b/svo/CMakeLists.txt
index 7612a94..08aafe3 100644
--- a/svo/CMakeLists.txt
+++ b/svo/CMakeLists.txt
@@ -49,7 +49,7 @@ ELSE()
 ENDIF()
 
 # Add catkin and required ROS packages
-FIND_PACKAGE(catkin REQUIRED COMPONENTS roscpp vikit_common vikit_ros)
+FIND_PACKAGE(catkin REQUIRED COMPONENTS roscpp roslib vikit_common vikit_ros)
 
 # Add plain cmake packages 
 FIND_PACKAGE(G2O REQUIRED)
@@ -62,7 +62,7 @@ FIND_PACKAGE(Boost REQUIRED COMPONENTS system thread signals)
 # Describe catkin Project
 catkin_package(
     DEPENDS Eigen OpenCV Sophus Boost fast
-    CATKIN_DEPENDS roscpp vikit_common vikit_ros
+    CATKIN_DEPENDS roscpp roslib vikit_common vikit_ros
     INCLUDE_DIRS include
     LIBRARIES svo
 )
@@ -127,13 +127,6 @@ TARGET_LINK_LIBRARIES(svo ${LINK_LIBS})
 ################################################################################
 # TESTS
 
-IF(DEFINED ENV{SVO_DATASET_DIR})
-  ADD_DEFINITIONS(-DTEST_DATA_DIR=\"$ENV{SVO_DATASET_DIR}\")
-ELSE()
-  ADD_DEFINITIONS(-DTEST_DATA_DIR=\"${PROJECT_SOURCE_DIR}/test/data\")
-ENDIF()
-ADD_DEFINITIONS(-DTEST_TRACE_DIR=\"${PROJECT_SOURCE_DIR}/test/results\")
-
 ADD_EXECUTABLE(test_feature_align test/test_feature_alignment.cpp)
 TARGET_LINK_LIBRARIES(test_feature_align svo)
 
diff --git a/svo/package.xml b/svo/package.xml
index c2997c2..d51d015 100644
--- a/svo/package.xml
+++ b/svo/package.xml
@@ -14,11 +14,13 @@
 
   <!-- Dependencies needed to compile this package. -->
   <build_depend>roscpp</build_depend>
+  <build_depend>roslib</build_depend>
   <build_depend>vikit_common</build_depend>
   <build_depend>vikit_ros</build_depend>
 
   <!-- Dependencies needed after this package is compiled. -->
   <run_depend>roscpp</run_depend>
+  <run_depend>roslib</run_depend>
   <run_depend>vikit_common</run_depend>
   <run_depend>vikit_ros</run_depend>
 </package>
\ No newline at end of file
diff --git a/svo/test/test_depth_filter.cpp b/svo/test/test_depth_filter.cpp
index 545d9bc..a53947d 100644
--- a/svo/test/test_depth_filter.cpp
+++ b/svo/test/test_depth_filter.cpp
@@ -32,6 +32,7 @@
 #include <svo/feature.h>
 #include <svo/point.h>
 #include <svo/config.h>
+#include "test_utils.h"
 
 namespace {
 
@@ -151,14 +152,15 @@ void DepthFilterTest::testReconstruction(
   }
 
   // trace error
-  std::string trace_name(std::string(TEST_TRACE_DIR) + "/depth_filter_" + experiment_name + ".txt");
+  std::string trace_dir(svo::test_utils::getTraceDir());
+  std::string trace_name(trace_dir + "/depth_filter_" + experiment_name + ".txt");
   std::ofstream ofs(trace_name.c_str());
   for(std::list<ConvergedSeed>::iterator i=results_.begin(); i!=results_.end(); ++i)
     ofs << i->x_ << ", " << i->y_ << ", " << fabs(i->error_) << std::endl;
   ofs.close();
   
   // trace convergence rate
-  trace_name = std::string(TEST_TRACE_DIR) + "/depth_filter_" + experiment_name + "_convergence.txt";
+  trace_name = trace_dir + "/depth_filter_" + experiment_name + "_convergence.txt";
   ofs.open(trace_name.c_str());
   for(std::list<size_t>::iterator it=n_converged_per_iteration.begin();
       it!=n_converged_per_iteration.end(); ++it)
@@ -166,7 +168,7 @@ void DepthFilterTest::testReconstruction(
   ofs.close();
 
   // write ply file for pointcloud visualization in Meshlab
-  trace_name = std::string(TEST_TRACE_DIR) + "/depth_filter_" + experiment_name + ".ply";
+  trace_name = trace_dir + "/depth_filter_" + experiment_name + ".ply";
   ofs.open(trace_name.c_str());
   ofs << "ply" << std::endl
       << "format ascii 1.0" << std::endl
@@ -193,7 +195,7 @@ void DepthFilterTest::testReconstruction(
 int main(int argc, char** argv)
 {
   DepthFilterTest test;
-  std::string dataset_dir(std::string(TEST_DATA_DIR) + "/flying_room_1_rig_1");
+  std::string dataset_dir(svo::test_utils::getDatasetDir() + "/flying_room_1_rig_1");
   std::string experiment_name("synth_fast_2d");
   test.testReconstruction(dataset_dir, experiment_name);
   return 0;
diff --git a/svo/test/test_feature_alignment.cpp b/svo/test/test_feature_alignment.cpp
index 04bddf7..a7352e0 100644
--- a/svo/test/test_feature_alignment.cpp
+++ b/svo/test/test_feature_alignment.cpp
@@ -19,6 +19,7 @@
 #include <svo/feature_alignment.h>
 #include <vikit/timer.h>
 #include <vikit/aligned_mem.h>
+#include "test_utils.h"
 #if __ARM_NEON__
 #include <arm_neon.h>
 #endif
@@ -52,7 +53,8 @@ void generateRefPatchNoWarpInterpolate(const cv::Mat& img, const Vector2d& px,
 
 int main(int argc, char **argv)
 {
-  std::string img_name(std::string(TEST_DATA_DIR) + "/flying_room_1_rig_1/img/frame_000001_0.png");
+
+  std::string img_name(svo::test_utils::getDatasetDir() + "/flying_room_1_rig_1/img/frame_000001_0.png");
   printf("Loading image '%s'\n", img_name.c_str());
   cv::Mat img(cv::imread(img_name, 0));
   assert(img.type() == CV_8UC1);
diff --git a/svo/test/test_feature_detection.cpp b/svo/test/test_feature_detection.cpp
index 00e21a2..6fa3723 100644
--- a/svo/test/test_feature_detection.cpp
+++ b/svo/test/test_feature_detection.cpp
@@ -23,6 +23,7 @@
 #include <vikit/vision.h>
 #include <vikit/abstract_camera.h>
 #include <vikit/atan_camera.h>
+#include "test_utils.h"
 
 namespace {
 
@@ -31,7 +32,7 @@ using namespace std;
 
 void testCornerDetector()
 {
-  std::string img_name(std::string(TEST_DATA_DIR) + "/flying_room_1_rig_1/img/frame_000001_0.png");
+  std::string img_name(svo::test_utils::getDatasetDir() + "/flying_room_1_rig_1/img/frame_000001_0.png");
   printf("Loading image '%s'\n", img_name.c_str());
   cv::Mat img(cv::imread(img_name, 0));
   assert(img.type() == CV_8UC1 && !img.empty());
diff --git a/svo/test/test_matcher.cpp b/svo/test/test_matcher.cpp
index 3138d48..c81c1fe 100644
--- a/svo/test/test_matcher.cpp
+++ b/svo/test/test_matcher.cpp
@@ -22,6 +22,7 @@
 #include <svo/frame.h>
 #include <svo/config.h>
 #include <svo/feature.h>
+#include "test_utils.h"
 
 namespace {
 
@@ -33,7 +34,7 @@ class MatcherTest {
     cam_ = new vk::PinholeCamera(752, 480, 217.083701215, 217.083701215, 376, 240);
 
     // load images
-    std::string dataset_dir(std::string(TEST_DATA_DIR) + "/flying_room_1_rig_1");
+    std::string dataset_dir(svo::test_utils::getDatasetDir() + "/flying_room_1_rig_1");
     std::string img_name(dataset_dir+"/img/frame_000071_0.png");
     printf("Loading image '%s'\n", img_name.c_str());
     cv::Mat img_ref(cv::imread(img_name, 0));
@@ -112,31 +113,32 @@ void MatcherTest::testEpipolarSearchFullImg()
   }
 
   // compute mean, median and variance of error in converged area
-  printf("n converged:  \t %zu (ref: 194472)\n", n_converged);
-  printf("mean error:   \t %f (ref: 1.0848)\n", sum_error*100/n_converged);
+  printf("n converged:  \t %zu mm (ref: 169936)\n", n_converged);
+  printf("mean error:   \t %f mm (ref: 1.912410)\n", sum_error*100/n_converged);
   std::vector<double>::iterator it = errors.begin()+0.5*errors.size();
   std::nth_element(errors.begin(), it, errors.end());
-  printf("50-percentile: \t %f (ref: 0.1343)\n", *it*100);
+  printf("50-percentile: \t %f mm (ref: 1.462241)\n", *it*100);
   it = errors.begin()+0.8*errors.size();
   std::nth_element(errors.begin(), it, errors.end());
-  printf("80-percentile: \t %f (ref: 0.3151)\n", *it*100);
+  printf("80-percentile: \t %f mm (ref: 2.620957)\n", *it*100);
   it = errors.begin()+0.95*errors.size();
   std::nth_element(errors.begin(), it, errors.end());
-  printf("95-percentile: \t %f (ref: 1.3753)\n", *it*100);
+  printf("95-percentile: \t %f mm (ref: 4.154051)\n", *it*100);
 
   // save results to file
   std::ofstream output_stream;
-  std::string output_filename(std::string(TEST_TRACE_DIR) + "/depthmap.bin");
+  std::string trace_dir(svo::test_utils::getTraceDir());
+  std::string output_filename(trace_dir + "/depthmap.bin");
   output_stream.open(output_filename.c_str(), std::ios::out | std::ios::binary | std::ios::trunc);
   output_stream.write((const char *)depthmap.data, depthmap.rows*depthmap.cols*sizeof(float));
   output_stream.close();
 
-  output_filename.assign(std::string(TEST_TRACE_DIR) + "/depthmap_error.bin");
+  output_filename.assign(trace_dir + "/depthmap_error.bin");
   output_stream.open(output_filename.c_str(), std::ios::out | std::ios::binary | std::ios::trunc);
   output_stream.write((const char *)error_map.data, error_map.rows*error_map.cols*sizeof(float));
   output_stream.close();
 
-  output_filename.assign(std::string(TEST_TRACE_DIR) + "/depthmap_mask.bin");
+  output_filename.assign(trace_dir + "/depthmap_mask.bin");
   output_stream.open(output_filename.c_str(), std::ios::out | std::ios::binary | std::ios::trunc);
   output_stream.write((const char *)mask.data, mask.rows*mask.cols*sizeof(uint8_t));
   output_stream.close();
diff --git a/svo/test/test_pipeline.cpp b/svo/test/test_pipeline.cpp
index d328bf1..364b089 100644
--- a/svo/test/test_pipeline.cpp
+++ b/svo/test/test_pipeline.cpp
@@ -28,6 +28,7 @@
 #include <opencv2/opencv.hpp>
 #include <sophus/se3.h>
 #include <iostream>
+#include "test_utils.h"
 
 namespace svo {
 
@@ -44,8 +45,6 @@ public:
 
 BenchmarkNode::BenchmarkNode()
 {
-  Config::traceName() = "benchmark";
-  Config::traceDir() =  std::string(TEST_TRACE_DIR)+"/..";
   cam_ = new vk::PinholeCamera(752, 480, 217.083701215, 217.083701215, 376, 240);
   vo_ = new svo::FrameHandlerMono(cam_);
   vo_->start();
@@ -62,8 +61,9 @@ void BenchmarkNode::runFromFolder()
   for(int img_id = 1; img_id < 310; ++img_id)
   {
     // load image
-	std::stringstream ss;
-    ss << std::string(TEST_DATA_DIR) << "/flying_room_1_rig_1/img/frame_" << std::setw( 6 ) << std::setfill( '0' ) << img_id << "_0.png";
+    std::stringstream ss;
+    ss << svo::test_utils::getDatasetDir() << "/flying_room_1_rig_1/img/frame_"
+       << std::setw( 6 ) << std::setfill( '0' ) << img_id << "_0.png";
     if(img_id == 1)
       std::cout << "reading image " << ss.str() << std::endl;
     cv::Mat img(cv::imread(ss.str().c_str(), 0));
@@ -75,8 +75,8 @@ void BenchmarkNode::runFromFolder()
     // display tracking quality
     if(vo_->lastFrame() != NULL)
     	std::cout << "Frame-Id: " << vo_->lastFrame()->id_ << " \t"
-    		      << "#Features: " << vo_->lastNumObservations() << " \t"
-    		      << "Proc. Time: " << vo_->lastProcessingTime()*1000 << "ms \n";
+                  << "#Features: " << vo_->lastNumObservations() << " \t"
+                  << "Proc. Time: " << vo_->lastProcessingTime()*1000 << "ms \n";
   }
 }
 
diff --git a/svo/test/test_pose_optimizer.cpp b/svo/test/test_pose_optimizer.cpp
index 0c9a63f..0f1ea31 100644
--- a/svo/test/test_pose_optimizer.cpp
+++ b/svo/test/test_pose_optimizer.cpp
@@ -25,6 +25,7 @@
 #include <svo/config.h>
 #include <svo/feature_detection.h>
 #include <svo/pose_optimizer.h>
+#include "test_utils.h"
 
 namespace {
 
@@ -39,7 +40,7 @@ class PoseOptimizerTest {
     cam_ = new vk::PinholeCamera(752, 480, 217.083701215, 217.083701215, 376, 240);
 
     // load image
-    std::string dataset_dir(std::string(TEST_DATA_DIR) + "/flying_room_1_rig_1");
+    std::string dataset_dir(svo::test_utils::getDatasetDir() + "/flying_room_1_rig_1");
     std::string img_name(dataset_dir+"/img/frame_000001_0.png");
     printf("Loading image '%s'\n", img_name.c_str());
     cv::Mat img(cv::imread(img_name, 0));
diff --git a/svo/test/test_sparse_img_align.cpp b/svo/test/test_sparse_img_align.cpp
index 51950a8..dbb4c20 100644
--- a/svo/test/test_sparse_img_align.cpp
+++ b/svo/test/test_sparse_img_align.cpp
@@ -26,6 +26,7 @@
 #include <svo/point.h>
 #include <svo/feature.h>
 #include <svo/config.h>
+#include "test_utils.h"
 
 namespace {
 
@@ -68,7 +69,8 @@ void SparseImgAlignTest::testSequence(
   std::list<double> translation_error;
 
   Sophus::SE3 T_prev_w, T_prevgt_w;
-  std::string trace_name(std::string(TEST_TRACE_DIR) + "/sparse_img_align_" + experiment_name + "_trans_estimate.txt");
+  std::string trace_dir(svo::test_utils::getTraceDir());
+  std::string trace_name(trace_dir + "/sparse_img_align_" + experiment_name + "_trans_estimate.txt");
   std::ofstream ofs(trace_name.c_str());
   for(int i=0; iter != sequence.end() && i<30; ++iter, ++i)
   {
@@ -142,7 +144,7 @@ void SparseImgAlignTest::testSequence(
   ofs.close();
 
   // trace error
-  trace_name = std::string(TEST_TRACE_DIR) + "/sparse_img_align_" + experiment_name + "_trans_error.txt";
+  trace_name = trace_dir + "/sparse_img_align_" + experiment_name + "_trans_error.txt";
   ofs.open(trace_name.c_str());
   for(std::list<double>::iterator it=translation_error.begin(); it!=translation_error.end(); ++it)
     ofs << *it << std::endl;
@@ -155,7 +157,7 @@ void SparseImgAlignTest::testSequence(
 int main(int argc, char** argv)
 {
   std::string experiment_name("flying_room_1_rig_1_fast_minlev0");
-  std::string dataset_dir(std::string(TEST_DATA_DIR) + "/flying_room_1_rig_1");
+  std::string dataset_dir(svo::test_utils::getDatasetDir() + "/flying_room_1_rig_1");
   svo::feature_detection::FastDetector detector;
   svo::Config::triangMinCornerScore() = 20;
   svo::Config::kltMinLevel() = 0;
diff --git a/svo/test/test_utils.h b/svo/test/test_utils.h
new file mode 100644
index 0000000..5793bb7
--- /dev/null
+++ b/svo/test/test_utils.h
@@ -0,0 +1,47 @@
+// This file is part of SVO - Semi-direct Visual Odometry.
+//
+// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
+// (Robotics and Perception Group, University of Zurich, Switzerland).
+//
+// SVO is free software: you can redistribute it and/or modify it under the
+// terms of the GNU General Public License as published by the Free Software
+// Foundation, either version 3 of the License, or any later version.
+//
+// SVO is distributed in the hope that it will be useful, but WITHOUT ANY
+// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
+// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef TEST_UTILS_H_
+#define TEST_UTILS_H_
+
+#include <string.h>
+#include <cstdlib> // for getenv
+#include <ros/package.h>
+#include <vikit/params_helper.h>
+
+namespace svo {
+namespace test_utils {
+
+std::string getDatasetDir()
+{
+  std::string dataset_dir(ros::package::getPath("svo")+"/test/data");
+  const char* env_dir = std::getenv("SVO_DATASET_DIR");
+  if(env_dir != NULL)
+    dataset_dir = std::string(env_dir);
+  return dataset_dir;
+}
+
+std::string getTraceDir()
+{
+  std::string default_dir(ros::package::getPath("svo")+"/test/results");
+  return vk::getParam<std::string>("svo/trace_dir", default_dir);
+}
+
+} // namespace test_utils
+} // namespace svo
+
+
+#endif // TEST_UTILS_H_