From 10bfa5a6ce0ce4fffdaa39b655742de884e4480e Mon Sep 17 00:00:00 2001 From: JackyLiu Date: Mon, 5 Jun 2017 00:28:44 +0800 Subject: [PATCH 1/3] Load TUM dataset groundtruth.txt as initial pose --- src/utils/ConfigArgs.h | 37 +++++++++++++++++++++++++++++++++++++ 1 file changed, 37 insertions(+) diff --git a/src/utils/ConfigArgs.h b/src/utils/ConfigArgs.h index 72dba96..9aea7f5 100644 --- a/src/utils/ConfigArgs.h +++ b/src/utils/ConfigArgs.h @@ -23,6 +23,7 @@ #include #include #include +#include class ConfigArgs { @@ -67,6 +68,7 @@ class ConfigArgs " -fl : Subsample pose graph for faster loop closure\n" " -fod : Enable fast odometry\n" " -h, --help : Display this help message and exit\n" + " -tum : path of groundtruth.txt provided by TUM RGBD dataset to load initial camera pose\n" "\n" "Example: " + argv0 + " -s 7 -v ../vocab.yml.gz -l loop.klg -ri -fl -od"; @@ -79,6 +81,10 @@ class ConfigArgs std::string vocabFile; std::string trajectoryFile; + std::string tumGT; + long long unsigned int utime; + float x, y, z, qx, qy, qz, qw; + int gpu; int voxelShift; int totalNumFrames; @@ -133,6 +139,11 @@ class ConfigArgs pcl::console::parse_argument(argc, argv, "-l", logFile); pcl::console::parse_argument(argc, argv, "-v", vocabFile); pcl::console::parse_argument(argc, argv, "-p", trajectoryFile); + pcl::console::parse_argument(argc, argv, "-tum", tumGT); + if(tumGT.size()) + { + loadTUM_GT_initialpose(); + } pcl::console::parse_argument(argc, argv, "-gpu", gpu); pcl::console::parse_argument(argc, argv, "-t", voxelShift); @@ -183,6 +194,32 @@ class ConfigArgs saveFile = logFile; } } + + void loadTUM_GT_initialpose() + { + assert(boost::filesystem::exists(tumGT.c_str())); + FILE *fp = fopen(tumGT.c_str(), "r"); + + + char buffer[255]; + + int iSkip = 3; + for(int i=0; i Date: Mon, 5 Jun 2017 13:51:33 +0800 Subject: [PATCH 2/3] Modify trajectory when saving it. --- src/backend/Deformation.cpp | 28 ++++++++++++++++++++++++++++ src/utils/ConfigArgs.h | 4 ++-- 2 files changed, 30 insertions(+), 2 deletions(-) diff --git a/src/backend/Deformation.cpp b/src/backend/Deformation.cpp index 0e64deb..3fa980a 100644 --- a/src/backend/Deformation.cpp +++ b/src/backend/Deformation.cpp @@ -39,12 +39,40 @@ Deformation::~Deformation() std::vector > newPoseGraph; iSAM->getCameraPoses(newPoseGraph); + + Eigen::Vector3f *init_t; + Eigen::Matrix3f init_r; + if(ConfigArgs::get().tumGT.size()) + { + //Eigen::Vector3f init_t(ConfigArgs::get().x, ConfigArgs::get().y, ConfigArgs::get().z); + init_t = new Eigen::Vector3f(ConfigArgs::get().x, ConfigArgs::get().y, ConfigArgs::get().z); + + Eigen::Quaternionf init_q(ConfigArgs::get().qw, ConfigArgs::get().qx, ConfigArgs::get().qy, ConfigArgs::get().qz); + Eigen::Vector3f upVector(0, -1, 0); + + init_r = init_q.toRotationMatrix(); + } for(unsigned int i = 0; i < newPoseGraph.size(); i++) { file << std::setprecision(6) << std::fixed << (double)newPoseGraph.at(i).first / 1000000.0 << " "; Eigen::Vector3f trans = newPoseGraph.at(i).second.topRightCorner(3, 1); Eigen::Matrix3f rot = newPoseGraph.at(i).second.topLeftCorner(3, 3); + if(ConfigArgs::get().tumGT.size()) + { + if(0==i) + { + trans += *init_t; + rot = init_r * rot; + } + else + { + trans += init_r.inverse() * (*init_t); + trans = init_r * trans; + + rot = init_r * rot; + } + } file << trans(0) << " " << trans(1) << " " << trans(2) << " "; diff --git a/src/utils/ConfigArgs.h b/src/utils/ConfigArgs.h index 9aea7f5..2ca5fc8 100644 --- a/src/utils/ConfigArgs.h +++ b/src/utils/ConfigArgs.h @@ -208,12 +208,12 @@ class ConfigArgs { fgets(buffer, 255, (FILE*) fp); } - + double time; //Read first camera pose if(fgets(buffer, 255, (FILE*) fp)) { printf("%s\n", buffer); - int n = sscanf(buffer, "%llu %f %f %f %f %f %f %f", &utime, &x, &y, &z, &qx, &qy, &qz, &qw); + int n = sscanf(buffer, "%f %f %f %f %f %f %f %f", &time, &x, &y, &z, &qx, &qy, &qz, &qw); assert(n == 8); } From d41ab7fb1460021d69eab4c50fa4b22dc0a22bcd Mon Sep 17 00:00:00 2001 From: JackyLiu Date: Tue, 6 Jun 2017 01:04:31 +0800 Subject: [PATCH 3/3] Update README.md --- README.md | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/README.md b/README.md index 1d69fc7..b04ebd8 100644 --- a/README.md +++ b/README.md @@ -169,6 +169,20 @@ We have provided a sample dataset which you can run easily with Kintinuous for d ./Kintinuous -s 7 -v ../vocab.yml.gz -l loop.klg -ri -fl -od ``` +## TUM rgbd dataset ## +[TUM RGBD dataset](http://vision.in.tum.de/data/datasets/rgbd-dataset/download) +1. Download [fr2_desk ](http://vision.in.tum.de/rgbd/dataset/freiburg2/rgbd_dataset_freiburg2_desk.tgz) +2. Uncompress by `tar xvfz rgbd_dataset_freiburg2_desk.tgz` +3. Download `associate.py` from TUM [link](http://vision.in.tum.de/data/datasets/rgbd-dataset/tools) +4. Associate files by `python associate.py ./rgbd_dataset_freiburg2_desk/depth.txt ./rgbd_dataset_freiburg2_desk/rgb.txt > ./rgbd_dataset_freiburg2_desk/associations.txt` +5. Use [png_to_klg](https://github.com/HTLife/png_to_klg) to convert TUM dataset +6. `./pngtoklg -w '~/rgbd_dataset_freiburg2_desk' -o '~/rgbd_dataset_freiburg2_desk/fr2_desk.klg' -t` + +```bash +./Kintinuous -s 7 -v ../vocab.yml.gz -l ~/rgbd_dataset_freiburg2_desk/fr2_desk.klg -ri -fl -od -tum ~/rgbd_dataset_freiburg2_desk/groundtruth.txt -f -s 10 -dg 0.1 +``` +Evaluate *.poses file in `~/rgbd_dataset_freiburg2_desk` with TUM [evaluate_ate.py](https://svncvpr.in.tum.de/cvpr-ros-pkg/trunk/rgbd_benchmark/rgbd_benchmark_tools/src/rgbd_benchmark_tools/) + # 5. License and Copyright # The use of the code within this repository and all code within files that make up the software that is Kintinuous is permitted for non-commercial purposes only. The full terms and conditions that apply to the code within this repository are detailed within the LICENSE.txt file and at [http://www.cs.nuim.ie/research/vision/data/kintinuous/code.php](http://www.cs.nuim.ie/research/vision/data/kintinuous/code.php) unless explicitly stated. By accessing this repository you agree to comply with these terms.