From ab867a3047908afa40b9fc1884693f00bbf1b9c3 Mon Sep 17 00:00:00 2001 From: Christian Forster Date: Wed, 11 Jun 2014 17:41:10 +0200 Subject: [PATCH] added instructions on how to download test data. updated all tests for new dataset. --- svo/src/depth_filter.cpp | 7 ++++-- svo/test/README.md | 26 +++++++++++++++++++--- svo/test/test_depth_filter.cpp | 34 ++++++++++++++--------------- svo/test/test_feature_alignment.cpp | 8 +++---- svo/test/test_feature_detection.cpp | 4 ++-- svo/test/test_matcher.cpp | 32 +++++++++++++-------------- svo/test/test_pipeline.cpp | 12 ++++++---- svo/test/test_pose_optimizer.cpp | 14 ++++++------ svo/test/test_sparse_img_align.cpp | 6 ++--- 9 files changed, 84 insertions(+), 59 deletions(-) diff --git a/svo/src/depth_filter.cpp b/svo/src/depth_filter.cpp index 7f8f3c3..1fdd342 100644 --- a/svo/src/depth_filter.cpp +++ b/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 "< results_; std::vector 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 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(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(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::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; } diff --git a/svo/test/test_feature_alignment.cpp b/svo/test/test_feature_alignment.cpp index a7352e0..dac8911 100644 --- a/svo/test/test_feature_alignment.cpp +++ b/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__ diff --git a/svo/test/test_feature_detection.cpp b/svo/test/test_feature_detection.cpp index b91e97a..3ba2b2f 100644 --- a/svo/test/test_feature_detection.cpp +++ b/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); diff --git a/svo/test/test_matcher.cpp b/svo/test/test_matcher.cpp index 37ad1f1..0b65cb9 100644 --- a/svo/test/test_matcher.cpp +++ b/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::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; diff --git a/svo/test/test_pipeline.cpp b/svo/test/test_pipeline.cpp index 364b089..ede3b25 100644 --- a/svo/test/test_pipeline.cpp +++ b/svo/test/test_pipeline.cpp @@ -45,7 +45,7 @@ public: 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_. + } } } diff --git a/svo/test/test_pose_optimizer.cpp b/svo/test/test_pose_optimizer.cpp index 7dd79f8..59e0b46 100644 --- a/svo/test/test_pose_optimizer.cpp +++ b/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( diff --git a/svo/test/test_sparse_img_align.cpp b/svo/test/test_sparse_img_align.cpp index fe1ece5..1312da0 100644 --- a/svo/test/test_sparse_img_align.cpp +++ b/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;