Example: kitti_dataset2rawlog
C++ example source code:
/* +------------------------------------------------------------------------+ | Mobile Robot Programming Toolkit (MRPT) | | https://www.mrpt.org/ | | | | Copyright (c) 2005-2022, Individual contributors, see AUTHORS file | | See: https://www.mrpt.org/Authors - All rights reserved. | | Released under BSD License. See: https://www.mrpt.org/License | +------------------------------------------------------------------------+ */ /* ------------------------------------------------------ kitti_dataset2rawlog A small tool to translate the KITTI datasets and the karlsruhe sequences http://www.cvlibs.net/datasets/karlsruhe_sequences/ into MRPT rawlog binary format, ready to be parsed by RawLogViewer or user programs. Usage: * Karlsruhe seqs: kitti_dataset2rawlog [PATH_TO_DIR_WITH_IMAGES] [CALIB_FILE] [OUTPUT_NAME] * KITTI seqs: kitti_dataset2rawlog [PATH_TO_IMAGE_00] [PATH_TO_IMAGE_01] [CALIB_FILE] [OUTPUT_NAME] Output files: - OUTPUT_NAME.rawlog: The output rawlog file. ------------------------------------------------------ */ #include <mrpt/img/TCamera.h> #include <mrpt/io/CFileGZOutputStream.h> #include <mrpt/io/CTextFileLinesParser.h> #include <mrpt/obs/CObservationStereoImages.h> #include <mrpt/serialization/CArchive.h> #include <mrpt/system/filesystem.h> #include <mrpt/system/string_utils.h> #include <iostream> using namespace std; using namespace mrpt; using namespace mrpt::obs; using namespace mrpt::serialization; const double STEREO_FPS = 10.0; void stereo2rawlog( bool is_kitti_dataset, const string& src_path0, const string& src_path1, const string& calib_file, const string& out_name) { /* Camera params: Left (1): 6.790081e+02 0.000000e+00 6.598034e+02 0.000000e+00 0.000000e+00 6.790081e+02 1.865724e+02 0.000000e+00 0.000000e+00 0.000000e+00 1.000000e+00 0.000000e+00 Right (2): 6.790081e+02 0.000000e+00 6.598034e+02 -3.887481e+02 0.000000e+00 6.790081e+02 1.865724e+02 0.000000e+00 0.000000e+00 0.000000e+00 1.000000e+00 0.000000e+00 base = 3.887481e+02 / 6.790081e+02 = 0.57252351 m */ // Parse calib file: mrpt::math::CMatrixDouble P1_roi, P2_roi; bool p1_ok = false, p2_ok = false; const string name_P0 = is_kitti_dataset ? "P0:" : "P1_roi:"; const string name_P1 = is_kitti_dataset ? "P1:" : "P2_roi:"; std::istringstream ss; mrpt::io::CTextFileLinesParser fp; fp.open(calib_file); while (fp.getNextLine(ss)) { string sStart; ss >> sStart; if (mrpt::system::strStartsI(sStart, name_P0)) { P1_roi.loadFromTextFile(ss); ASSERT_(P1_roi.cols() == 12 && P1_roi.rows() == 1); p1_ok = true; } if (mrpt::system::strStartsI(sStart, name_P1)) { P2_roi.loadFromTextFile(ss); ASSERT_(P2_roi.cols() == 12 && P2_roi.rows() == 1); p2_ok = true; } } if (!p1_ok || !p2_ok) throw std::runtime_error("Couldn't load P*_ROI calib matrices!"); mrpt::img::TCamera cam_params_l; cam_params_l.ncols = 1344; cam_params_l.nrows = 391; cam_params_l.fx(P2_roi(0, 0)); cam_params_l.fy(P2_roi(0, 5)); cam_params_l.cx(P2_roi(0, 2)); cam_params_l.cy(P2_roi(0, 6)); mrpt::img::TCamera cam_params_r = cam_params_l; // base = -P2_roi(1,4)/P2_roi(1,1) const double baseline = -P2_roi(0, 3) / P2_roi(0, 0); const mrpt::poses::CPose3DQuat l2r_pose( baseline, 0.0, 0.0, mrpt::math::CQuaternionDouble()); cout << "baseline: " << baseline << endl; // Create rawlog file ---------------------------------------------- const string out_rawlog_fil = out_name + string(".rawlog"); const string out_imgs_dir = out_name + string("_Images"); cout << "Creating rawlog: " << out_rawlog_fil << endl; mrpt::io::CFileGZOutputStream f_out(out_rawlog_fil); if (is_kitti_dataset) { cout << "Creating imgs dir: " << out_imgs_dir << endl; mrpt::system::createDirectory(out_imgs_dir); } mrpt::system::TTimeStamp tim0 = mrpt::system::now(); // For each image: for (int i = 0;; i++) { string sImgFile_L, sImgFile_R; string sImg_L, sImg_R; string sTrgImgFile_L, sTrgImgFile_R; string sTrgImg_L, sTrgImg_R; // If !="", will copy the file. if (is_kitti_dataset) { // 0000000000.png // 1234567890 sImgFile_L = mrpt::format("%010i.png", i); sImgFile_R = mrpt::format("%010i.png", i); sImg_L = mrpt::format("%s/%s", src_path0.c_str(), sImgFile_L.c_str()); sImg_R = mrpt::format("%s/%s", src_path1.c_str(), sImgFile_R.c_str()); sTrgImgFile_L = mrpt::format("I0_%s", sImgFile_L.c_str()); sTrgImgFile_R = mrpt::format("I1_%s", sImgFile_R.c_str()); sTrgImg_L = mrpt::format( "%s/%s", out_imgs_dir.c_str(), sTrgImgFile_L.c_str()); sTrgImg_R = mrpt::format( "%s/%s", out_imgs_dir.c_str(), sTrgImgFile_R.c_str()); } else { // I1_000000.png // I2_002570.png sImgFile_L = mrpt::format("I1_%06i.png", i); sImgFile_R = mrpt::format("I2_%06i.png", i); sTrgImgFile_L = sImgFile_L; sTrgImgFile_R = sImgFile_R; sImg_L = mrpt::format("%s/%s", src_path0.c_str(), sImgFile_L.c_str()); sImg_R = mrpt::format("%s/%s", src_path0.c_str(), sImgFile_R.c_str()); } if (!mrpt::system::fileExists(sImg_L) || !mrpt::system::fileExists(sImg_R)) { cout << "Couldn't detect image pair " << sImg_L << " | " << sImg_R << " -> ending.\n"; break; } CObservationStereoImages obs; obs.timestamp = mrpt::system::time_tToTimestamp( mrpt::system::timestampTotime_t(tim0) + i / STEREO_FPS); obs.cameraPose = mrpt::poses::CPose3DQuat(mrpt::poses::CPose3D( 0, 0, 0, -90.0_deg, 0.0_deg, DEG2RAD(-90.0 - 4.6))); obs.leftCamera = cam_params_l; obs.rightCamera = cam_params_r; obs.rightCameraPose = l2r_pose; obs.imageLeft.setExternalStorage(sTrgImgFile_L); obs.imageRight.setExternalStorage(sTrgImgFile_R); obs.sensorLabel = "CAMERA1"; // save: archiveFrom(f_out) << obs; // Copy img: if (is_kitti_dataset) { if (!mrpt::system::copyFile(sImg_L, sTrgImg_L)) { cerr << "Error copying file: " << sImg_L << " => " << sTrgImg_L << endl; return; } if (!mrpt::system::copyFile(sImg_R, sTrgImg_R)) { cerr << "Error copying file: " << sImg_R << " => " << sTrgImg_R << endl; return; } } // cout << "Writing entry #" << i << endl; } cout << "\nAll done!\n"; } // ------------------------------------------------------ // MAIN // ------------------------------------------------------ int main(int argc, char** argv) { try { if (argc != 4 && argc != 5) { cerr << "Usage:\n" "Karlsruhe seqs:\n" " kitti_dataset2rawlog [PATH_TO_DIR_WITH_IMAGES] " "[CALIB_FILE] [OUTPUT_NAME]\n" "KITTI seqs:\n" " kitti_dataset2rawlog [PATH_TO_IMAGE_00] " "[PATH_TO_IMAGE_01] [CALIB_FILE] [OUTPUT_NAME]\n"; return 1; } if (argc == 4) { // "Karlsruhe seqs": stereo2rawlog(false, argv[1], "", argv[2], argv[3]); } else { // "KITTI": stereo2rawlog(true, argv[1], argv[2], argv[3], argv[4]); } return 0; } catch (const std::exception& e) { std::cerr << "MRPT error: " << mrpt::exception_to_str(e) << std::endl; return -1; } catch (...) { printf("Untyped exception!!"); return -1; } }