added instructions on how to download test data. updated all tests fo…

…r new dataset.

svo/src/depth_filter.cpp

@@ -126,7 +126,9 @@ void DepthFilter::initializeSeeds(FramePtr frame)
     seeds_.push_back(Seed(ftr, new_keyframe_mean_depth_, new_keyframe_min_depth_));
   });
 
-  SVO_DEBUG_STREAM("DepthFilter: Initialized "<<new_features.size()<<" new seeds");
+  if(options_.verbose)
+    SVO_INFO_STREAM("DepthFilter: Initialized "<<new_features.size()<<" new seeds");
+  seeds_updating_halt_ = false;
 }
 
 void DepthFilter::removeKeyframe(FramePtr frame)
@@ -160,7 +162,8 @@ void DepthFilter::reset()
     frame_queue_.pop();
   seeds_updating_halt_ = false;
 
-  SVO_DEBUG_STREAM("DepthFilter: RESET.");
+  if(options_.verbose)
+    SVO_INFO_STREAM("DepthFilter: RESET.");
 }
 
 void DepthFilter::updateSeedsLoop()

svo/test/README.md

@@ -1,4 +1,24 @@
-Download Synthetic Test Data:
------------------------------
+#### Download Test Data
 
-TODO
+Create a folder where you will store all datasets, e.g.:
+
+    mkdir Datasets
+
+In your `.bashrc` script, create a new environment variable pointing to the dataset folder:
+
+    export SVO_DATASET_DIR=${HOME}/Datasets
+
+Source your new `.bashrc` script, go to the new dataset folder and download the test data:
+
+    source `~/.bashrc`
+    cd ${SVO_DATASET_DIR}
+    wget http://rpg.ifi.uzh.ch/datasets/sin2_tex2_h1_v8_d.tar.gz -O - | tar -xz
+
+You can then run the tests, e.g.:
+
+    rosrun svo test_pipeline
+
+or if you are not using ROS:
+
+    cd svo/bin
+    ./test_pipeline

svo/test/test_depth_filter.cpp

