| 151 | } |
| 152 | |
| 153 | void stereo_tracking(const std::shared_ptr<PLPSLAM::config> &cfg, |
| 154 | const std::string &vocab_file_path, const unsigned int cam_num, const std::string &mask_img_path, |
| 155 | const float scale, const std::string &map_db_path) |
| 156 | { |
| 157 | const cv::Mat mask = mask_img_path.empty() ? cv::Mat{} : cv::imread(mask_img_path, cv::IMREAD_GRAYSCALE); |
| 158 | // build a SLAM system |
| 159 | PLPSLAM::system SLAM(cfg, vocab_file_path); |
| 160 | // startup the SLAM process |
| 161 | SLAM.startup(); |
| 162 | |
| 163 | // create a viewer object |
| 164 | // and pass the frame_publisher and the map_publisher |
| 165 | #ifdef USE_PANGOLIN_VIEWER |
| 166 | pangolin_viewer::viewer viewer(cfg, &SLAM, SLAM.get_frame_publisher(), SLAM.get_map_publisher()); |
| 167 | #elif USE_SOCKET_PUBLISHER |
| 168 | socket_publisher::publisher publisher(cfg, &SLAM, SLAM.get_frame_publisher(), SLAM.get_map_publisher()); |
| 169 | #endif |
| 170 | |
| 171 | cv::VideoCapture videos[2]; |
| 172 | for (int i = 0; i < 2; i++) |
| 173 | { |
| 174 | videos[i] = cv::VideoCapture(cam_num + i); |
| 175 | if (!videos[i].isOpened()) |
| 176 | { |
| 177 | spdlog::critical("cannot open a camera {}", cam_num + i); |
| 178 | SLAM.shutdown(); |
| 179 | return; |
| 180 | } |
| 181 | } |
| 182 | |
| 183 | const PLPSLAM::util::stereo_rectifier rectifier(cfg); |
| 184 | |
| 185 | cv::Mat frames[2]; |
| 186 | cv::Mat frames_rectified[2]; |
| 187 | double timestamp = 0.0; |
| 188 | std::vector<double> track_times; |
| 189 | unsigned int num_frame = 0; |
| 190 | |
| 191 | bool is_not_end = true; |
| 192 | // run the SLAM in another thread |
| 193 | std::thread thread([&]() |
| 194 | { |
| 195 | while (is_not_end) |
| 196 | { |
| 197 | // check if the termination of SLAM system is requested or not |
| 198 | if (SLAM.terminate_is_requested()) |
| 199 | { |
| 200 | break; |
| 201 | } |
| 202 | |
| 203 | is_not_end = videos[0].read(frames[0]) && videos[1].read(frames[1]); |
| 204 | if (frames[0].empty() || frames[1].empty()) |
| 205 | { |
| 206 | continue; |
| 207 | } |
| 208 | for (int i = 0; i < 2; i++) |
| 209 | { |
| 210 | if (scale != 1.0) |
no test coverage detected