diff --git a/README.md b/README.md index a104f62..967c882 100644 --- a/README.md +++ b/README.md @@ -21,8 +21,11 @@ The tool is a normal ROS package. Place it under a workspace and build it with c ## Usage **Command-line arguments:** -`--dataroot`: The path to the directory that contains the 'maps', 'samples' and 'sweeps'. +`--dataroot,-s`: The path to the directory that contains the 'maps', 'samples' and 'sweeps'. `--version`: (optional) The sub-directory that contains the metadata .json files. Default = "v1.0-mini" +`--scene_number,-n`: (optional) Only convert a given scene +`--compress,-c`: (optional) whether to use compressed images to reduce file size + **Converting the 'mini' dataset:** @@ -40,6 +43,10 @@ This processes 4 scenes simultaneously, however the scene numbers are not proces rosrun nuscenes2bag nuscenes2bag --dataroot /path/to/nuscenes_mini_meta_v1.0/ --out nuscenes_bags/ --jobs 4 ``` +Convert one scene to a bag file with compressed images: +``` +rosrun nuscenes2bag nuscenes2bag -c --scene_number 0061 --dataroot /path/to/nuscenes_mini_meta_v1.0/ --out nuscenes_bags/ +``` **Converting other datasets:** @@ -72,4 +79,4 @@ Built using: ## Authors - [clynamen](https://github.com/clynamen/) - - [ChernoA](https://github.com/ChernoA) \ No newline at end of file + - [ChernoA](https://github.com/ChernoA) diff --git a/include/nuscenes2bag/ImageDirectoryConverter.hpp b/include/nuscenes2bag/ImageDirectoryConverter.hpp index f420fc4..e4b421f 100644 --- a/include/nuscenes2bag/ImageDirectoryConverter.hpp +++ b/include/nuscenes2bag/ImageDirectoryConverter.hpp @@ -14,4 +14,6 @@ namespace nuscenes2bag { boost::optional readImageFile(const fs::path& filePath) noexcept; +boost::optional readImageFileCompressed(const fs::path& filePath) noexcept; + } diff --git a/include/nuscenes2bag/NuScenes2Bag.hpp b/include/nuscenes2bag/NuScenes2Bag.hpp index 21d8eed..25e0301 100644 --- a/include/nuscenes2bag/NuScenes2Bag.hpp +++ b/include/nuscenes2bag/NuScenes2Bag.hpp @@ -19,7 +19,8 @@ struct NuScenes2Bag { const std::string& version, const fs::path &outputRosbagPath, int32_t threadNumber, - boost::optional sceneNumberOpt + boost::optional sceneNumberOpt, + bool compressImgs ); private: diff --git a/include/nuscenes2bag/SceneConverter.hpp b/include/nuscenes2bag/SceneConverter.hpp index 38d505c..651bdde 100644 --- a/include/nuscenes2bag/SceneConverter.hpp +++ b/include/nuscenes2bag/SceneConverter.hpp @@ -9,7 +9,7 @@ namespace nuscenes2bag { class SceneConverter { public: - SceneConverter(const MetaDataProvider& metaDataProvider); + SceneConverter(const MetaDataProvider& metaDataProvider, bool compressImgs); void submit(const Token& sceneToken, FileProgress& fileProgress); @@ -25,6 +25,7 @@ class SceneConverter { std::vector egoPoseInfos; SceneId sceneId; Token sceneToken; + bool compressImgs; }; } \ No newline at end of file diff --git a/src/ImageDirectoryConverter.cpp b/src/ImageDirectoryConverter.cpp index 24d5a62..92f0db1 100644 --- a/src/ImageDirectoryConverter.cpp +++ b/src/ImageDirectoryConverter.cpp @@ -22,4 +22,22 @@ readImageFile(const fs::path& filePath) noexcept return boost::none; } +boost::optional +readImageFileCompressed(const fs::path& filePath) noexcept +{ + cv::Mat image; + try { + image = imread(filePath.string().c_str(), cv::IMREAD_COLOR); + sensor_msgs::CompressedImagePtr msg = + cv_bridge::CvImage(std_msgs::Header(), "bgr8", image).toCompressedImageMsg(); + + return boost::optional(*msg); + + } catch (const std::exception& e) { + PRINT_EXCEPTION(e); + } + + return boost::none; +} + } \ No newline at end of file diff --git a/src/NuScenes2Bag.cpp b/src/NuScenes2Bag.cpp index 413dd75..95cf1b0 100644 --- a/src/NuScenes2Bag.cpp +++ b/src/NuScenes2Bag.cpp @@ -35,7 +35,8 @@ NuScenes2Bag::convertDirectory(const fs::path& inDatasetPath, const std::string& version, const fs::path& outputRosbagPath, int threadNumber, - boost::optional sceneNumberOpt) + boost::optional sceneNumberOpt, + bool compressImgs) { if ((threadNumber < 1) || (threadNumber > 64)) { std::cout << "Forcing at least one job number (-j1)" << std::endl; @@ -91,7 +92,7 @@ NuScenes2Bag::convertDirectory(const fs::path& inDatasetPath, for (const auto& sceneToken : chosenSceneTokens) { std::unique_ptr sceneConverter = - std::make_unique(metaDataReader); + std::make_unique(metaDataReader, compressImgs); sceneConverter->submit(sceneToken, fileProgress); SceneConverter* sceneConverterPtr = sceneConverter.get(); sceneConverters.push_back(std::move(sceneConverter)); @@ -136,7 +137,7 @@ NuScenes2Bag::convertDirectory(const fs::path& inDatasetPath, for (const auto& sceneToken : chosenSceneTokens) { boost::shared_ptr sceneConverter = - boost::make_shared(SceneConverter(metaDataReader)); + boost::make_shared(SceneConverter(metaDataReader, compressImgs)); sceneConverter->submit(sceneToken, fileProgress); sceneConverters.push_back(std::move(sceneConverter)); diff --git a/src/SceneConverter.cpp b/src/SceneConverter.cpp index 1b06a44..514d064 100644 --- a/src/SceneConverter.cpp +++ b/src/SceneConverter.cpp @@ -17,8 +17,8 @@ using namespace std; namespace nuscenes2bag { -SceneConverter::SceneConverter(const MetaDataProvider& metaDataProvider) - : metaDataProvider(metaDataProvider) +SceneConverter::SceneConverter(const MetaDataProvider& metaDataProvider, bool compressImgs) + : metaDataProvider(metaDataProvider), compressImgs(compressImgs) {} boost::optional @@ -121,9 +121,16 @@ SceneConverter::convertSampleDatas(rosbag::Bag& outBag, std::string sensorName = toLower(calibratedSensorName.name); if (sampleType == SampleType::CAMERA) { - auto topicName = sensorName + "/raw"; - auto msg = readImageFile(sampleFilePath); - writeMsg(topicName, sensorName, sampleData.timeStamp, outBag, msg); + if (compressImgs) { + auto topicName = sensorName + "/compressed"; + auto msg = readImageFileCompressed(sampleFilePath); + writeMsg(topicName, sensorName, sampleData.timeStamp, outBag, msg); + } + else { + auto topicName = sensorName + "/raw"; + auto msg = readImageFile(sampleFilePath); + writeMsg(topicName, sensorName, sampleData.timeStamp, outBag, msg); + } } else if (sampleType == SampleType::LIDAR) { auto topicName = sensorName; diff --git a/src/main.cpp b/src/main.cpp index ff0755f..ca0e765 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -15,6 +15,7 @@ main(const int argc, const char* argv[]) std::string outputBagName; int32_t threadNumber = -1; int32_t sceneNumber = -1; + bool compressImgs = false; options_description desc{ "Options" }; desc.add_options()("help,h", "show help"); @@ -32,7 +33,10 @@ main(const int argc, const char* argv[]) "out,o", value(&outputBagName), "output bag name")( "jobs,j", value(&threadNumber), - "number of jobs (thread number)"); + "number of jobs (thread number)")( + "compress,c", + bool_switch(&compressImgs), + "whether to use compressed images to reduce file size"); variables_map vm; desc.add(inputDesc); @@ -52,7 +56,7 @@ main(const int argc, const char* argv[]) sceneNumberOpt = sceneNumber; } converter.convertDirectory( - sampleDirPath, version, outputBagName, threadNumber, sceneNumberOpt); + sampleDirPath, version, outputBagName, threadNumber, sceneNumberOpt, compressImgs); } } catch (const error& ex) { std::cerr << ex.what() << '\n';