1 // This file is part of OpenMVG, an Open Multiple View Geometry C++ library.
2 
3 // Copyright (c) 2018 Yan Qingsong, Pierre Moulon.
4 
5 // This Source Code Form is subject to the terms of the Mozilla Public
6 // License, v. 2.0. If a copy of the MPL was not distributed with this
7 // file, You can obtain one at http://mozilla.org/MPL/2.0/.
8 
9 #ifndef IO_READ_GT_KITTI_HPP
10 #define IO_READ_GT_KITTI_HPP
11 
12 #include "io_readGTInterface.hpp"
13 #include "io_loadImages.hpp"
14 
15 #include "openMVG/cameras/PinholeCamera.hpp"
16 
17 #include <fstream>
18 #include <iomanip>
19 
20 // The feature of the Kitti's Data:
21 // 1. all the gt information are stored in two file:
22 //  - ID.txt for the camera poses,
23 //  - calib.txt for the camera intrinsic parameters.
24 // 2. the gt information's line number is the same with the corresponding image file name
25 class SfM_Data_GT_Loader_Kitti : public SfM_Data_GT_Loader_Interface
26 {
27 private:
28   std::vector<cameras::PinholeCamera> cameras_data_; // Store all the camera information
29 public:
loadGT()30   bool loadGT() override
31   {
32     // Check all the files under the path
33     std::vector<std::string> gt_files = stlplus::folder_files( this->gt_dir_ );
34 
35     // Kitti's Data store all the data in two file
36     // ID.txt    -> camera location ([R|C])
37     // calib.txt -> camera calibration
38     // So make sure there is only one file under the gt_dir
39     if (gt_files.size() != 2 || gt_files.empty())
40     {
41       std::cerr << "Error: Maybe give wrong gt_dir!" << std::endl
42         << "Make sure there you have only those 2 files under the gt_dir! (<ID>.txt and calib.txt)" << std::endl;
43       return false;
44     }
45 
46     // Read the camera calibration
47     Mat3 calibration_matrix;
48     auto calib_file_it = std::find(gt_files.begin(), gt_files.end(), "calib.txt");
49     if (calib_file_it != gt_files.cend())
50     {
51       std::ifstream calib_file( stlplus::create_filespec(this->gt_dir_, "calib.txt"));
52       if (!calib_file)
53       {
54         std::cerr << "Cannot open the calib.txt file" << std::endl;
55         return false;
56       }
57       std::string temp;
58       calib_file >> temp;
59       Eigen::Matrix<double, 3, 4, Eigen::RowMajor> P;
60       for (int i = 0; i < 12; ++i)
61         calib_file >> *(P.data() + i);
62       calibration_matrix = P.block<3, 3>(0, 0);
63     }
64     else
65     {
66       std::cerr << "Cannot find the expected calib.txt file" << std::endl;
67       return false;
68     }
69     gt_files.erase(calib_file_it);
70 
71     // Load the gt_data from the file
72     std::ifstream gt_file( stlplus::create_filespec(this->gt_dir_, gt_files[0]));
73     if (!gt_file)
74     {
75       std::cerr << "Error: Failed to open file '" << gt_files[0] << "' for reading" << std::endl;
76       return false;
77     }
78     int image_number_count = 0;
79     while (gt_file)
80     {
81       std::string line;
82       std::getline(gt_file, line);
83       ++image_number_count;
84     }
85     cameras_data_.reserve(image_number_count);
86 
87     gt_file.clear(std::ios::goodbit);
88     gt_file.seekg(std::ios::beg);
89 
90     int frame_index = 0;
91     while (gt_file)
92     {
93       Eigen::Matrix<double, 3, 4, Eigen::RowMajor> RC;
94       for (int i = 0; i < 12; ++i)
95       {
96         gt_file >> *(RC.data() + i);
97       }
98       if (!gt_file)
99         break;
100 
101       const Mat3 R = RC.block<3, 3>(0, 0);
102       const Vec3 C = RC.block<3, 1>(0, 3);
103       Mat34 P;
104       P_From_KRt(calibration_matrix, R, - R * C, &P);
105       cameras_data_.emplace_back(P);
106 
107       // Parse image name
108       std::ostringstream os;
109       os << std::setw(6) << std::setfill('0') << frame_index << ".png";
110       images_.emplace_back( os.str() );
111 
112       frame_index++;
113     }
114     gt_file.close();
115 
116     return true;
117   }
118 
loadImages()119   bool loadImages() override
120   {
121     return LoadImages(this->image_dir_, this->images_, this->cameras_data_, this->sfm_data_);
122   }
123 };
124 
125 
126 #endif // IO_READ_GT_KITTI_HPP
127