Output TUM rgbd dataset compatible *.poses file

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.
 

src/backend/Deformation.cpp

@@ -39,12 +39,40 @@ Deformation::~Deformation()
     std::vector<std::pair<uint64_t, Eigen::Matrix4f> > 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) << " ";
 

src/utils/ConfigArgs.h

@@ -23,6 +23,7 @@
 #include <cassert>
 #include <string>
 #include <pcl/console/parse.h>
+#include <boost/filesystem.hpp>
 
 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<iSkip; i++)
+            {
+                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, "%f %f %f %f %f %f %f %f", &time, &x, &y, &z, &qx, &qy, &qz, &qw);
+
+                assert(n == 8);
+            }
+
+            fclose(fp);
+        }
 };
 
 #endif /* CONFIGARGS_H_ */