@@ -54,7 +54,6 @@ class DepthFilterTest {
 
   std::list<ConvergedSeed> results_;
   std::vector<double> errors_;
-  double error_sum_;
   size_t n_converged_seeds_;
   vk::PinholeCamera* cam_;
   svo::DepthFilter* depth_filter_;
@@ -65,7 +64,7 @@ class DepthFilterTest {
 
 DepthFilterTest::DepthFilterTest() :
     n_converged_seeds_(0),
-    cam_(new vk::PinholeCamera(752, 480, 217.083701215, 217.083701215, 376, 240)),
+    cam_(new vk::PinholeCamera(752, 480, 315.5, 315.5, 376.0, 240.0)),
     depth_filter_(NULL)
 {
   errors_.reserve(1000);
@@ -91,7 +90,6 @@ void DepthFilterTest::depthFilterCb(svo::Point* point, double depth_sigma2)
 
 void DepthFilterTest::testReconstruction(
     std::string dataset_dir,
-
     std::string experiment_name)
 {
   vk::FileReader<vk::blender_utils::file_format::ImageNameAndPose> sequence_file_reader(dataset_dir+"/trajectory.txt");
@@ -110,28 +108,28 @@ void DepthFilterTest::testReconstruction(
           cam_->width(), cam_->height(), svo::Config::gridSize(), svo::Config::nPyrLevels()));
   svo::DepthFilter::callback_t depth_filter_cb = boost::bind(&DepthFilterTest::depthFilterCb, this, _1, _2);
   depth_filter_ = new svo::DepthFilter(feature_detector, depth_filter_cb);
+  depth_filter_->options_.verbose = true;
 
-  for(int i=0; it != sequence.end() && i<50; ++it, ++i)
+  for(int i=0; it != sequence.end() && i<20; ++it, ++i)
   {
     std::string img_name(dataset_dir+"/img/" + (*it).image_name_ + "_0.png");
-    if(i==0)
-      printf("reading image: '%s'\n", img_name.c_str());
+    printf("reading image: '%s'\n", img_name.c_str());
     cv::Mat img(cv::imread(img_name, 0));
     assert(!img.empty());
 
-    Sophus::SE3 T_w_f = Sophus::SE3(it->q_.toRotationMatrix(), it->t_);
+    Sophus::SE3 T_w_f(it->q_, it->t_);
     if(i == 0)
     {
       // create reference frame and load ground truth depthmap
-      frame_ref_.reset(new svo::Frame(cam_, img, 0.0));
+      frame_ref_ = boost::make_shared<svo::Frame>(cam_, img, 0.0);
       frame_ref_->T_f_w_ = T_w_f.inverse();
-      depth_filter_->addKeyframe(frame_ref_, 3, 1);
+      depth_filter_->addKeyframe(frame_ref_, 2, 0.5);
       vk::blender_utils::loadBlenderDepthmap(dataset_dir+"/depth/" + (*it).image_name_ + "_0.depth", *cam_, depth_ref_);
       continue;
     }
 
-    n_converged_seeds_=0;
-    frame_cur_ = svo::FramePtr(new svo::Frame(cam_, img, 0.0));
+    n_converged_seeds_ = 0;
+    frame_cur_ = boost::make_shared<svo::Frame>(cam_, img, 0.0);
     frame_cur_->T_f_w_ = T_w_f.inverse();
     depth_filter_->addFrame(frame_cur_);
     n_converged_per_iteration.push_back(n_converged_seeds_);
@@ -140,19 +138,19 @@ void DepthFilterTest::testReconstruction(
 
   // compute mean, median and variance of error in converged area
   {
-    printf("# converged:  \t %zu (ref: 349)\n", errors_.size());
+    printf("# converged:  \t %zu (ref: 287)\n", errors_.size());
     double sum_error = 0;
     std::for_each(errors_.begin(), errors_.end(), [&](double& e){sum_error+=e;});
-    printf("mean error:   \t %f cm (ref: 1.510507)\n", sum_error*100/errors_.size());
+    printf("mean error:   \t %f cm (ref: 0.080357)\n", sum_error*100/errors_.size());
     std::vector<double>::iterator it = errors_.begin()+0.5*errors_.size();
     std::nth_element(errors_.begin(), it, errors_.end());
-    printf("50-percentile: \t %f cm (ref: 0.809308)\n", *it*100);
+    printf("50-percentile: \t %f cm (ref: 0.062042)\n", *it*100);
     it = errors_.begin()+0.8*errors_.size();
     std::nth_element(errors_.begin(), it, errors_.end());
-    printf("80-percentile: \t %f cm (ref: 1.580725)\n", *it*100);
+    printf("80-percentile: \t %f cm (ref: 0.124526)\n", *it*100);
     it = errors_.begin()+0.95*errors_.size();
     std::nth_element(errors_.begin(), it, errors_.end());
-    printf("95-percentile: \t %f cm (ref: 2.557531)\n", *it*100);
+    printf("95-percentile: \t %f cm (ref: 0.200417)\n", *it*100);
   }
 
   // trace error
@@ -199,8 +197,8 @@ void DepthFilterTest::testReconstruction(
 int main(int argc, char** argv)
 {
   DepthFilterTest test;
-  std::string dataset_dir(svo::test_utils::getDatasetDir() + "/flying_room_1_rig_1");
-  std::string experiment_name("synth_fast_2d");
+  std::string dataset_dir(svo::test_utils::getDatasetDir() + "/sin2_tex2_h1_v8_d");
+  std::string experiment_name("sin2_tex2_h1_v8_d");
   test.testReconstruction(dataset_dir, experiment_name);
   return 0;
 }

svo/test/test_feature_alignment.cpp

@@ -54,7 +54,7 @@ void generateRefPatchNoWarpInterpolate(const cv::Mat& img, const Vector2d& px,
 int main(int argc, char **argv)
 {
 
-  std::string img_name(svo::test_utils::getDatasetDir() + "/flying_room_1_rig_1/img/frame_000001_0.png");
+  std::string img_name(svo::test_utils::getDatasetDir() + "/sin2_tex2_h1_v8_d/img/frame_000002_0.png");
   printf("Loading image '%s'\n", img_name.c_str());
   cv::Mat img(cv::imread(img_name, 0));
   assert(img.type() == CV_8UC1);
@@ -86,7 +86,7 @@ int main(int argc, char **argv)
     svo::feature_alignment::align1D(img, dir, ref_patch_with_border.data, ref_patch, 3, px_est, h_inv);
   }
   Vector2d e = px_est-px_true;
-  printf("1000Xalign 1D took %fms, error = %fpx \t (ref i7-W520: 1.637000ms, Odroid-U2: ?, 0.008788px) \n", t.stop()*1000, e.norm());
+  printf("1000Xalign 1D took %fms, error = %fpx \t (ref i7-W520: 1.982000ms, 0.000033px) \n", t.stop()*1000, e.norm());
 
   t.start();
   for(int i=0; i<1000; ++i)
@@ -95,7 +95,7 @@ int main(int argc, char **argv)
     svo::feature_alignment::align2D(img, ref_patch_with_border.data, ref_patch, 3, px_est, true);
   }
   e = px_est-px_true;
-  printf("1000Xalign 2D took %fms, error = %fpx \t (ref i7-W520: 2.147000ms, Odroid-U2: ?, 0.009632px)\n", t.stop()*1000, e.norm());
+  printf("1000Xalign 2D took %fms, error = %fpx \t (ref i7-W520: 2.306000ms, 0.015102px)\n", t.stop()*1000, e.norm());
 
 #ifdef __SSE2__
   // Note, this KLT implementation is not invariant to illuminatino changes!
@@ -106,7 +106,7 @@ int main(int argc, char **argv)
     svo::feature_alignment::align2D_SSE2(img, ref_patch_with_border.data, ref_patch, 3, px_est);
   }
   e = px_est-px_true;
-  printf("1000Xalign 2D SSE2 %fms, error = %fpx \t (ref i7-W520: 0.548000ms, 0.023032px)\n", t.stop()*1000, e.norm());
+  printf("1000Xalign 2D SSE2 %fms, error = %fpx \t (ref i7-W520: 0.460000ms, 0.021881px)\n", t.stop()*1000, e.norm());
 #endif
 
 #ifdef __ARM_NEON__

svo/test/test_feature_detection.cpp

@@ -34,7 +34,7 @@ using namespace std;
 
 void testCornerDetector()
 {
-  std::string img_name(svo::test_utils::getDatasetDir() + "/flying_room_1_rig_1/img/frame_000001_0.png");
+  std::string img_name(svo::test_utils::getDatasetDir() + "/sin2_tex2_h1_v8_d/img/frame_000002_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());
@@ -51,7 +51,7 @@ void testCornerDetector()
   {
     fast_detector.detect(frame.get(), frame->img_pyr_, svo::Config::triangMinCornerScore(), fts);
   }
-  printf("Fast corner detection took %f ms, %zu corners detected (ref i7-W520: 8.06878ms, 416)\n", t.stop()*10, fts.size());
+  printf("Fast corner detection took %f ms, %zu corners detected (ref i7-W520: 7.166360ms, 40000)\n", t.stop()*10, fts.size());
   printf("Note, in this case, feature detection also contains the cam2world projection of the feature.\n");
   cv::Mat img_rgb = cv::Mat(img.size(), CV_8UC3);
   cv::cvtColor(img, img_rgb, CV_GRAY2RGB);

svo/test/test_matcher.cpp

@@ -31,14 +31,14 @@ class MatcherTest {
  public:
   MatcherTest()
   {
-    cam_ = new vk::PinholeCamera(752, 480, 217.083701215, 217.083701215, 376, 240);
+    cam_ = new vk::PinholeCamera(752, 480, 315.5, 315.5, 376.0, 240.0);
 
     // load images
-    std::string dataset_dir(svo::test_utils::getDatasetDir() + "/flying_room_1_rig_1");
-    std::string img_name(dataset_dir+"/img/frame_000071_0.png");
+    std::string dataset_dir(svo::test_utils::getDatasetDir() + "/sin2_tex2_h1_v8_d");
+    std::string img_name(dataset_dir+"/img/frame_000002_0.png");
     printf("Loading image '%s'\n", img_name.c_str());
     cv::Mat img_ref(cv::imread(img_name, 0));
-    img_name = std::string(dataset_dir+"/img/frame_000097_0.png");
+    img_name = std::string(dataset_dir+"/img/frame_000006_0.png");
     printf("Loading image '%s'\n", img_name.c_str());
     cv::Mat img_cur(cv::imread(img_name, 0));
     assert(!img_ref.empty() && !img_cur.empty());
@@ -49,15 +49,15 @@ class MatcherTest {
     ref_ftr_ = new svo::Feature(frame_ref_, Eigen::Vector2d(300, 260), 0);
 
     // set poses
-    Eigen::Vector3d t_w_ref(1.0054399967193604, 1.75655996799469, 4.610799789428711);
-    Eigen::Vector3d t_w_cur(-0.046640001237392426, 2.163439989089966, 4.6279802322387695);
-    Eigen::Quaterniond q_w_ref(0.015279541723430157, 0.7095794081687927, 0.7044594287872314, 0.0006494877743534744);
-    Eigen::Quaterniond q_w_cur(0.004395846277475357, -0.7776520252227783, -0.6286740899085999, -0.0026246386114507914);
-    frame_ref_->T_f_w_ = Sophus::SE3(q_w_ref.toRotationMatrix(), t_w_ref).inverse();
-    frame_cur_->T_f_w_ = Sophus::SE3(q_w_cur.toRotationMatrix(), t_w_cur).inverse();
+    Eigen::Vector3d t_w_ref(0.1131, 0.1131, 2.0000);
+    Eigen::Vector3d t_w_cur(0.5673, 0.5641, 2.0000);
+    Eigen::Quaterniond q_w_ref(0.0, 0.8227, 0.2149, 0.0);
+    Eigen::Quaterniond q_w_cur(0.0, 0.8235, 0.2130, 0.0);
+    frame_ref_->T_f_w_ = Sophus::SE3(q_w_ref, t_w_ref).inverse();
+    frame_cur_->T_f_w_ = Sophus::SE3(q_w_cur, t_w_cur).inverse();
 
     // load ground-truth depth
-    vk::blender_utils::loadBlenderDepthmap(dataset_dir+"/depth/frame_000071_0.depth", *cam_, depth_ref_);
+    vk::blender_utils::loadBlenderDepthmap(dataset_dir+"/depth/frame_000002_0.depth", *cam_, depth_ref_);
   }
 
   virtual ~MatcherTest()
@@ -114,17 +114,17 @@ void MatcherTest::testEpipolarSearchFullImg()
   }
 
   // compute mean, median and variance of error in converged area
-  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);
+  printf("n converged:  \t %zu mm (ref: 216114)\n", n_converged);
+  printf("mean error:   \t %f mm (ref: 0.410084)\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 mm (ref: 1.462241)\n", *it*100);
+  printf("50-percentile: \t %f mm (ref: 0.083203)\n", *it*100);
   it = errors.begin()+0.8*errors.size();
   std::nth_element(errors.begin(), it, errors.end());
-  printf("80-percentile: \t %f mm (ref: 2.620957)\n", *it*100);
+  printf("80-percentile: \t %f mm (ref: 0.161824)\n", *it*100);
   it = errors.begin()+0.95*errors.size();
   std::nth_element(errors.begin(), it, errors.end());
-  printf("95-percentile: \t %f mm (ref: 4.154051)\n", *it*100);
+  printf("95-percentile: \t %f mm (ref: 0.263539)\n", *it*100);
 
   // save results to file
   std::ofstream output_stream;

svo/test/test_pipeline.cpp

@@ -45,7 +45,7 @@ class BenchmarkNode
 
 BenchmarkNode::BenchmarkNode()
 {
-  cam_ = new vk::PinholeCamera(752, 480, 217.083701215, 217.083701215, 376, 240);
+  cam_ = new vk::PinholeCamera(752, 480, 315.5, 315.5, 376.0, 240.0);
   vo_ = new svo::FrameHandlerMono(cam_);
   vo_->start();
 }
@@ -58,13 +58,13 @@ BenchmarkNode::~BenchmarkNode()
 
 void BenchmarkNode::runFromFolder()
 {
-  for(int img_id = 1; img_id < 310; ++img_id)
+  for(int img_id = 2; img_id < 188; ++img_id)
   {
     // load image
     std::stringstream ss;
-    ss << svo::test_utils::getDatasetDir() << "/flying_room_1_rig_1/img/frame_"
+    ss << svo::test_utils::getDatasetDir() << "/sin2_tex2_h1_v8_d/img/frame_"
        << std::setw( 6 ) << std::setfill( '0' ) << img_id << "_0.png";
-    if(img_id == 1)
+    if(img_id == 2)
       std::cout << "reading image " << ss.str() << std::endl;
     cv::Mat img(cv::imread(ss.str().c_str(), 0));
     assert(!img.empty());
@@ -74,9 +74,13 @@ 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";
+
+    	// access the pose of the camera via vo_->lastFrame()->T_f_w_.
+    }
   }
 }
 

svo/test/test_pose_optimizer.cpp

@@ -37,11 +37,11 @@ class PoseOptimizerTest {
  public:
   PoseOptimizerTest()
   {
-    cam_ = new vk::PinholeCamera(752, 480, 217.083701215, 217.083701215, 376, 240);
+    cam_ = new vk::PinholeCamera(752, 480, 315.5, 315.5, 376.0, 240.0);
 
     // load image
-    std::string dataset_dir(svo::test_utils::getDatasetDir() + "/flying_room_1_rig_1");
-    std::string img_name(dataset_dir+"/img/frame_000001_0.png");
+    std::string dataset_dir(svo::test_utils::getDatasetDir() + "/sin2_tex2_h1_v8_d");
+    std::string img_name(dataset_dir+"/img/frame_000002_0.png");
     printf("Loading image '%s'\n", img_name.c_str());
     cv::Mat img(cv::imread(img_name, 0));
     assert(!img.empty());
@@ -50,12 +50,12 @@ class PoseOptimizerTest {
     frame_.reset(new svo::Frame(cam_, img, 1.0));
 
     // set pose
-    Eigen::Vector3d t_w_ref(2.139359951019287, -1.9544399976730347, 2.8595199584960938  );
-    Eigen::Quaterniond q_w_ref(0.0027939670253545046, 0.8382523059844971, 0.5443645715713501, -0.03150530532002449);
-    frame_->T_f_w_ = Sophus::SE3(q_w_ref.toRotationMatrix(), t_w_ref).inverse();
+    Eigen::Vector3d t_w_ref(0.1131, 0.1131, 2.0000);
+    Eigen::Quaterniond q_w_ref(0.0, 0.8227, 0.2149, 0.0);
+    frame_->T_f_w_ = Sophus::SE3(q_w_ref, t_w_ref).inverse();
 
     // load ground-truth depth
-    vk::blender_utils::loadBlenderDepthmap(dataset_dir + "/depth/frame_000001_0.depth", *cam_, depthmap_);
+    vk::blender_utils::loadBlenderDepthmap(dataset_dir + "/depth/frame_000002_0.depth", *cam_, depthmap_);
 
     // detect features
     feature_detection::FastDetector detector(

svo/test/test_sparse_img_align.cpp

@@ -47,7 +47,7 @@ class SparseImgAlignTest {
 };
 
 SparseImgAlignTest::SparseImgAlignTest() :
-    cam_(new vk::PinholeCamera(752, 480, 217.083701215, 217.083701215, 376, 240))
+    cam_(new vk::PinholeCamera(752, 480, 315.5, 315.5, 376.0, 240.0))
 {}
 
 SparseImgAlignTest::~SparseImgAlignTest()
@@ -82,7 +82,7 @@ void SparseImgAlignTest::testSequence(
     assert(!img.empty());
 
     // load pose
-    Sophus::SE3 T_w_gt = Sophus::SE3(iter->q_.toRotationMatrix(), iter->t_);
+    Sophus::SE3 T_w_gt(iter->q_, iter->t_);
     Sophus::SE3 T_gt_w = T_w_gt.inverse(); // ground-truth
 
     if(i==0)
@@ -152,7 +152,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(svo::test_utils::getDatasetDir() + "/flying_room_1_rig_1");
+  std::string dataset_dir(svo::test_utils::getDatasetDir() + "/sin2_tex2_h1_v8_d");
   svo::Config::triangMinCornerScore() = 20;
   svo::Config::kltMinLevel() = 0;
   SparseImgAlignTest test;