Skip to content

Commit 1095118

Browse files
joanpepcompanyjoanpepcompany
committed
Added: extract images from realsense and from rosbag 2 TUM format
1 parent fdd62ae commit 1095118

File tree

4 files changed

+559
-0
lines changed

4 files changed

+559
-0
lines changed

‎bag_tools/CMakeLists.txt

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -14,15 +14,22 @@ catkin_package(
1414
include_directories(include ${Boost_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS} ${console_bridge_INCLUDE_DIRS})
1515

1616
add_executable(extract_images src/extract_images.cpp)
17+
add_executable(extract_images_from_realsense src/extract_images_from_realsense.cpp)
18+
19+
add_executable(extract_tum_format src/extract_tum_format.cpp)
1720
add_executable(extract_stereo_images src/extract_stereo_images.cpp)
1821
add_executable(process_stereo src/process_stereo.cpp)
1922

2023
target_link_libraries(extract_images ${Boost_LIBRARIES} ${OpenCV_LIBRARIES} ${catkin_LIBRARIES} ${console_bridge_LIBRARIES})
24+
target_link_libraries(extract_images_from_realsense ${Boost_LIBRARIES} ${OpenCV_LIBRARIES} ${catkin_LIBRARIES} ${console_bridge_LIBRARIES})
2125
target_link_libraries(extract_stereo_images ${Boost_LIBRARIES} ${OpenCV_LIBRARIES} ${catkin_LIBRARIES} ${console_bridge_LIBRARIES})
26+
target_link_libraries(extract_tum_format ${Boost_LIBRARIES} ${OpenCV_LIBRARIES} ${catkin_LIBRARIES} ${console_bridge_LIBRARIES})
2227
target_link_libraries(process_stereo ${Boost_LIBRARIES} ${OpenCV_LIBRARIES} ${catkin_LIBRARIES} ${console_bridge_LIBRARIES})
2328

2429
install(TARGETS
2530
extract_images
31+
extract_images_from_realsense
32+
extract_tum_format
2633
extract_stereo_images
2734
process_stereo
2835
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}

‎bag_tools/include/bag_tools/tum_bag_processor.h

Lines changed: 154 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,154 @@
1+
/// Copyright (c) 2012,
2+
/// Systems, Robotics and Vision Group
3+
/// University of the Balearican Islands
4+
/// All rights reserved.
5+
///
6+
/// Redistribution and use in source and binary forms, with or without
7+
/// modification, are permitted provided that the following conditions are met:
8+
/// * Redistributions of source code must retain the above copyright
9+
/// notice, this list of conditions and the following disclaimer.
10+
/// * Redistributions in binary form must reproduce the above copyright
11+
/// notice, this list of conditions and the following disclaimer in the
12+
/// documentation and/or other materials provided with the distribution.
13+
/// * Neither the name of Systems, Robotics and Vision Group, University of
14+
/// the Balearican Islands nor the names of its contributors may be used
15+
/// to endorse or promote products derived from this software without
16+
/// specific prior written permission.
17+
///
18+
/// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
19+
/// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
20+
/// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
21+
/// ARE DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
22+
/// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
23+
/// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
24+
/// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
25+
/// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
26+
/// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
27+
/// THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
28+
29+
#include <ros/ros.h>
30+
#include <rosbag/bag.h>
31+
#include <rosbag/view.h>
32+
33+
#include <boost/foreach.hpp>
34+
#include <boost/progress.hpp>
35+
36+
#include <message_filters/subscriber.h>
37+
#include <message_filters/time_synchronizer.h>
38+
39+
#include <sensor_msgs/Image.h>
40+
#include <sensor_msgs/CameraInfo.h>
41+
42+
43+
namespace bag_tools
44+
{
45+
46+
/**
47+
* Inherits from message_filters::SimpleFilter<M>
48+
* to use protected signalMessage function
49+
*/
50+
template <class M>
51+
class BagSubscriber : public message_filters::SimpleFilter<M>
52+
{
53+
public:
54+
void newMessage(const boost::shared_ptr<M const> &msg)
55+
{
56+
this->signalMessage(msg);
57+
}
58+
};
59+
60+
class TUMBagProcessor
61+
{
62+
63+
public:
64+
TUMBagProcessor(const std::string& tum_base_topic) :
65+
tum_base_topic_(tum_base_topic),
66+
sync_(depth_img_sub_, color_img_sub_, _info_sub_, color_info_sub_, 25)
67+
{
68+
ros::Time::init();
69+
}
70+
71+
template<class C>
72+
void registerCallback(const C& callback)
73+
{
74+
sync_.registerCallback(callback);
75+
}
76+
77+
/**
78+
* Processes given bagfile, calls registered callback function when
79+
* a synchronized color + depth with camera infos is found.
80+
*/
81+
void processBag(const std::string &filename)
82+
{
83+
84+
// Image topics to load
85+
std::string depth_cam_image = tum_base_topic_+ "/depth/image_rect_raw";
86+
std::string color_cam_image = tum_base_topic_ + "/color/image_rect_color";
87+
std::string depth_cam_info = tum_base_topic_ + "/depth/camera_info";
88+
std::string color_cam_info = tum_base_topic_ + "/color/camera_info";
89+
90+
std::vector<std::string> topics;
91+
topics.push_back(depth_cam_image);
92+
topics.push_back(color_cam_image);
93+
topics.push_back(depth_cam_info);
94+
topics.push_back(color_cam_info);
95+
96+
std::cout << "Starting processing " << filename << ", ";
97+
rosbag::Bag bag;
98+
bag.open(filename, rosbag::bagmode::Read);
99+
rosbag::View view(bag, rosbag::TopicQuery(topics));
100+
101+
int num_messages = view.size();
102+
std::cout << num_messages << " messages to process." << std::endl;
103+
104+
// Load all messages
105+
boost::progress_display show_progress(num_messages);
106+
BOOST_FOREACH(rosbag::MessageInstance const m, view)
107+
{
108+
if (m.getTopic() == depth_cam_image || ("/" + m.getTopic() == depth_cam_image))
109+
{
110+
sensor_msgs::Image::ConstPtr depth_img = m.instantiate<sensor_msgs::Image>();
111+
if (depth_img != NULL)
112+
depth_img_sub_.newMessage(depth_img);
113+
}
114+
115+
if (m.getTopic() == color_cam_image || ("/" + m.getTopic() == color_cam_image))
116+
{
117+
sensor_msgs::Image::ConstPtr color_img = m.instantiate<sensor_msgs::Image>();
118+
if (color_img != NULL)
119+
color_img_sub_.newMessage(color_img);
120+
}
121+
122+
if (m.getTopic() == depth_cam_info || ("/" + m.getTopic() == depth_cam_info))
123+
{
124+
sensor_msgs::CameraInfo::ConstPtr depth_info = m.instantiate<sensor_msgs::CameraInfo>();
125+
if (depth_info != NULL)
126+
_info_sub_.newMessage(depth_info);
127+
}
128+
129+
if (m.getTopic() == color_cam_info || ("/" + m.getTopic() == color_cam_info))
130+
{
131+
sensor_msgs::CameraInfo::ConstPtr color_info = m.instantiate<sensor_msgs::CameraInfo>();
132+
if (color_info != NULL)
133+
color_info_sub_.newMessage(color_info);
134+
}
135+
++show_progress;
136+
}
137+
bag.close();
138+
std::cout << "Finished processing " << filename << std::endl;
139+
}
140+
141+
private:
142+
143+
// Fake subscribers to capture images
144+
BagSubscriber<sensor_msgs::Image> depth_img_sub_, color_img_sub_;
145+
BagSubscriber<sensor_msgs::CameraInfo> _info_sub_, color_info_sub_;
146+
147+
std::string tum_base_topic_;
148+
149+
message_filters::TimeSynchronizer<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo> sync_;
150+
151+
};
152+
153+
} // end of namespace
154+

‎bag_tools/src/extract_images_from_realsense.cpp

Lines changed: 167 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,167 @@
1+
/// Copyright (c) 2012,
2+
/// Systems, Robotics and Vision Group
3+
/// University of the Balearican Islands
4+
/// All rights reserved.
5+
///
6+
/// Redistribution and use in source and binary forms, with or without
7+
/// modification, are permitted provided that the following conditions are met:
8+
/// * Redistributions of source code must retain the above copyright
9+
/// notice, this list of conditions and the following disclaimer.
10+
/// * Redistributions in binary form must reproduce the above copyright
11+
/// notice, this list of conditions and the following disclaimer in the
12+
/// documentation and/or other materials provided with the distribution.
13+
/// * Neither the name of Systems, Robotics and Vision Group, University of
14+
/// the Balearican Islands nor the names of its contributors may be used
15+
/// to endorse or promote products derived from this software without
16+
/// specific prior written permission.
17+
///
18+
/// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
19+
/// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
20+
/// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
21+
/// ARE DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
22+
/// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
23+
/// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
24+
/// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
25+
/// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
26+
/// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
27+
/// THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
28+
29+
#include <ros/ros.h>
30+
31+
#include <boost/format.hpp>
32+
33+
#include <sensor_msgs/Image.h>
34+
#include <sensor_msgs/CompressedImage.h>
35+
#include <sensor_msgs/CameraInfo.h>
36+
37+
#include <image_proc/processor.h>
38+
#include <camera_calibration_parsers/parse.h>
39+
40+
#include <opencv2/highgui/highgui.hpp> // for cv::imwrite
41+
42+
#include <cv_bridge/cv_bridge.h>
43+
44+
#include "bag_tools/image_bag_processor.h"
45+
#include <iostream>
46+
/**
47+
* Saves color images from raw input
48+
*/
49+
class ImageSaver
50+
{
51+
public:
52+
ImageSaver(const std::string& save_dir, const std::string& filetype,
53+
const std::string& prefix) :
54+
save_dir_(save_dir), filetype_(filetype), prefix_(prefix), num_saved_(0)
55+
{}
56+
57+
void save(const sensor_msgs::ImageConstPtr &img)
58+
{
59+
60+
cv_bridge::CvImagePtr cv_ptr;
61+
62+
if (img->encoding == "16UC1")
63+
{
64+
try
65+
{
66+
cv_ptr = cv_bridge::toCvCopy(img, sensor_msgs::image_encodings::TYPE_16UC1);
67+
}
68+
catch (cv_bridge::Exception& e)
69+
{
70+
ROS_ERROR("cv_bridge exception: %s", e.what());
71+
return;
72+
}
73+
cv_ptr->image.convertTo(cv_ptr->image, CV_8U, 255.0 / 4096.0);
74+
save_image(img->header.stamp.toNSec(), cv_ptr->image);
75+
}
76+
77+
else if (img->encoding == "rgb8")
78+
{
79+
try
80+
{
81+
cv_ptr = cv_bridge::toCvCopy(img, sensor_msgs::image_encodings::TYPE_8UC3);
82+
}
83+
catch (cv_bridge::Exception& e)
84+
{
85+
ROS_ERROR("cv_bridge exception: %s", e.what());
86+
return;
87+
}
88+
cv::cvtColor( cv_ptr->image, cv_ptr->image, cv::COLOR_BGR2RGB );
89+
save_image(img->header.stamp.toNSec(), cv_ptr->image);
90+
}
91+
92+
else
93+
{
94+
image_proc::ImageSet output;
95+
image_geometry::PinholeCameraModel camera_model; // empty, we don't need it here
96+
if (!processor_.process(img, camera_model, output, image_proc::Processor::COLOR))
97+
{
98+
std::cerr << "ERROR Processing image" << std::endl;
99+
return;
100+
}
101+
save_image(img->header.stamp.toNSec(), output.color);
102+
103+
}
104+
105+
}
106+
void save_compressed(const sensor_msgs::CompressedImageConstPtr& _compressed)
107+
{
108+
cv::Mat image_uncomrpessed = cv::imdecode(cv::Mat(_compressed->data),1);
109+
save_image(_compressed->header.stamp.toNSec(), image_uncomrpessed);
110+
}
111+
112+
private:
113+
void save_image(uint64_t _time_stamp, const cv::Mat& _image)
114+
{
115+
std::string filename =
116+
boost::str(boost::format("%s/%lu.%s")
117+
% save_dir_
118+
// % prefix_
119+
// % num_saved_
120+
% _time_stamp
121+
% filetype_);
122+
if (!cv::imwrite(filename, _image))
123+
{
124+
ROS_ERROR_STREAM("ERROR Saving " << filename);
125+
}
126+
else
127+
{
128+
ROS_DEBUG_STREAM("Saved " << filename);
129+
num_saved_++;
130+
}
131+
}
132+
133+
image_proc::Processor processor_;
134+
std::string save_dir_;
135+
std::string filetype_;
136+
std::string prefix_;
137+
int num_saved_;
138+
};
139+
140+
int main(int argc, char** argv)
141+
{
142+
if (argc < 4)
143+
{
144+
std::cout << "Usage: " << argv[0] << " OUT_DIR FILETYPE IMAGE_TOPIC BAGFILE [BAGFILE...]" << std::endl;
145+
std::cout << " Example: " << argv[0] << " /tmp jpg /stereo_down/left/image_raw bag1.bag bag2.bag" << std::endl;
146+
return 0;
147+
}
148+
149+
std::string out_dir(argv[1]);
150+
std::string filetype(argv[2]);
151+
std::string image_topic(argv[3]);
152+
153+
ros::Time::init();
154+
155+
std::string prefix("image");
156+
157+
ImageSaver saver(out_dir, filetype, prefix);
158+
bag_tools::ImageBagProcessor processor(image_topic);
159+
processor.registerCallback(boost::bind(&ImageSaver::save, saver, _1));
160+
processor.registerCompressedCallback(boost::bind(&ImageSaver::save_compressed, saver, _1));
161+
162+
for (int i = 4; i < argc; ++i)
163+
processor.processBag(argv[i]);
164+
165+
return 0;
166+
}
167+

0 commit comments

Comments
 (0)