├── LICENSE ├── README.md ├── dvs_crop_bag ├── crop_msg │ ├── CMakeLists.txt │ ├── README.md │ ├── dependencies.yaml │ ├── package.xml │ ├── process_folder.py │ └── src │ │ └── crop_msg.cpp └── crop_time │ ├── CMakeLists.txt │ ├── README.md │ ├── dependencies.yaml │ ├── package.xml │ ├── process_folder.py │ └── src │ └── crop_time.cpp ├── dvs_frame_filter ├── CMakeLists.txt ├── README.md ├── dependencies.yaml ├── package.xml ├── process_folder.py └── src │ └── frame_filter.cpp ├── dvs_hot_pixel_filter ├── CMakeLists.txt ├── README.md ├── dependencies.yaml ├── images │ └── hot_pixel_map.png ├── include │ └── dvs_hot_pixel_filter │ │ └── utils.h ├── package.xml └── src │ ├── hot_pixel_filter.cpp │ └── utils.cpp ├── dvs_reverse_events ├── CMakeLists.txt ├── README.md ├── dependencies.yaml ├── include │ └── dvs_reverse_events │ │ └── utils.h ├── package.xml └── src │ ├── reverse_events.cpp │ └── utils.cpp ├── dvs_rosbag_stats ├── CMakeLists.txt ├── README.md ├── dependencies.yaml ├── package.xml ├── process_folder.py └── src │ └── rosbag_stats.cpp ├── dvs_sort_events ├── CMakeLists.txt ├── README.md ├── dependencies.yaml ├── include │ └── dvs_sort_events │ │ └── utils.h ├── package.xml ├── process_folder.py └── src │ ├── sort_events.cpp │ └── utils.cpp └── script_templates └── rosbag_filter_topic.sh /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2019 Cedric 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # DVS Tools 2 | 3 | A collection of useful tools to deal with rosbags and data from event cameras. 4 | -------------------------------------------------------------------------------- /dvs_crop_bag/crop_msg/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(dvs_crop_msg) 3 | 4 | find_package(catkin_simple REQUIRED) 5 | 6 | catkin_simple() 7 | 8 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -O3") 9 | 10 | # make the executable 11 | cs_add_executable(dvs_crop_msg 12 | src/crop_msg.cpp 13 | ) 14 | 15 | # link the executable to the necesarry libs 16 | target_link_libraries(dvs_crop_msg 17 | ${catkin_LIBRARIES} 18 | ) 19 | 20 | cs_install() 21 | 22 | -------------------------------------------------------------------------------- /dvs_crop_bag/crop_msg/README.md: -------------------------------------------------------------------------------- 1 | # DVS crop bag 2 | 3 | Crops bag by number of messages. 4 | 5 | ### Usage: 6 | #### Single .bag file: 7 | 8 | rosrun dvs_crop_msg dvs_crop_msg path_to_input.bag 9 | 10 | num_skip is an optional parameters that specifies the number of messages to skip at the beginning. 11 | 12 | #### Process folder (used to process an entire folder of .bag files): 13 | 14 | python process_folder.py --folder --num_msgs 15 | 16 | ## Installation 17 | 18 | Assumes you have already set up your workspace (see [here](https://github.com/cedric-scheerlinck/dvs_image_reconstruction) for more details). 19 | 20 | Build 21 | 22 | catkin build dvs_crop_msg 23 | source ~/catkin_ws/devel/setup.bash 24 | 25 | Saves output to ```path_to_input.bag.filtered```. 26 | 27 | -------------------------------------------------------------------------------- /dvs_crop_bag/crop_msg/dependencies.yaml: -------------------------------------------------------------------------------- 1 | repositories: 2 | catkin_simple: 3 | type: git 4 | url: git@github.com:catkin/catkin_simple.git 5 | version: master 6 | gflags_catkin: 7 | type: git 8 | url: git@github.com:ethz-asl/gflags_catkin.git 9 | version: master 10 | -------------------------------------------------------------------------------- /dvs_crop_bag/crop_msg/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | dvs_crop_msg 4 | 0.0.0 5 | Crop a rosbag by message 6 | Cedric Scheerlinck 7 | GNU GPL 8 | 9 | catkin 10 | catkin_simple 11 | 12 | gflags_catkin 13 | roscpp 14 | rosbag 15 | dvs_msgs 16 | 17 | 18 | -------------------------------------------------------------------------------- /dvs_crop_bag/crop_msg/process_folder.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | 3 | # !/usr/bin/python 4 | """ 5 | 6 | """ 7 | 8 | from os.path import join 9 | import os 10 | import subprocess 11 | from glob import glob 12 | import random 13 | import yaml 14 | import argparse 15 | import numpy as np 16 | 17 | 18 | def build_run_command(path_to_input_bag, num_msgs, num_skip): 19 | # "rosrun dvs_crop_msg dvs_crop_msg path_to_input num_msgs" 20 | rosrun_cmd = ['rosrun', 21 | 'dvs_crop_msg', 22 | 'dvs_crop_msg', 23 | path_to_input_bag, 24 | str(num_msgs), 25 | str(num_skip)] 26 | 27 | return rosrun_cmd 28 | 29 | 30 | if __name__ == "__main__": 31 | 32 | # Parse command-line arguments 33 | parser = argparse.ArgumentParser() 34 | parser.add_argument("--folder", 35 | required=True, 36 | type=str, 37 | help="directory of rosbags") 38 | parser.add_argument("--num_msgs", 39 | required=True, 40 | type=int, 41 | help="number of messages to write") 42 | parser.add_argument("--num_skip", 43 | required=False, 44 | type=int, 45 | default=0, 46 | help="number of messages to skip") 47 | args = parser.parse_args() 48 | 49 | for file in os.listdir(args.folder): 50 | if file.endswith(".bag"): 51 | # print(file) 52 | # append the arguments to the rosrun command 53 | rosrun_cmd = build_run_command(os.path.join(args.folder, file), args.num_msgs, args.num_skip) 54 | # print(rosrun_cmd) 55 | print(subprocess.check_output(rosrun_cmd)) 56 | 57 | # now summarise output txt files 58 | 59 | # num_events_list = list() 60 | # num_frames_list = list() 61 | # duration_list = list() 62 | # stats_dir = os.path.join(args.folder, "stats") 63 | # for file in os.listdir(stats_dir): 64 | # if file.endswith(".txt"): 65 | # num_events, num_frames, duration = np.loadtxt(os.path.join(stats_dir, file), delimiter=',') 66 | # num_events_list.append(num_events) 67 | # num_frames_list.append(num_frames) 68 | # duration_list.append(duration) 69 | # 70 | # print(np.sum(num_events_list), np.sum(num_frames_list), np.sum(duration_list)) 71 | 72 | -------------------------------------------------------------------------------- /dvs_crop_bag/crop_msg/src/crop_msg.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | #define foreach BOOST_FOREACH 17 | 18 | bool parse_arguments(int argc, char* argv[], 19 | std::string* path_to_input_rosbag, 20 | int& num_msgs, 21 | int& num_skip) 22 | // double& duration) 23 | { 24 | // std::cout << argc << std::endl; 25 | if(argc < 3) 26 | { 27 | std::cerr << "Not enough arguments" << std::endl; 28 | std::cerr << "Usage: rosrun dvs_crop_bag dvs_crop_bag path_to_bag.bag num_msgs"; 29 | return false; 30 | } 31 | 32 | *path_to_input_rosbag = std::string(argv[1]); 33 | 34 | // duration = std::stod(argv[2]); 35 | 36 | num_msgs = std::stoi(argv[2]); 37 | 38 | if (argc == 4) 39 | { 40 | num_skip = std::stoi(argv[3]); 41 | } 42 | 43 | return true; 44 | } 45 | 46 | int main(int argc, char* argv[]) 47 | { 48 | std::string path_to_input_rosbag; 49 | int num_msgs; 50 | int num_skip = 0; 51 | int msg_count = 0; 52 | int skip_count = 0; 53 | 54 | if (!parse_arguments(argc, 55 | argv, 56 | &path_to_input_rosbag, 57 | num_msgs, 58 | num_skip)) 59 | { 60 | return -1; 61 | } 62 | 63 | rosbag::Bag input_bag; 64 | try 65 | { 66 | input_bag.open(path_to_input_rosbag, rosbag::bagmode::Read); 67 | } 68 | catch(rosbag::BagIOException e) 69 | { 70 | std::cerr << "Error: could not open rosbag: " << path_to_input_rosbag << std::endl; 71 | return -1; 72 | } 73 | 74 | rosbag::Bag output_bag; 75 | std::string path_to_output_rosbag = path_to_input_rosbag + ".filtered"; 76 | output_bag.open(path_to_output_rosbag, rosbag::bagmode::Write); 77 | 78 | rosbag::View view(input_bag); 79 | foreach(rosbag::MessageInstance const m, view) 80 | { 81 | if (skip_count++ < num_skip) 82 | { 83 | continue; 84 | } 85 | 86 | if (msg_count++ >= num_msgs) 87 | { 88 | break; 89 | } 90 | 91 | if(m.getDataType() == "sensor_msgs/Image") 92 | { 93 | sensor_msgs::ImageConstPtr img_msg = m.instantiate(); 94 | output_bag.write(m.getTopic(), img_msg->header.stamp, m); 95 | } 96 | else if(m.getDataType() == "dvs_msgs/EventArray") 97 | { 98 | dvs_msgs::EventArrayConstPtr s = m.instantiate(); 99 | ros::Time timestamp = s->header.stamp; 100 | if (timestamp < ros::TIME_MIN) 101 | { 102 | timestamp = s->events.back().ts; 103 | } 104 | output_bag.write(m.getTopic(), timestamp, m); 105 | } 106 | else if(m.getDataType() == "sensor_msgs/Imu") 107 | { 108 | sensor_msgs::ImuConstPtr imu_msg = m.instantiate(); 109 | output_bag.write(m.getTopic(), imu_msg->header.stamp, m); 110 | } 111 | else 112 | { 113 | output_bag.write(m.getTopic(), m.getTime(), m); 114 | } 115 | } 116 | 117 | std::cout << "Wrote " << msg_count - 1 << " messages." << std::endl; 118 | 119 | output_bag.close(); 120 | input_bag.close(); 121 | 122 | return 0; 123 | } 124 | -------------------------------------------------------------------------------- /dvs_crop_bag/crop_time/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(dvs_crop_time) 3 | 4 | find_package(catkin_simple REQUIRED) 5 | 6 | catkin_simple() 7 | 8 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -O3") 9 | 10 | # make the executable 11 | cs_add_executable(dvs_crop_time 12 | src/crop_time.cpp 13 | ) 14 | 15 | # link the executable to the necesarry libs 16 | target_link_libraries(dvs_crop_time 17 | ${catkin_LIBRARIES} 18 | ) 19 | 20 | cs_install() 21 | 22 | -------------------------------------------------------------------------------- /dvs_crop_bag/crop_time/README.md: -------------------------------------------------------------------------------- 1 | # DVS crop time 2 | 3 | Crops bag by timestamp. 4 | 5 | ### Usage: 6 | #### Single .bag file: 7 | 8 | rosrun dvs_crop_time dvs_crop_time path_to_input.bag --t1s= --t1e= --t2s= --t2e= 9 | 10 | t1s: timestamp 1 start 11 | t1e: timestamp 1 end 12 | t2s: timestamp 2 start 13 | t2e: timestamp 2 end 14 | 15 | specify the start and end boundaries for each crop 'segment'. The time intervals not included within a start and end segment will be removed, and the timestamps shifted to make the bag 'continuous'. 16 | 17 | 18 | #### Process folder (used to process a folder of .bag files): 19 | 20 | python process_folder.py --folder --config_path 21 | 22 | config.txt is a text file with one line per bag that you want to process: 23 | 24 | filename.bag t1s t1e t2s t2e 25 | 26 | ## Installation 27 | 28 | Assumes you have already set up your workspace (see [here](https://github.com/cedric-scheerlinck/dvs_image_reconstruction) for more details). 29 | 30 | Build 31 | 32 | catkin build dvs_crop_time 33 | source ~/catkin_ws/devel/setup.bash 34 | 35 | Saves output to ```path_to_input.bag.filtered```. 36 | 37 | -------------------------------------------------------------------------------- /dvs_crop_bag/crop_time/dependencies.yaml: -------------------------------------------------------------------------------- 1 | repositories: 2 | catkin_simple: 3 | type: git 4 | url: git@github.com:catkin/catkin_simple.git 5 | version: master 6 | gflags_catkin: 7 | type: git 8 | url: git@github.com:ethz-asl/gflags_catkin.git 9 | version: master 10 | -------------------------------------------------------------------------------- /dvs_crop_bag/crop_time/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | dvs_crop_time 4 | 0.0.0 5 | Crop a rosbag by timestamp 6 | Cedric Scheerlinck 7 | GNU GPL 8 | 9 | catkin 10 | catkin_simple 11 | 12 | gflags_catkin 13 | roscpp 14 | rosbag 15 | dvs_msgs 16 | 17 | 18 | -------------------------------------------------------------------------------- /dvs_crop_bag/crop_time/process_folder.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | 3 | # !/usr/bin/python 4 | """ 5 | 6 | """ 7 | 8 | from os.path import join 9 | import os 10 | import subprocess 11 | from glob import glob 12 | import random 13 | import yaml 14 | import argparse 15 | import numpy as np 16 | 17 | 18 | def build_run_command(path_to_input_bag, time_intervals): 19 | # "rosrun dvs_crop_time dvs_crop_time path_to_input" 20 | rosrun_cmd = ['rosrun', 21 | 'dvs_crop_time', 22 | 'dvs_crop_time', 23 | path_to_input_bag, 24 | f'--t1s={time_intervals[0]}', 25 | f'--t1e={time_intervals[1]}', 26 | f'--t2s={time_intervals[2]}', 27 | f'--t2e={time_intervals[3]}'] 28 | 29 | return rosrun_cmd 30 | 31 | 32 | if __name__ == "__main__": 33 | 34 | # Parse command-line arguments 35 | parser = argparse.ArgumentParser() 36 | parser.add_argument("--folder", 37 | required=True, 38 | type=str, 39 | help="directory of rosbags") 40 | parser.add_argument("--config_path", 41 | required=True, 42 | type=str, 43 | help="path to times.txt config file") 44 | args = parser.parse_args() 45 | 46 | with open(args.config_path, 'r') as f: 47 | for line in f.readlines(): 48 | argList = line.rstrip().split(' ') 49 | rosrun_cmd = build_run_command(os.path.join(args.folder, argList[0]), argList[1:]) 50 | print(subprocess.check_output(rosrun_cmd)) 51 | 52 | -------------------------------------------------------------------------------- /dvs_crop_bag/crop_time/src/crop_time.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | #define foreach BOOST_FOREACH 19 | 20 | DEFINE_double(t1s, -1, "timestamp 1 start"); 21 | DEFINE_double(t1e, -1, "timestamp 1 end"); 22 | DEFINE_double(t2s, -1, "timestamp 2 start"); 23 | DEFINE_double(t2e, -1, "timestamp 2 end"); 24 | DEFINE_double(t3s, -1, "timestamp 3 start"); 25 | DEFINE_double(t3e, -1, "timestamp 3 end"); 26 | 27 | bool parse_arguments(int argc, char* argv[], 28 | std::string* path_to_input_rosbag 29 | ) 30 | { 31 | constexpr int expected_num_arguments = 2; 32 | if(argc < expected_num_arguments) 33 | { 34 | std::cerr << "Not enough arguments, "<< argc << " given, " << expected_num_arguments << " expected." << std::endl; 35 | std::cerr << "Usage: rosrun dvs_crop_bag dvs_crop_bag path_to_bag.bag" << std::endl; 36 | return false; 37 | } 38 | 39 | *path_to_input_rosbag = std::string(argv[1]); 40 | 41 | return true; 42 | } 43 | 44 | bool time_within_crop(const ros::Time& timestamp) 45 | { 46 | static const double first_timestamp = timestamp.toSec(); // initialised only once 47 | const double t_now = timestamp.toSec() - first_timestamp; 48 | 49 | bool in_t1 = t_now > FLAGS_t1s && t_now < FLAGS_t1e; 50 | bool in_t2 = t_now > FLAGS_t2s && t_now < FLAGS_t2e; 51 | bool in_t3 = t_now > FLAGS_t3s && t_now < FLAGS_t3e; 52 | return in_t1 || in_t2 || in_t3; 53 | } 54 | 55 | dvs_msgs::EventArray new_event_msg(rosbag::MessageInstance const& m, 56 | const ros::Duration& duration_to_subtract 57 | ) 58 | { 59 | dvs_msgs::EventArray event_array_msg; 60 | 61 | std::vector events; 62 | dvs_msgs::EventArrayConstPtr s = m.instantiate(); 63 | for(auto e : s->events) 64 | { 65 | double new_ts = e.ts.toSec() - duration_to_subtract.toSec(); 66 | if (new_ts > 0.001) // not too close to 'zero' 67 | { 68 | e.ts = ros::Time(new_ts); 69 | events.push_back(e); 70 | } 71 | } 72 | 73 | event_array_msg.events = events; 74 | event_array_msg.width = s->width; 75 | event_array_msg.height = s->height; 76 | event_array_msg.header.stamp = s->header.stamp - duration_to_subtract; 77 | 78 | return event_array_msg; 79 | } 80 | 81 | int main(int argc, char* argv[]) 82 | { 83 | std::string path_to_input_rosbag; 84 | // parse my arguments first before google gflags 85 | if (!parse_arguments(argc, 86 | argv, 87 | &path_to_input_rosbag 88 | )) 89 | { 90 | return -1; 91 | } 92 | 93 | google::ParseCommandLineFlags(&argc, &argv, true); 94 | 95 | constexpr double TIME_PADDING = 0.01; 96 | ros::Time previous_endtime = ros::Time(TIME_PADDING); 97 | ros::Duration duration_to_subtract = ros::Duration(0.0 - previous_endtime.toSec() - TIME_PADDING); 98 | bool was_within_crop = false; 99 | 100 | rosbag::Bag input_bag; 101 | try 102 | { 103 | input_bag.open(path_to_input_rosbag, rosbag::bagmode::Read); 104 | } 105 | catch(rosbag::BagIOException e) 106 | { 107 | std::cerr << "Error: could not open rosbag: " << path_to_input_rosbag << std::endl; 108 | return -1; 109 | } 110 | 111 | rosbag::Bag output_bag; 112 | std::string path_to_output_rosbag = path_to_input_rosbag + ".filtered"; 113 | output_bag.open(path_to_output_rosbag, rosbag::bagmode::Write); 114 | 115 | rosbag::View view(input_bag); 116 | foreach(rosbag::MessageInstance const& m, view) 117 | { 118 | const ros::Time timestamp = m.getTime(); 119 | if (time_within_crop(timestamp)) 120 | { 121 | if (!was_within_crop) 122 | { 123 | duration_to_subtract = ros::Duration(timestamp.toSec() - previous_endtime.toSec() - TIME_PADDING); 124 | was_within_crop = true; 125 | } 126 | const ros::Time new_timestamp = timestamp - duration_to_subtract; 127 | 128 | if (new_timestamp < ros::TIME_MIN) // sanity check 129 | { 130 | std::cerr << "new timestamp < ros::TIME_MIN, skipping." << std::endl; 131 | continue; 132 | } 133 | if (m.getDataType() == "dvs_msgs/EventArray") 134 | { 135 | output_bag.write(m.getTopic(), new_timestamp, new_event_msg(m, duration_to_subtract)); 136 | } 137 | else 138 | { 139 | output_bag.write(m.getTopic(), new_timestamp, m); 140 | } 141 | } 142 | else 143 | { 144 | if (was_within_crop) 145 | { 146 | previous_endtime = timestamp - duration_to_subtract; 147 | } 148 | was_within_crop = false; 149 | } 150 | } 151 | 152 | output_bag.close(); 153 | input_bag.close(); 154 | 155 | return 0; 156 | } 157 | -------------------------------------------------------------------------------- /dvs_frame_filter/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(dvs_frame_filter) 3 | 4 | find_package(catkin_simple REQUIRED) 5 | 6 | catkin_simple() 7 | 8 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -O3") 9 | 10 | # make the executable 11 | cs_add_executable(dvs_frame_filter 12 | src/frame_filter.cpp 13 | ) 14 | 15 | # link the executable to the necesarry libs 16 | target_link_libraries(dvs_frame_filter 17 | ${catkin_LIBRARIES} 18 | ) 19 | 20 | cs_install() 21 | 22 | -------------------------------------------------------------------------------- /dvs_frame_filter/README.md: -------------------------------------------------------------------------------- 1 | # DVS Frame Filter 2 | 3 | Removes bad frames that do not have correct dimensions (height, width). 4 | If height or width are not provided or set to zero, it will automatically detect the correct height and width based on mode of a sample of event and image messages. 5 | 6 | ### Usage: 7 | #### Single .bag file: 8 | 9 | rosrun dvs_frame_filter dvs_frame_filter path_to_input.bag 10 | 11 | height and width are optional. 12 | 13 | #### Process folder (used to process an entire folder of .bag files): 14 | 15 | python process_folder.py --folder --height --width 16 | 17 | --height, --width are optional. 18 | If --height or --width are not provided or set to zero, it will automatically detect height and width for each bag. 19 | 20 | ## Installation 21 | 22 | Assumes you have already set up your workspace (see [here](https://github.com/cedric-scheerlinck/dvs_image_reconstruction) for more details). 23 | 24 | Build 25 | 26 | catkin build dvs_frame_filter 27 | source ~/catkin_ws/devel/setup.bash 28 | 29 | Saves output to ```path_to_input.bag.filtered```. 30 | 31 | -------------------------------------------------------------------------------- /dvs_frame_filter/dependencies.yaml: -------------------------------------------------------------------------------- 1 | repositories: 2 | catkin_simple: 3 | type: git 4 | url: git@github.com:catkin/catkin_simple.git 5 | version: master 6 | gflags_catkin: 7 | type: git 8 | url: git@github.com:ethz-asl/gflags_catkin.git 9 | version: master 10 | -------------------------------------------------------------------------------- /dvs_frame_filter/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | dvs_frame_filter 4 | 0.0.0 5 | Removes "bad" image frames from a rosbag 6 | Cedric Scheerlinck 7 | GNU GPL 8 | 9 | catkin 10 | catkin_simple 11 | 12 | gflags_catkin 13 | roscpp 14 | rosbag 15 | dvs_msgs 16 | 17 | 18 | -------------------------------------------------------------------------------- /dvs_frame_filter/process_folder.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | 3 | # !/usr/bin/python 4 | """ 5 | 6 | """ 7 | 8 | from os.path import join 9 | import os 10 | import subprocess 11 | from glob import glob 12 | import random 13 | import yaml 14 | import argparse 15 | import numpy as np 16 | 17 | 18 | def build_run_command(path_to_input_bag, height, width): 19 | # "rosrun dvs_frame_filter dvs_frame_filter path_to_input" 20 | if height == 0 or width == 0: 21 | rosrun_cmd = ['rosrun', 22 | 'dvs_frame_filter', 23 | 'dvs_frame_filter', 24 | path_to_input_bag] 25 | else: 26 | # "rosrun dvs_frame_filter dvs_frame_filter path_to_input height width" 27 | rosrun_cmd = ['rosrun', 28 | 'dvs_frame_filter', 29 | 'dvs_frame_filter', 30 | path_to_input_bag, 31 | height, 32 | width] 33 | 34 | return rosrun_cmd 35 | 36 | 37 | if __name__ == "__main__": 38 | 39 | # Parse command-line arguments 40 | parser = argparse.ArgumentParser() 41 | parser.add_argument("--folder", 42 | required=True, 43 | type=str, 44 | help="directory of rosbags") 45 | parser.add_argument("--height", 46 | required=False, 47 | type=int, 48 | help="height") 49 | parser.add_argument("--width", 50 | required=False, 51 | type=int, 52 | default=0, 53 | help="width") 54 | args = parser.parse_args() 55 | 56 | for file in os.listdir(args.folder): 57 | if file.endswith(".bag"): 58 | # print(file) 59 | # append the arguments to the rosrun command 60 | rosrun_cmd = build_run_command(os.path.join(args.folder, file), args.height, args.width) 61 | # print rosrun_cmd 62 | print(subprocess.check_output(rosrun_cmd)) 63 | 64 | # now summarise output txt files 65 | 66 | # num_events_list = list() 67 | # num_frames_list = list() 68 | # duration_list = list() 69 | # stats_dir = os.path.join(args.folder, "stats") 70 | # for file in os.listdir(stats_dir): 71 | # if file.endswith(".txt"): 72 | # num_events, num_frames, duration = np.loadtxt(os.path.join(stats_dir, file), delimiter=',') 73 | # num_events_list.append(num_events) 74 | # num_frames_list.append(num_frames) 75 | # duration_list.append(duration) 76 | # 77 | # print(np.sum(num_events_list), np.sum(num_frames_list), np.sum(duration_list)) 78 | 79 | -------------------------------------------------------------------------------- /dvs_frame_filter/src/frame_filter.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | #define foreach BOOST_FOREACH 17 | 18 | bool parse_arguments(int argc, char* argv[], 19 | std::string* path_to_input_rosbag, 20 | int& height, 21 | int& width) 22 | { 23 | // std::cout << argc << std::endl; 24 | if(argc < 2) 25 | { 26 | std::cerr << "Not enough arguments" << std::endl; 27 | std::cerr << "Usage: rosrun dvs_frame_filter dvs_frame_filter path_to_bag.bag"; 28 | return false; 29 | } 30 | 31 | *path_to_input_rosbag = std::string(argv[1]); 32 | 33 | if (argc == 4) 34 | { 35 | height = std::stoi(argv[2]); 36 | width = std::stoi(argv[3]); 37 | } 38 | 39 | return true; 40 | } 41 | 42 | int mode(std::vector a) 43 | { 44 | int previousValue = a.front(); 45 | int mode = previousValue; 46 | int count = 1; 47 | int modeCount = 1; 48 | 49 | for (auto const& value: a) 50 | { 51 | if (value == previousValue) 52 | { // count occurrences of the current value 53 | ++count; 54 | } 55 | else 56 | { // now this is a different value 57 | if (count > modeCount) // first check if count exceeds the current mode 58 | { 59 | modeCount = count; 60 | mode = previousValue; 61 | } 62 | count = 1; // reset count for the new number 63 | previousValue = value; 64 | } 65 | } 66 | 67 | return mode; 68 | } 69 | 70 | void correct_frame_size(std::string path_to_input_rosbag, 71 | int& height, 72 | int& width) 73 | { 74 | rosbag::Bag input_bag; 75 | try 76 | { 77 | input_bag.open(path_to_input_rosbag, rosbag::bagmode::Read); 78 | } 79 | catch(rosbag::BagIOException e) 80 | { 81 | std::cerr << "Error: could not open rosbag: " << path_to_input_rosbag << std::endl; 82 | return; 83 | } 84 | 85 | rosbag::View view(input_bag); 86 | height = 0; // defaults 87 | width = 0; 88 | std::vector height_vec; 89 | std::vector width_vec; 90 | constexpr int N_SAMPLES = 100; 91 | int event_message_count = 0; 92 | int image_message_count = 0; 93 | 94 | // try and infer correct frame dimensions from 95 | // N samples of EventArray message and 96 | // 2N samples of Image messages. 97 | 98 | foreach(rosbag::MessageInstance const m, view) 99 | { 100 | if(m.getDataType() == "dvs_msgs/EventArray" && event_message_count < N_SAMPLES) 101 | { 102 | dvs_msgs::EventArrayConstPtr s = m.instantiate(); 103 | 104 | height_vec.push_back(s->height); 105 | width_vec.push_back(s->width); 106 | event_message_count++; 107 | } 108 | 109 | if(m.getDataType() == "sensor_msgs/Image" && image_message_count < 2*N_SAMPLES) 110 | { 111 | sensor_msgs::ImageConstPtr img_msg = m.instantiate(); 112 | 113 | height_vec.push_back(img_msg->height); 114 | width_vec.push_back(img_msg->width); 115 | image_message_count++; 116 | } 117 | 118 | if (height_vec.size() == 3*N_SAMPLES && width_vec.size() == 3*N_SAMPLES) 119 | { 120 | break; 121 | } 122 | } 123 | 124 | if (height_vec.size() == 0 || width_vec.size() == 0) 125 | { 126 | input_bag.close(); 127 | return; 128 | } 129 | 130 | sort(height_vec.begin(), height_vec.end()); 131 | sort(width_vec.begin(), width_vec.end()); 132 | 133 | height = mode(height_vec); 134 | width = mode(width_vec); 135 | 136 | input_bag.close(); 137 | return; 138 | } 139 | 140 | int main(int argc, char* argv[]) 141 | { 142 | std::string path_to_input_rosbag; 143 | int height = 0; 144 | int width = 0; 145 | int num_bad_frames = 0; 146 | 147 | if (!parse_arguments(argc, 148 | argv, 149 | &path_to_input_rosbag, 150 | height, 151 | width)) 152 | { 153 | return -1; 154 | } 155 | 156 | if (height == 0 || width == 0) 157 | { 158 | correct_frame_size(path_to_input_rosbag, height, width); 159 | std::cout << "Auto-detected height, width: " << height << ", " << width << std::endl; 160 | } 161 | else 162 | { 163 | std::cout << "Manually entered height, width: " << height << ", " << width << std::endl; 164 | } 165 | 166 | if (height == 0 || width == 0) 167 | { 168 | std::cerr << "Error: could not automatically determine correct frame size." << std::endl; 169 | return -1; 170 | } 171 | 172 | rosbag::Bag input_bag; 173 | try 174 | { 175 | input_bag.open(path_to_input_rosbag, rosbag::bagmode::Read); 176 | } 177 | catch(rosbag::BagIOException e) 178 | { 179 | std::cerr << "Error: could not open rosbag: " << path_to_input_rosbag << std::endl; 180 | return -1; 181 | } 182 | 183 | rosbag::Bag output_bag; 184 | std::string path_to_output_rosbag = path_to_input_rosbag + ".filtered"; 185 | output_bag.open(path_to_output_rosbag, rosbag::bagmode::Write); 186 | 187 | rosbag::View view(input_bag); 188 | foreach(rosbag::MessageInstance const m, view) 189 | { 190 | if(m.getDataType() == "sensor_msgs/Image") 191 | { 192 | sensor_msgs::ImageConstPtr img_msg = m.instantiate(); 193 | if (img_msg->height == height && img_msg->width == width) 194 | { 195 | output_bag.write(m.getTopic(), img_msg->header.stamp, m); 196 | } 197 | else 198 | { 199 | num_bad_frames++; 200 | } 201 | } 202 | else if(m.getDataType() == "dvs_msgs/EventArray") 203 | { 204 | dvs_msgs::EventArrayConstPtr s = m.instantiate(); 205 | ros::Time timestamp = s->header.stamp; 206 | if (timestamp < ros::TIME_MIN) 207 | { 208 | timestamp = s->events.back().ts; 209 | } 210 | output_bag.write(m.getTopic(), timestamp, m); 211 | } 212 | else if(m.getDataType() == "sensor_msgs/Imu") 213 | { 214 | sensor_msgs::ImuConstPtr imu_msg = m.instantiate(); 215 | output_bag.write(m.getTopic(), imu_msg->header.stamp, m); 216 | } 217 | else 218 | { 219 | output_bag.write(m.getTopic(), m.getTime(), m); 220 | } 221 | } 222 | 223 | std::cout << "Removed " << num_bad_frames << " bad frames." << std::endl; 224 | 225 | output_bag.close(); 226 | input_bag.close(); 227 | 228 | return 0; 229 | } 230 | -------------------------------------------------------------------------------- /dvs_hot_pixel_filter/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(dvs_hot_pixel_filter) 3 | 4 | find_package(catkin_simple REQUIRED) 5 | catkin_simple(ALL_DEPS_REQUIRED) 6 | find_package(OpenCV REQUIRED) 7 | 8 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -O3") 9 | 10 | cs_add_library(${PROJECT_NAME} src/utils.cpp) 11 | target_link_libraries(${PROJECT_NAME} ${OpenCV_LIBRARIES}) 12 | 13 | cs_add_executable(hot_pixel_filter src/hot_pixel_filter.cpp) 14 | target_link_libraries(hot_pixel_filter ${PROJECT_NAME}) 15 | 16 | cs_install() 17 | -------------------------------------------------------------------------------- /dvs_hot_pixel_filter/README.md: -------------------------------------------------------------------------------- 1 | # DVS Hot Pixel Filter 2 | 3 | Sometimes, you have a rosbag that contains hot pixels that fire many noise events. 4 | This package automatically detects hot pixels and creates a new rosbag without hot pixel events. 5 | It counts all events in the rosbag, then removes pixels based on either: 6 | 7 | * Top N pixels that receive most events, where N can be set using optional command-line flag --n_hot_pix, 8 | 9 | **or**, if --n_hot_pix is not set (or = -1), 10 | * Pixels that receive more than n_std*σ (standard deviations) events (default n_std = 5). 11 | 12 | Usage: 13 | 14 | rosrun dvs_hot_pixel_filter hot_pixel_filter path_to_input.bag 15 | 16 | Additional (optional) command-line flags include: 17 | * --n_hot_pix= 18 | * --n_std= (default n_std = 5) 19 | * --no_stats (do not save stats on disk) 20 | 21 | A new rosbag will be written, name ```input.bag.filtered```. It is an exact copy of the original bag, except hot pixel events have been removed. 22 | 23 | ## Installation 24 | 25 | Assumes you have already set up your workspace (see [here](https://github.com/cedric-scheerlinck/dvs_image_reconstruction) for more details). 26 | 27 | Clone this repository into your catkin workspace: 28 | 29 | cd ~/catkin_ws/src/ 30 | git clone https://github.com/cedric-scheerlinck/dvs_hot_pixel_filter.git 31 | 32 | Clone dependencies: 33 | 34 | vcs-import < dvs_hot_pixel_filter/dependencies.yaml 35 | 36 | Build 37 | 38 | catkin build dvs_hot_pixel_filter 39 | source ~/catkin_ws/devel/setup.bash 40 | 41 | ## Statistics 42 | 43 | Saves output statistics to ```./stats//```. 44 | 45 | 46 | ### Hot Pixels List 47 | 48 | ```hot_pixels.txt``` contains a list of hot pixel coordinates with format: ```x, y```. 49 | 50 | ### Event Count Histogram 51 | 52 | The package will output histograms before and after the hot pixels have been removed. 53 | 54 | ![event_count_histogram](images/hot_pixel_map.png) 55 | 56 | -------------------------------------------------------------------------------- /dvs_hot_pixel_filter/dependencies.yaml: -------------------------------------------------------------------------------- 1 | repositories: 2 | catkin_simple: 3 | type: git 4 | url: git@github.com:catkin/catkin_simple.git 5 | version: master 6 | gflags_catkin: 7 | type: git 8 | url: git@github.com:ethz-asl/gflags_catkin.git 9 | version: master 10 | -------------------------------------------------------------------------------- /dvs_hot_pixel_filter/images/hot_pixel_map.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cedric-scheerlinck/dvs_tools/76309b6257d7623a77a34e84ff9506a55b777d02/dvs_hot_pixel_filter/images/hot_pixel_map.png -------------------------------------------------------------------------------- /dvs_hot_pixel_filter/include/dvs_hot_pixel_filter/utils.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | namespace rosbag 10 | { 11 | class Bag; 12 | class MessageInstance; 13 | class View; 14 | } 15 | 16 | namespace dvs_hot_pixel_filter 17 | { 18 | namespace utils 19 | { 20 | 21 | typedef std::unordered_map topic_mats; 22 | typedef std::unordered_map> topic_points; 23 | 24 | template 25 | bool contains(T element, std::vector my_vector) 26 | { 27 | // returns true if my_vector contains element, otherwise false. 28 | for (auto topic_name : my_vector) 29 | { 30 | if (element == topic_name) 31 | { 32 | return true; 33 | } 34 | } 35 | return false; 36 | } 37 | 38 | bool parse_arguments(int argc, char* argv[], 39 | std::string* path_to_input_rosbag); 40 | 41 | std::string extract_bag_name(const std::string fullname); 42 | 43 | std::string usable_filename(const std::string filename_in); 44 | 45 | void write_histogram_image(const std::string filename, 46 | const cv::Mat& histogram, 47 | const std::vector& hot_pixels 48 | = std::vector()); 49 | 50 | void build_histograms(rosbag::View& view, 51 | topic_mats& histograms); 52 | 53 | void detect_hot_pixels(const topic_mats& histograms_by_topic, 54 | const double& num_std_devs, 55 | const int num_hot_pixels, 56 | topic_points& hot_pixels_by_topic); 57 | 58 | void hot_pixels_by_threshold(const cv::Mat& histogram, 59 | const double& threshold, 60 | std::vector& hot_pixels); 61 | 62 | void hot_pixels_by_ranking(const cv::Mat& histogram, 63 | const double& num_hot_pixels, 64 | std::vector& hot_pixels); 65 | 66 | void find_threshold(const cv::Mat& histogram, 67 | const double num_std_devs, 68 | double& threshold); 69 | 70 | void write_all_msgs(rosbag::View& view, 71 | topic_points& hot_pixels_by_topic, 72 | rosbag::Bag& output_bag); 73 | 74 | void write_event_msg(const std::string topic_name, 75 | const dvs_msgs::EventArrayConstPtr event_array_ptr, 76 | const std::vector& hot_pixels, 77 | rosbag::Bag& output_bag); 78 | 79 | void write_msg(const rosbag::MessageInstance& m, 80 | topic_points& hot_pixels_topic, 81 | rosbag::Bag& output_bag); 82 | 83 | void write_hot_pixels(const std::string filename, 84 | const std::vector& hot_pixels); 85 | 86 | void save_stats(const std::string bag_name, 87 | const std::string topic_name, 88 | const cv::Mat& histogram, 89 | const std::vector& hot_pixels, 90 | const bool one_topic); 91 | 92 | 93 | 94 | 95 | } // namespace utils 96 | } // namespace dvs_hot_pixel_filter 97 | -------------------------------------------------------------------------------- /dvs_hot_pixel_filter/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | dvs_hot_pixel_filter 4 | 0.0.0 5 | Find hot pixels in a rosbag and create a new rosbag without the hot pixels 6 | Cedric Scheerlinck 7 | GNU GPL 8 | 9 | catkin 10 | catkin_simple 11 | 12 | gflags_catkin 13 | roscpp 14 | rosbag 15 | dvs_msgs 16 | 17 | 18 | -------------------------------------------------------------------------------- /dvs_hot_pixel_filter/src/hot_pixel_filter.cpp: -------------------------------------------------------------------------------- 1 | #include "dvs_hot_pixel_filter/utils.h" 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | 8 | constexpr double NUM_STD_DEVS = 5; // default 9 | 10 | DEFINE_double(n_std, NUM_STD_DEVS, "Number of standard deviations for hot pixel threshold"); 11 | DEFINE_int32(n_hot_pix, -1, "Number of hot pixels to be removed"); 12 | DEFINE_bool(no_stats, false, "Do not save statistics to disk"); 13 | 14 | int main(int argc, char* argv[]) 15 | { 16 | google::ParseCommandLineFlags(&argc, &argv, true); 17 | 18 | // parse input arguments and setup input and output rosbags 19 | std::string path_to_input_rosbag; 20 | 21 | if(!dvs_hot_pixel_filter::utils::parse_arguments(argc, argv, &path_to_input_rosbag)) 22 | { 23 | return -1; 24 | } 25 | 26 | if (FLAGS_n_hot_pix == -1) 27 | { 28 | std::cout << "Number of hot pixels will be determined automatically\n" 29 | "by thresholding event_count > " << FLAGS_n_std << " STD (standard deviations)" << std::endl; 30 | } else 31 | { 32 | std::cout << "The " << FLAGS_n_hot_pix << " hottest pixel(s) will be removed" << std::endl; 33 | } 34 | 35 | std::string bag_name = dvs_hot_pixel_filter::utils::extract_bag_name( 36 | path_to_input_rosbag); 37 | 38 | std::string path_to_output_rosbag = path_to_input_rosbag + ".filtered"; 39 | 40 | rosbag::Bag input_bag; 41 | try 42 | { 43 | input_bag.open(path_to_input_rosbag, rosbag::bagmode::Read); 44 | } 45 | catch(rosbag::BagIOException& e) 46 | { 47 | std::cerr << "Error: could not open rosbag: " << path_to_input_rosbag << std::endl; 48 | return -1; 49 | } 50 | 51 | rosbag::Bag output_bag; 52 | output_bag.open(path_to_output_rosbag, rosbag::bagmode::Write); 53 | 54 | rosbag::View view(input_bag); 55 | 56 | // initialise variables and start computation 57 | dvs_hot_pixel_filter::utils::topic_mats histograms_by_topic; 58 | dvs_hot_pixel_filter::utils::topic_points hot_pixels_by_topic; 59 | 60 | dvs_hot_pixel_filter::utils::build_histograms( 61 | view, histograms_by_topic); 62 | 63 | dvs_hot_pixel_filter::utils::detect_hot_pixels( 64 | histograms_by_topic, FLAGS_n_std, FLAGS_n_hot_pix, hot_pixels_by_topic); 65 | 66 | dvs_hot_pixel_filter::utils::write_all_msgs( 67 | view, hot_pixels_by_topic, output_bag); 68 | 69 | std::cout << "Computing stats..." << std::endl; 70 | 71 | output_bag.close(); 72 | input_bag.close(); 73 | 74 | // write statistics 75 | const bool one_topic = histograms_by_topic.size() == 1; 76 | std::cout << "Topic\t\t# Events\t#Hot pixels\t% Events discarded" << std::endl; 77 | for(auto topic : histograms_by_topic) 78 | { 79 | const std::string topic_name = topic.first; 80 | cv::Mat& histogram = topic.second; 81 | std::vector& hot_pixels = hot_pixels_by_topic[topic_name]; 82 | dvs_hot_pixel_filter::utils::save_stats( 83 | bag_name, topic_name, histogram, hot_pixels, one_topic); 84 | } 85 | 86 | return 0; 87 | } 88 | -------------------------------------------------------------------------------- /dvs_hot_pixel_filter/src/utils.cpp: -------------------------------------------------------------------------------- 1 | #include "dvs_hot_pixel_filter/utils.h" 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | DECLARE_bool(no_stats); 13 | 14 | namespace dvs_hot_pixel_filter { 15 | namespace utils { 16 | 17 | const std::string OUTPUT_FOLDER = "./stats/"; 18 | 19 | bool parse_arguments(int argc, char* argv[], 20 | std::string* path_to_input_rosbag) 21 | { 22 | if(argc < 2) 23 | { 24 | std::cerr << "Error: not enough input arguments.\n" 25 | "Usage:\n\trosrun dvs_hot_pixel_filter hot_pixel_filter path_to_bag.bag\n\n" 26 | "Additional (optional) command-line flags include:\n" 27 | "\t--n_hot_pix=\n" 28 | "\t--n_std=\n" 29 | "\t--no_stats (do not save stats on disk)"<< std::endl; 30 | return false; 31 | } 32 | 33 | *path_to_input_rosbag = std::string(argv[1]); 34 | std::cout << "Input bag: " << *path_to_input_rosbag << std::endl; 35 | return true; 36 | } 37 | 38 | void write_histogram_image(const std::string filename, 39 | const cv::Mat& histogram, 40 | const std::vector& hot_pixels) 41 | { 42 | cv::Mat display_image; 43 | 44 | if (!hot_pixels.empty()) 45 | { 46 | cv::Vec3b colour = cv::Vec3b(255, 0, 0); 47 | cv::Mat local_hist; 48 | histogram.copyTo(local_hist); 49 | // create mask 50 | cv::Mat mask = cv::Mat::zeros(histogram.size(), CV_8UC1); 51 | for (auto point : hot_pixels) 52 | { 53 | mask.at(point) = 1; 54 | } 55 | double max; 56 | cv::minMaxLoc(local_hist, nullptr, &max); 57 | local_hist.setTo(max, mask); 58 | cv::normalize(local_hist, display_image, 0, 255, cv::NORM_MINMAX, CV_8UC1); 59 | cv::applyColorMap(display_image, display_image, cv::COLORMAP_HOT); 60 | display_image.setTo(colour, mask); 61 | } 62 | else 63 | { 64 | cv::normalize(histogram, display_image, 0, 255, cv::NORM_MINMAX, CV_8UC1); 65 | cv::applyColorMap(display_image, display_image, cv::COLORMAP_HOT); 66 | } 67 | 68 | cv::Mat large_image; 69 | cv::resize(display_image, large_image, cv::Size(), 3, 3, cv::INTER_NEAREST); 70 | cv::imwrite(filename, large_image); 71 | } 72 | 73 | std::string extract_bag_name(const std::string fullname) 74 | { 75 | int pos = 0; 76 | int len = fullname.length(); 77 | // go from the back to the first forward- or back-slash. 78 | for (int i = len; i > 0; i--) 79 | { 80 | if (fullname[i] == '/' || fullname[i] == '\\') 81 | { 82 | pos = i + 1; 83 | break; 84 | } 85 | } 86 | int count = 4; 87 | // now go from there to the first '.' 88 | for (int i = 0; i < len; i++) 89 | { 90 | if (fullname[pos + i] == '.') 91 | { 92 | count = i; 93 | break; 94 | } 95 | } 96 | std::string bag_name = fullname.substr(pos, count); 97 | return bag_name; 98 | } 99 | 100 | void build_histograms(rosbag::View& view, 101 | topic_mats& histograms) 102 | { 103 | std::cout << "Building event count histogram(s)..." << std::endl; 104 | 105 | std::vector seen_topics; 106 | for(const rosbag::MessageInstance& m : view) 107 | { 108 | if(m.getDataType() == "dvs_msgs/EventArray") 109 | { 110 | const std::string topic_name = m.getTopic(); 111 | // pointer to the message 112 | dvs_msgs::EventArrayConstPtr s = m.instantiate(); 113 | const cv::Size msg_size = cv::Size(s->width, s->height); 114 | 115 | cv::Mat& histogram = histograms[topic_name]; 116 | 117 | // initialise event_count_histogram if we haven't seen the topic yet 118 | if ( !contains(topic_name, seen_topics) ) 119 | { 120 | histogram = cv::Mat::zeros(msg_size, CV_64FC1); 121 | seen_topics.push_back(topic_name); 122 | std::cout << "added " << topic_name << " to seen_topics" << std::endl; 123 | } 124 | 125 | if (msg_size != histogram.size()) 126 | { 127 | std::cerr << "Error: a new event message in " << topic_name << 128 | " does not match the existing topic histogram size.\n message: " << 129 | msg_size << "\t histogram: " << histogram.size() << std::endl; 130 | return; 131 | } 132 | 133 | for(auto e : s->events) 134 | { 135 | // accumulate events without discrimination 136 | histogram.at(e.y, e.x)++; 137 | } 138 | } 139 | } 140 | 141 | std::cout << "...done!" << std::endl; 142 | } 143 | 144 | void detect_hot_pixels(const topic_mats& histograms_by_topic, 145 | const double& num_std_devs, 146 | const int num_hot_pixels, 147 | topic_points& hot_pixels_by_topic) 148 | { 149 | for(const auto& topic : histograms_by_topic) 150 | { 151 | const std::string topic_name = topic.first; 152 | const cv::Mat& histogram = topic.second; 153 | std::vector& hot_pixels = hot_pixels_by_topic[topic_name]; 154 | if (num_hot_pixels == -1) 155 | { 156 | // auto-detect hot pixels 157 | double threshold; 158 | dvs_hot_pixel_filter::utils::find_threshold( 159 | histogram, num_std_devs, threshold); 160 | dvs_hot_pixel_filter::utils::hot_pixels_by_threshold( 161 | histogram, threshold, hot_pixels); 162 | } 163 | else 164 | { 165 | // user-specified number of hot pixels 166 | dvs_hot_pixel_filter::utils::hot_pixels_by_ranking( 167 | histogram, num_hot_pixels, hot_pixels); 168 | } 169 | } 170 | } 171 | 172 | void hot_pixels_by_threshold(const cv::Mat& histogram, 173 | const double& threshold, 174 | std::vector& hot_pixels) 175 | { 176 | for (int y = 0; y < histogram.rows; y++) 177 | { 178 | for (int x = 0; x < histogram.cols; x++) 179 | { 180 | if (histogram.at(y, x) > threshold) 181 | { 182 | hot_pixels.push_back(cv::Point(x, y)); 183 | } 184 | } 185 | } 186 | } 187 | void hot_pixels_by_ranking(const cv::Mat& histogram, 188 | const double& num_hot_pixels, 189 | std::vector& hot_pixels) 190 | { 191 | cv::Mat local_hist; 192 | histogram.copyTo(local_hist); 193 | 194 | for (int i = 0; i < num_hot_pixels; i++) 195 | { 196 | double max; 197 | cv::Point maxLoc; 198 | cv::minMaxLoc(local_hist, nullptr, &max, nullptr, &maxLoc); 199 | 200 | hot_pixels.push_back(maxLoc); 201 | local_hist.at(maxLoc) = 0; 202 | } 203 | } 204 | 205 | void find_threshold(const cv::Mat& histogram, 206 | const double num_std_devs, 207 | double& threshold) 208 | { 209 | cv::Scalar mean_Scalar, stdDev_Scalar; 210 | cv::meanStdDev(histogram, mean_Scalar, stdDev_Scalar, histogram > 0); 211 | 212 | const double mean = mean_Scalar[0]; 213 | const double stdDev = stdDev_Scalar[0]; 214 | threshold = mean + num_std_devs*stdDev; 215 | } 216 | 217 | void write_all_msgs(rosbag::View& view, 218 | topic_points& hot_pixels_by_topic, 219 | rosbag::Bag& output_bag) 220 | { 221 | constexpr int log_every_n_messages = 10000; 222 | const uint32_t num_messages = view.size(); 223 | uint32_t message_index = 0; 224 | std::cout << "Writing..." << std::endl; 225 | // write the new rosbag without hot pixels by iterating over all messages 226 | for(rosbag::MessageInstance const m : view) 227 | { 228 | write_msg( 229 | m, hot_pixels_by_topic, output_bag); 230 | if(message_index++ % log_every_n_messages == 0) 231 | { 232 | std::cout << "Message: " << message_index << " / " << num_messages << std::endl; 233 | } 234 | } 235 | 236 | std::cout << "Message: " << num_messages << " / " << num_messages << std::endl; 237 | std::cout << "...done!" << std::endl; 238 | } 239 | 240 | void write_event_msg(const std::string topic_name, 241 | const dvs_msgs::EventArrayConstPtr event_array_ptr, 242 | const std::vector& hot_pixels, 243 | rosbag::Bag& output_bag) 244 | { 245 | std::vector events; 246 | for(auto e : event_array_ptr->events) 247 | { 248 | if (!contains(cv::Point(e.x, e.y), hot_pixels)) 249 | { 250 | events.push_back(e); 251 | } 252 | } 253 | 254 | if (events.size() > 0) 255 | { 256 | // Write new event array message to output rosbag 257 | 258 | dvs_msgs::EventArray event_array_msg; 259 | event_array_msg.events = events; 260 | event_array_msg.width = event_array_ptr->width; 261 | event_array_msg.height = event_array_ptr->height; 262 | event_array_msg.header.stamp = events.back().ts; 263 | 264 | output_bag.write(topic_name, event_array_msg.header.stamp, event_array_msg); 265 | } 266 | } 267 | 268 | void write_msg(const rosbag::MessageInstance& m, 269 | topic_points& hot_pixels_topic, 270 | rosbag::Bag& output_bag) 271 | { 272 | if(m.getDataType() == "dvs_msgs/EventArray") 273 | { 274 | const std::string topic_name = m.getTopic(); 275 | std::vector& hot_pixels = hot_pixels_topic[topic_name]; 276 | dvs_msgs::EventArrayConstPtr event_array_ptr = m.instantiate(); 277 | write_event_msg(topic_name, event_array_ptr, hot_pixels, output_bag); 278 | 279 | } 280 | else if(m.getDataType() == "sensor_msgs/Image") 281 | { 282 | sensor_msgs::ImageConstPtr img_msg = m.instantiate(); 283 | output_bag.write(m.getTopic(), img_msg->header.stamp, m); 284 | } 285 | else if(m.getDataType() == "sensor_msgs/Imu") 286 | { 287 | sensor_msgs::ImuConstPtr imu_msg = m.instantiate(); 288 | output_bag.write(m.getTopic(), imu_msg->header.stamp, m); 289 | } 290 | else 291 | { 292 | output_bag.write(m.getTopic(), m.getTime(), m); 293 | } 294 | } 295 | 296 | std::string usable_filename(const std::string filename_in) 297 | { 298 | std::string filename = filename_in; 299 | std::replace( filename.begin(), filename.end(), '/', '_'); // replace all '/' to '_' 300 | std::replace( filename.begin(), filename.end(), '\\', '_'); // replace all '\' to '_' 301 | return filename; 302 | } 303 | 304 | void write_hot_pixels(const std::string filename, 305 | const std::vector& hot_pixels) 306 | { 307 | std::ofstream hot_pixels_file; 308 | hot_pixels_file.open(filename); 309 | // the important part 310 | for (const auto& point : hot_pixels) 311 | { 312 | hot_pixels_file << point.x << ", " << point.y << "\n"; 313 | } 314 | hot_pixels_file.close(); 315 | } 316 | 317 | void save_stats(const std::string bag_name, 318 | const std::string topic_name, 319 | const cv::Mat& histogram, 320 | const std::vector& hot_pixels, 321 | const bool one_topic) 322 | { 323 | cv::Mat histogram_after; 324 | histogram.copyTo(histogram_after); 325 | for (auto point : hot_pixels) 326 | { 327 | histogram_after.at(point) = 0; 328 | } 329 | 330 | const double num_events = cv::sum(histogram)[0]; 331 | const double num_events_after = cv::sum(histogram_after)[0]; 332 | const double percent_events_discarded = (1 - num_events_after/num_events)*100; 333 | 334 | std::cout << std::setprecision(4) << topic_name << "\t" << num_events << 335 | "\t" << hot_pixels.size() << "\t\t0\t(before)" << std::endl; 336 | 337 | std::cout << std::setprecision(4) << topic_name << "\t" << num_events_after << 338 | "\t0\t\t" << percent_events_discarded << "\t(after)" << std::endl; 339 | 340 | if (!FLAGS_no_stats) 341 | { 342 | // save images 343 | std::string dstDir = OUTPUT_FOLDER + bag_name + "/"; 344 | if (!one_topic) 345 | { 346 | dstDir += usable_filename(topic_name) + "/"; 347 | } 348 | boost::filesystem::create_directories(dstDir); // create if needed 349 | std::string fname_b = dstDir + "hist_before.png"; 350 | std::string fname_a = dstDir + "hist_after.png"; 351 | std::string fname_hp = dstDir + "hot_pixels.txt"; 352 | 353 | write_histogram_image(fname_b, histogram); 354 | write_histogram_image(fname_a, histogram_after, hot_pixels); 355 | write_hot_pixels(fname_hp, hot_pixels); 356 | } 357 | } 358 | 359 | } // namespace utils 360 | } // namespace dvs_hot_pixel_filter 361 | -------------------------------------------------------------------------------- /dvs_reverse_events/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(dvs_reverse_events) 3 | 4 | find_package(catkin_simple REQUIRED) 5 | catkin_simple(ALL_DEPS_REQUIRED) 6 | find_package(OpenCV REQUIRED) 7 | 8 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -O3") 9 | 10 | cs_add_library(${PROJECT_NAME} src/utils.cpp) 11 | target_link_libraries(${PROJECT_NAME} ${OpenCV_LIBRARIES}) 12 | 13 | cs_add_executable(reverse_events src/reverse_events.cpp) 14 | target_link_libraries(reverse_events ${PROJECT_NAME}) 15 | 16 | cs_install() 17 | -------------------------------------------------------------------------------- /dvs_reverse_events/README.md: -------------------------------------------------------------------------------- 1 | # DVS Reverse Events 2 | Sometimes, you have a whacky feeling and you just want to reverse the timestamps of all events in a rosbag. 3 | This package allows you to do so. 4 | 5 | Usage: 6 | 7 | rosrun dvs_reverse_events reverse_events path_to_input.bag 8 | 9 | A new rosbag will be written, name ```input.bag.reversed```. It is an exact copy of the original bag, except hot pixel events have been removed. 10 | 11 | ## Installation 12 | 13 | Assumes you have already set up your workspace (see [here](https://github.com/cedric-scheerlinck/dvs_image_reconstruction) for more details). 14 | 15 | Clone this repository into your catkin workspace: 16 | 17 | cd ~/catkin_ws/src/ 18 | git clone https://github.com/cedric-scheerlinck/dvs_reverse_events.git 19 | 20 | Clone dependencies: 21 | 22 | vcs-import < dvs_reverse_events/dependencies.yaml 23 | 24 | Build 25 | 26 | catkin build dvs_reverse_events 27 | source ~/catkin_ws/devel/setup.bash 28 | -------------------------------------------------------------------------------- /dvs_reverse_events/dependencies.yaml: -------------------------------------------------------------------------------- 1 | repositories: 2 | catkin_simple: 3 | type: git 4 | url: git@github.com:catkin/catkin_simple.git 5 | version: master 6 | gflags_catkin: 7 | type: git 8 | url: git@github.com:ethz-asl/gflags_catkin.git 9 | version: master 10 | -------------------------------------------------------------------------------- /dvs_reverse_events/include/dvs_reverse_events/utils.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | namespace rosbag 10 | { 11 | class Bag; 12 | class MessageInstance; 13 | class View; 14 | } 15 | 16 | namespace dvs_reverse_events 17 | { 18 | namespace utils 19 | { 20 | 21 | typedef std::unordered_map topic_mats; 22 | typedef std::unordered_map> topic_points; 23 | typedef std::unordered_map> topic_events; 24 | 25 | template 26 | bool contains(T element, std::vector my_vector) 27 | { 28 | // returns true if my_vector contains element, otherwise false. 29 | for (auto topic_name : my_vector) 30 | { 31 | if (element == topic_name) 32 | { 33 | return true; 34 | } 35 | } 36 | return false; 37 | } 38 | 39 | bool parse_arguments(int argc, char* argv[], 40 | std::string* path_to_input_rosbag); 41 | 42 | std::string extract_bag_name(const std::string fullname); 43 | 44 | std::string usable_filename(const std::string filename_in); 45 | 46 | void load_events(rosbag::View& view, topic_events& events_by_topic); 47 | 48 | void reverse_event_timestamps(topic_events& events_by_topic); 49 | 50 | void flip_polarity(topic_events& events_by_topic); 51 | 52 | void write_all_msgs(rosbag::View& view, 53 | topic_events& events_by_topic, 54 | rosbag::Bag& output_bag); 55 | 56 | void copy_msg(const rosbag::MessageInstance& m, 57 | rosbag::Bag& , 58 | double& total_duration); 59 | 60 | void write_msg(const rosbag::MessageInstance& m, 61 | topic_events& events_by_topic, 62 | rosbag::Bag& output_bag); 63 | 64 | void write_event_msg(const std::string topic_name, 65 | const dvs_msgs::EventArrayConstPtr event_array_ptr, 66 | const std::vector& event_vec, 67 | rosbag::Bag& output_bag); 68 | 69 | void write_hot_pixels(const std::string filename, 70 | const std::vector& hot_pixels); 71 | 72 | } // namespace utils 73 | } // namespace dvs_reverse_events 74 | -------------------------------------------------------------------------------- /dvs_reverse_events/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | dvs_reverse_events 4 | 0.0.0 5 | Swap all timestamps so that events are reversed 6 | Cedric Scheerlinck 7 | GNU GPL 8 | 9 | catkin 10 | catkin_simple 11 | 12 | gflags_catkin 13 | roscpp 14 | rosbag 15 | dvs_msgs 16 | 17 | 18 | -------------------------------------------------------------------------------- /dvs_reverse_events/src/reverse_events.cpp: -------------------------------------------------------------------------------- 1 | #include "dvs_reverse_events/utils.h" 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | 8 | constexpr double NUM_STD_DEVS = 5; // default 9 | 10 | DEFINE_double(n_std, NUM_STD_DEVS, "Number of standard deviations for hot pixel threshold"); 11 | DEFINE_int32(n_hot_pix, -1, "Number of hot pixels to be removed"); 12 | DEFINE_bool(polflip, false, "Flip polarities"); 13 | DEFINE_bool(mirror, true, "Mirror timestamps"); 14 | DEFINE_bool(fb, false, "Save both forward and backward pass in one bag"); 15 | DEFINE_double(t_minus, 0, "Time to subtract from every msg"); 16 | 17 | int main(int argc, char* argv[]) 18 | { 19 | google::ParseCommandLineFlags(&argc, &argv, true); 20 | 21 | // parse input arguments and setup input and output rosbags 22 | std::string path_to_input_rosbag; 23 | 24 | if(!dvs_reverse_events::utils::parse_arguments(argc, argv, &path_to_input_rosbag)) 25 | { 26 | return -1; 27 | } 28 | 29 | std::string bag_name = dvs_reverse_events::utils::extract_bag_name( 30 | path_to_input_rosbag); 31 | 32 | std::string extension = (FLAGS_polflip)? ".reversedflip" : ".reversed"; 33 | std::string path_to_output_rosbag = path_to_input_rosbag + extension; 34 | 35 | rosbag::Bag input_bag; 36 | try 37 | { 38 | input_bag.open(path_to_input_rosbag, rosbag::bagmode::Read); 39 | } 40 | catch(rosbag::BagIOException& e) 41 | { 42 | std::cerr << "Error: could not open rosbag: " << path_to_input_rosbag << std::endl; 43 | return -1; 44 | } 45 | 46 | rosbag::Bag output_bag; 47 | output_bag.open(path_to_output_rosbag, rosbag::bagmode::Write); 48 | 49 | rosbag::View view(input_bag); 50 | 51 | // initialise variables and start computation 52 | 53 | dvs_reverse_events::utils::topic_events events_by_topic; 54 | 55 | dvs_reverse_events::utils::load_events(view, events_by_topic); 56 | 57 | dvs_reverse_events::utils::reverse_event_timestamps(events_by_topic); 58 | 59 | if (FLAGS_polflip) 60 | { 61 | dvs_reverse_events::utils::flip_polarity(events_by_topic); 62 | } 63 | 64 | dvs_reverse_events::utils::write_all_msgs(view, events_by_topic, output_bag); 65 | 66 | output_bag.close(); 67 | input_bag.close(); 68 | 69 | // write statistics 70 | // const bool one_topic = histograms_by_topic.size() == 1; 71 | // std::cout << "Topic\t\t# Events\t#Hot pixels\t% Events discarded" << std::endl; 72 | // for(auto topic : histograms_by_topic) 73 | // { 74 | // const std::string topic_name = topic.first; 75 | // cv::Mat& histogram = topic.second; 76 | // std::vector& hot_pixels = hot_pixels_by_topic[topic_name]; 77 | // dvs_reverse_events::utils::save_stats( 78 | // bag_name, topic_name, histogram, hot_pixels, one_topic); 79 | // } 80 | 81 | return 0; 82 | } 83 | -------------------------------------------------------------------------------- /dvs_reverse_events/src/utils.cpp: -------------------------------------------------------------------------------- 1 | #include "dvs_reverse_events/utils.h" 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | DECLARE_bool(mirror); 13 | DECLARE_bool(fb); 14 | DECLARE_double(t_minus); 15 | 16 | namespace dvs_reverse_events { 17 | namespace utils { 18 | //fds 19 | const std::string OUTPUT_FOLDER = "./stats/"; 20 | 21 | bool parse_arguments(int argc, char* argv[], 22 | std::string* path_to_input_rosbag) 23 | { 24 | if(argc < 2) 25 | { 26 | std::cerr << "Error: not enough input arguments.\n" 27 | "Usage:\n\trosrun dvs_reverse_events reverse_events path_to_bag.bag\n\n" 28 | "Additional (optional) command-line flags include:\n" 29 | "\t--n_hot_pix=\n" 30 | "\t--n_std=\n" 31 | "\t--no_stats (do not save stats on disk)"<< std::endl; 32 | return false; 33 | } 34 | 35 | *path_to_input_rosbag = std::string(argv[1]); 36 | std::cout << "Input bag: " << *path_to_input_rosbag << std::endl; 37 | return true; 38 | } 39 | 40 | void load_events(rosbag::View& view, topic_events& events_by_topic) 41 | { 42 | std::cout << "loading events..." << std::endl; 43 | 44 | std::vector seen_topics; 45 | for(const rosbag::MessageInstance& m : view) 46 | { 47 | if(m.getDataType() == "dvs_msgs/EventArray") 48 | { 49 | const std::string topic_name = m.getTopic(); 50 | // pointer to the message 51 | dvs_msgs::EventArrayConstPtr s = m.instantiate(); 52 | const cv::Size msg_size = cv::Size(s->width, s->height); 53 | 54 | std::vector& event_vec = events_by_topic[topic_name]; 55 | 56 | // add to seen topic 57 | if ( !contains(topic_name, seen_topics) ) 58 | { 59 | seen_topics.push_back(topic_name); 60 | std::cout << "added " << topic_name << " to seen_topics" << std::endl; 61 | } 62 | 63 | for(auto e : s->events) 64 | { 65 | // accumulate events without discrimination 66 | event_vec.push_back(e); 67 | } 68 | } 69 | } 70 | 71 | std::cout << "...done!" << std::endl; 72 | } 73 | 74 | void reverse_event_timestamps(topic_events& events_by_topic) 75 | { 76 | for(auto& topic : events_by_topic) 77 | { 78 | const std::string topic_name = topic.first; 79 | std::vector& event_vec = topic.second; 80 | if (FLAGS_mirror) 81 | { 82 | const double largest_time = event_vec.back().ts.toSec(); 83 | for (auto& e : event_vec) 84 | { 85 | double ts = e.ts.toSec(); 86 | ts = -ts + largest_time; 87 | e.ts = ros::Time(ts); 88 | } 89 | } 90 | else 91 | { 92 | const int length = event_vec.size(); 93 | const int midpoint = static_cast(length/2); 94 | for (int i = 0; i < midpoint; i++) 95 | { 96 | const ros::Time tmp = event_vec[i].ts; 97 | event_vec[i].ts = event_vec[length - i - 1].ts; 98 | event_vec[length - i].ts = tmp; 99 | } 100 | } 101 | } 102 | } 103 | 104 | void flip_polarity(topic_events& events_by_topic) 105 | { 106 | for(auto& topic : events_by_topic) 107 | { 108 | const std::string topic_name = topic.first; 109 | std::vector& event_vec = topic.second; 110 | const int length = event_vec.size(); 111 | const int midpoint = static_cast(length/2); 112 | for (auto& e : event_vec) 113 | { 114 | e.polarity = !e.polarity; 115 | } 116 | } 117 | } 118 | 119 | void write_histogram_image(const std::string filename, 120 | const cv::Mat& histogram, 121 | const std::vector& hot_pixels) 122 | { 123 | cv::Mat display_image; 124 | 125 | if (!hot_pixels.empty()) 126 | { 127 | cv::Vec3b colour = cv::Vec3b(255, 0, 0); 128 | cv::Mat local_hist; 129 | histogram.copyTo(local_hist); 130 | // create mask 131 | cv::Mat mask = cv::Mat::zeros(histogram.size(), CV_8UC1); 132 | for (auto point : hot_pixels) 133 | { 134 | mask.at(point) = 1; 135 | } 136 | double max; 137 | cv::minMaxLoc(local_hist, nullptr, &max); 138 | local_hist.setTo(max, mask); 139 | cv::normalize(local_hist, display_image, 0, 255, cv::NORM_MINMAX, CV_8UC1); 140 | cv::applyColorMap(display_image, display_image, cv::COLORMAP_HOT); 141 | display_image.setTo(colour, mask); 142 | } 143 | else 144 | { 145 | cv::normalize(histogram, display_image, 0, 255, cv::NORM_MINMAX, CV_8UC1); 146 | cv::applyColorMap(display_image, display_image, cv::COLORMAP_HOT); 147 | } 148 | 149 | cv::Mat large_image; 150 | cv::resize(display_image, large_image, cv::Size(), 3, 3, cv::INTER_NEAREST); 151 | cv::imwrite(filename, large_image); 152 | } 153 | 154 | std::string extract_bag_name(const std::string fullname) 155 | { 156 | int pos = 0; 157 | int len = fullname.length(); 158 | // go from the back to the first forward- or back-slash. 159 | for (int i = len; i > 0; i--) 160 | { 161 | if (fullname[i] == '/' || fullname[i] == '\\') 162 | { 163 | pos = i + 1; 164 | break; 165 | } 166 | } 167 | int count = 4; 168 | // now go from there to the first '.' 169 | for (int i = 0; i < len; i++) 170 | { 171 | if (fullname[pos + i] == '.') 172 | { 173 | count = i; 174 | break; 175 | } 176 | } 177 | std::string bag_name = fullname.substr(pos, count); 178 | return bag_name; 179 | } 180 | 181 | void write_all_msgs(rosbag::View& view, 182 | topic_events& events_by_topic, 183 | rosbag::Bag& output_bag) 184 | { 185 | constexpr int log_every_n_messages = 10000; 186 | const uint32_t num_messages = view.size(); 187 | uint32_t message_index = 0; 188 | std::cout << "Writing..." << std::endl; 189 | if (!FLAGS_fb) 190 | { 191 | // copy paste everything except events from the intput rosbag into the output 192 | for(rosbag::MessageInstance const m : view) 193 | { 194 | write_msg(m, events_by_topic, output_bag); 195 | if(message_index++ % log_every_n_messages == 0) 196 | { 197 | std::cout << "Message: " << message_index << " / " << num_messages << std::endl; 198 | } 199 | } 200 | } 201 | else 202 | { 203 | std::cout << "forward-pass" << std::endl; 204 | 205 | double total_duration; 206 | std::string event_topic; 207 | // copy paste everything except events from the intput rosbag into the output 208 | for(rosbag::MessageInstance const m : view) 209 | { 210 | copy_msg(m, output_bag, total_duration); 211 | if(message_index++ % log_every_n_messages == 0) 212 | { 213 | std::cout << "Message: " << message_index << " / " << num_messages << std::endl; 214 | } 215 | if(m.getDataType() == "dvs_msgs/EventArray") 216 | { 217 | event_topic = m.getTopic(); 218 | } 219 | } 220 | std::cout << "backward-pass" << std::endl; 221 | std::cout << "End time: " << total_duration << std::endl; 222 | message_index = 0; 223 | std::vector& event_vec = events_by_topic[event_topic]; 224 | int idx = event_vec.size() - 1; 225 | for(rosbag::MessageInstance const m : view) 226 | { 227 | 228 | if(m.getTopic() == event_topic) 229 | { 230 | dvs_msgs::EventArrayConstPtr event_array_ptr = m.instantiate(); 231 | std::vector local_events; 232 | 233 | for(int i = 0; i < event_array_ptr->events.size(); i++) 234 | { 235 | event_vec[idx].ts += ros::Duration(total_duration); 236 | local_events.push_back(event_vec[idx]); 237 | idx--; 238 | } 239 | 240 | if (local_events.size() > 0) 241 | { 242 | // Write new event array message to output rosbag 243 | 244 | dvs_msgs::EventArray event_array_msg; 245 | event_array_msg.events = local_events; 246 | event_array_msg.width = event_array_ptr->width; 247 | event_array_msg.height = event_array_ptr->height; 248 | event_array_msg.header.stamp = local_events.back().ts; 249 | 250 | output_bag.write(event_topic, event_array_msg.header.stamp, event_array_msg); 251 | } 252 | 253 | if(message_index++ % log_every_n_messages == 0) 254 | { 255 | std::cout << "Message: " << message_index << " / " << num_messages << std::endl; 256 | } 257 | } 258 | } 259 | } 260 | 261 | std::cout << "Message: " << num_messages << " / " << num_messages << std::endl; 262 | std::cout << "...done!" << std::endl; 263 | } 264 | 265 | void copy_msg(const rosbag::MessageInstance& m, 266 | rosbag::Bag& output_bag, 267 | double& total_duration) 268 | { 269 | if(m.getDataType() == "dvs_msgs/EventArray") 270 | { 271 | dvs_msgs::EventArrayConstPtr event_array_ptr = m.instantiate(); 272 | output_bag.write(m.getTopic(), event_array_ptr->header.stamp - ros::Duration(FLAGS_t_minus), m); 273 | total_duration = event_array_ptr->header.stamp.toSec() - FLAGS_t_minus; 274 | } 275 | else if(m.getDataType() == "sensor_msgs/Image") 276 | { 277 | sensor_msgs::ImageConstPtr img_msg = m.instantiate(); 278 | output_bag.write(m.getTopic(), img_msg->header.stamp - ros::Duration(FLAGS_t_minus), m); 279 | } 280 | else if(m.getDataType() == "sensor_msgs/Imu") 281 | { 282 | sensor_msgs::ImuConstPtr imu_msg = m.instantiate(); 283 | output_bag.write(m.getTopic(), imu_msg->header.stamp - ros::Duration(FLAGS_t_minus), m); 284 | } 285 | else 286 | { 287 | output_bag.write(m.getTopic(), m.getTime() - ros::Duration(FLAGS_t_minus), m); 288 | } 289 | } 290 | 291 | void write_msg(const rosbag::MessageInstance& m, 292 | topic_events& events_by_topic, 293 | rosbag::Bag& output_bag) 294 | { 295 | if(m.getDataType() == "dvs_msgs/EventArray") 296 | { 297 | const std::string topic_name = m.getTopic(); 298 | std::vector& event_vec = events_by_topic[topic_name]; 299 | dvs_msgs::EventArrayConstPtr event_array_ptr = m.instantiate(); 300 | write_event_msg(topic_name, event_array_ptr, event_vec, output_bag); 301 | } 302 | // else if(m.getDataType() == "sensor_msgs/Image") 303 | // { 304 | // sensor_msgs::ImageConstPtr img_msg = m.instantiate(); 305 | // output_bag.write(m.getTopic(), img_msg->header.stamp, m); 306 | // } 307 | // else if(m.getDataType() == "sensor_msgs/Imu") 308 | // { 309 | // sensor_msgs::ImuConstPtr imu_msg = m.instantiate(); 310 | // output_bag.write(m.getTopic(), imu_msg->header.stamp, m); 311 | // } 312 | // else 313 | // { 314 | // output_bag.write(m.getTopic(), m.getTime(), m); 315 | // } 316 | } 317 | 318 | void write_event_msg(const std::string topic_name, 319 | const dvs_msgs::EventArrayConstPtr event_array_ptr, 320 | const std::vector& event_vec, 321 | rosbag::Bag& output_bag) 322 | { 323 | std::vector local_events; 324 | static int idx = event_vec.size() - 1; 325 | 326 | for(int i = 0; i < event_array_ptr->events.size(); i++) 327 | { 328 | local_events.push_back(event_vec[idx]); 329 | idx--; 330 | } 331 | 332 | if (local_events.size() > 0) 333 | { 334 | // Write new event array message to output rosbag 335 | 336 | dvs_msgs::EventArray event_array_msg; 337 | event_array_msg.events = local_events; 338 | event_array_msg.width = event_array_ptr->width; 339 | event_array_msg.height = event_array_ptr->height; 340 | event_array_msg.header.stamp = local_events.back().ts; 341 | 342 | output_bag.write(topic_name, event_array_msg.header.stamp, event_array_msg); 343 | } 344 | } 345 | 346 | std::string usable_filename(const std::string filename_in) 347 | { 348 | std::string filename = filename_in; 349 | std::replace( filename.begin(), filename.end(), '/', '_'); // replace all '/' to '_' 350 | std::replace( filename.begin(), filename.end(), '\\', '_'); // replace all '\' to '_' 351 | return filename; 352 | } 353 | 354 | } // namespace utils 355 | } // namespace dvs_reverse_events 356 | -------------------------------------------------------------------------------- /dvs_rosbag_stats/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(dvs_rosbag_stats) 3 | 4 | find_package(catkin_simple REQUIRED) 5 | 6 | catkin_simple() 7 | 8 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -O3") 9 | 10 | # make the executable 11 | cs_add_executable(dvs_rosbag_stats 12 | src/rosbag_stats.cpp 13 | ) 14 | 15 | # link the executable to the necesarry libs 16 | target_link_libraries(dvs_rosbag_stats 17 | ${catkin_LIBRARIES} 18 | ) 19 | 20 | cs_install() 21 | 22 | -------------------------------------------------------------------------------- /dvs_rosbag_stats/README.md: -------------------------------------------------------------------------------- 1 | # DVS Rosbag Stats 2 | 3 | Description. 4 | 5 | ### Usage: 6 | #### Single .bag file: 7 | 8 | rosrun dvs_rosbag_stats dvs_rosbag_stats path_to_input.bag 9 | 10 | #### Process folder (used to process an entire folder of .bag files): 11 | 12 | python process_folder.py --folder 13 | 14 | ## Installation 15 | 16 | Assumes you have already set up your workspace (see [here](https://github.com/cedric-scheerlinck/dvs_image_reconstruction) for more details). 17 | 18 | Build 19 | 20 | catkin build dvs_rosbag_stats 21 | source ~/catkin_ws/devel/setup.bash 22 | 23 | ## Statistics 24 | 25 | Saves output statistics to ```/stats/.txt```. 26 | -------------------------------------------------------------------------------- /dvs_rosbag_stats/dependencies.yaml: -------------------------------------------------------------------------------- 1 | repositories: 2 | catkin_simple: 3 | type: git 4 | url: git@github.com:catkin/catkin_simple.git 5 | version: master 6 | gflags_catkin: 7 | type: git 8 | url: git@github.com:ethz-asl/gflags_catkin.git 9 | version: master 10 | -------------------------------------------------------------------------------- /dvs_rosbag_stats/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | dvs_rosbag_stats 4 | 0.0.0 5 | Compute statistics about an event rosbag 6 | Cedric Scheerlinck 7 | GNU GPL 8 | 9 | catkin 10 | catkin_simple 11 | 12 | gflags_catkin 13 | roscpp 14 | rosbag 15 | dvs_msgs 16 | 17 | 18 | -------------------------------------------------------------------------------- /dvs_rosbag_stats/process_folder.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | 3 | # !/usr/bin/python 4 | """ 5 | 6 | """ 7 | 8 | from os.path import join 9 | import os 10 | import subprocess 11 | from glob import glob 12 | import random 13 | import yaml 14 | import argparse 15 | import numpy as np 16 | 17 | 18 | def build_run_command(path_to_input_bag): 19 | # "rosrun dvs_rosbag_stats dvs_rosbag_stats path_to_input" 20 | rosrun_cmd = ['rosrun', 'dvs_rosbag_stats', 'dvs_rosbag_stats', path_to_input_bag] 21 | return rosrun_cmd 22 | 23 | if __name__ == "__main__": 24 | 25 | # Parse command-line arguments 26 | parser = argparse.ArgumentParser() 27 | parser.add_argument("--folder", required=True, 28 | type=str, help="directory of rosbags") 29 | args = parser.parse_args() 30 | 31 | for file in os.listdir(args.folder): 32 | if file.endswith(".bag"): 33 | # print(file) 34 | # append the arguments to the rosrun command 35 | rosrun_cmd = build_run_command(os.path.join(args.folder, file)) 36 | # print rosrun_cmd 37 | print(subprocess.check_output(rosrun_cmd)) 38 | 39 | # now summarise output txt files 40 | 41 | num_events_list = list() 42 | num_frames_list = list() 43 | duration_list = list() 44 | stats_dir = os.path.join(args.folder, "stats") 45 | for file in os.listdir(stats_dir): 46 | if file.endswith(".txt"): 47 | num_events, num_frames, duration = np.loadtxt(os.path.join(stats_dir, file), delimiter=',') 48 | num_events_list.append(num_events) 49 | num_frames_list.append(num_frames) 50 | duration_list.append(duration) 51 | 52 | print(np.sum(num_events_list), np.sum(num_frames_list), np.sum(duration_list)) 53 | 54 | -------------------------------------------------------------------------------- /dvs_rosbag_stats/src/rosbag_stats.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | #define foreach BOOST_FOREACH 17 | 18 | bool parse_arguments(int argc, char* argv[], 19 | std::string* path_to_input_rosbag) 20 | { 21 | if(argc < 2) 22 | { 23 | std::cerr << "Not enough arguments" << std::endl; 24 | std::cerr << "Usage: rosrun dvs_rosbag_stats dvs_rosbag_stats path_to_bag.bag"; 25 | return false; 26 | } 27 | 28 | *path_to_input_rosbag = std::string(argv[1]); 29 | 30 | return true; 31 | } 32 | 33 | bool compute_stats(const std::string path_to_input_rosbag, 34 | int& num_events, 35 | int& num_frames, 36 | double& duration) 37 | { 38 | std::cout << "Processing: " << path_to_input_rosbag << std::endl; 39 | 40 | auto const pos = path_to_input_rosbag.find_last_of('/'); 41 | const std::string output_dir = path_to_input_rosbag.substr(0, pos + 1) + "stats/"; 42 | const std::string output_filename = path_to_input_rosbag.substr( 43 | pos + 1, path_to_input_rosbag.length() - (pos + 1) - 4) + ".txt"; 44 | const std::string path_to_output = output_dir + output_filename; 45 | boost::filesystem::create_directories(output_dir); 46 | 47 | rosbag::Bag input_bag; 48 | try 49 | { 50 | input_bag.open(path_to_input_rosbag, rosbag::bagmode::Read); 51 | } 52 | catch(rosbag::BagIOException e) 53 | { 54 | std::cerr << "Error: could not open rosbag: " << path_to_input_rosbag << std::endl; 55 | return false; 56 | } 57 | 58 | rosbag::View view(input_bag); 59 | 60 | std::unordered_map> event_packets_for_each_event_topic; 61 | 62 | const uint32_t num_messages = view.size(); 63 | uint32_t message_index = 0; 64 | 65 | int num_events_tmp = 0; 66 | int num_frames_tmp = 0; 67 | double start_time; 68 | double end_time = 0; 69 | bool first_msg = true; 70 | 71 | foreach(rosbag::MessageInstance const m, view) 72 | { 73 | if (m.getDataType() == "dvs_msgs/EventArray") 74 | { 75 | 76 | std::vector& events = event_packets_for_each_event_topic[m.getTopic()]; 77 | dvs_msgs::EventArrayConstPtr s = m.instantiate(); 78 | num_events_tmp += s->events.size(); 79 | if (first_msg) 80 | { 81 | start_time = s->events.front().ts.toSec(); 82 | first_msg = false; 83 | } 84 | end_time = std::max(s->events.back().ts.toSec(), end_time); 85 | } 86 | else if (m.getDataType() == "sensor_msgs/Image") 87 | { 88 | num_frames_tmp += 1; 89 | } 90 | } 91 | 92 | input_bag.close(); 93 | 94 | num_events = num_events_tmp; 95 | num_frames = num_frames_tmp; 96 | duration = end_time - start_time; 97 | 98 | return true; 99 | } 100 | 101 | bool write_stats(const std::string path_to_output, 102 | const int& num_events, 103 | const int& num_frames, 104 | const double& duration) 105 | { 106 | 107 | std::ofstream stats_file; 108 | stats_file.open(path_to_output); 109 | // stats_file << "Number of events: " << num_events << '\n'; 110 | // stats_file << "Number of frames: " << num_frames << '\n'; 111 | // stats_file << "Total duration (s): " << duration << '\n'; 112 | stats_file << num_events << ", " << num_frames << ", " << duration << std::endl; 113 | stats_file.close(); 114 | return true; 115 | } 116 | 117 | bool hasEnding (std::string const &fullString, std::string const &ending) { 118 | if (fullString.length() >= ending.length()) { 119 | return (0 == fullString.compare (fullString.length() - ending.length(), ending.length(), ending)); 120 | } else { 121 | return false; 122 | } 123 | } 124 | 125 | int main(int argc, char* argv[]) 126 | { 127 | std::string path_to_input_rosbag; 128 | int max_num_events_per_packet; 129 | ros::Duration max_duration_event_packet; 130 | 131 | if (!parse_arguments(argc, argv, &path_to_input_rosbag)) 132 | { 133 | return -1; 134 | } 135 | 136 | int num_events; 137 | int num_frames; 138 | double duration; 139 | 140 | if (!compute_stats(path_to_input_rosbag, 141 | num_events, 142 | num_frames, 143 | duration)) 144 | { 145 | return -1; 146 | } 147 | 148 | auto const pos = path_to_input_rosbag.find_last_of('/'); 149 | const std::string output_dir = path_to_input_rosbag.substr(0, pos + 1) + "stats/"; 150 | const std::string output_filename = path_to_input_rosbag.substr( 151 | pos + 1, path_to_input_rosbag.length() - (pos + 1) - 4) + ".txt"; 152 | const std::string path_to_output = output_dir + output_filename; 153 | boost::filesystem::create_directories(output_dir); 154 | write_stats(path_to_output, 155 | num_events, 156 | num_frames, 157 | duration); 158 | return 0; 159 | } 160 | -------------------------------------------------------------------------------- /dvs_sort_events/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(dvs_sort_events) 3 | 4 | find_package(catkin_simple REQUIRED) 5 | 6 | catkin_simple() 7 | 8 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -O3") 9 | 10 | cs_add_library(${PROJECT_NAME} src/utils.cpp) 11 | target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) 12 | 13 | # make the executable 14 | cs_add_executable(sort src/sort_events.cpp) 15 | # link the executable to the project lib 16 | target_link_libraries(sort ${PROJECT_NAME}) 17 | 18 | cs_install() 19 | 20 | -------------------------------------------------------------------------------- /dvs_sort_events/README.md: -------------------------------------------------------------------------------- 1 | # DVS sort events 2 | 3 | Sort events in a bag by timestamp. 4 | 5 | ### Usage: 6 | #### Single .bag file: 7 | 8 | rosrun dvs_sort_events sort path_to_input.bag --max_events= --max_duration_ms= 9 | 10 | max_events is the maximum number of events per message. 11 | max_duration_ms is the maximum duration of a message in milliseconds. 12 | 13 | If both are set, messages are written with whichever condition is met first. 14 | If neither are set, defaults to 30ms time-windows. 15 | 16 | #### Process folder (used to process a folder of .bag files): 17 | 18 | python process_folder.py --folder --max_events --max_duration_ms 19 | 20 | ## Installation 21 | 22 | Assumes you have already set up your workspace (see [here](https://github.com/cedric-scheerlinck/dvs_image_reconstruction) for more details). 23 | 24 | Build 25 | 26 | catkin build dvs_sort_events 27 | source ~/catkin_ws/devel/setup.bash 28 | 29 | Saves output to ```path_to_input.bag.sorted```. 30 | 31 | -------------------------------------------------------------------------------- /dvs_sort_events/dependencies.yaml: -------------------------------------------------------------------------------- 1 | repositories: 2 | catkin_simple: 3 | type: git 4 | url: git@github.com:catkin/catkin_simple.git 5 | version: master 6 | gflags_catkin: 7 | type: git 8 | url: git@github.com:ethz-asl/gflags_catkin.git 9 | version: master 10 | -------------------------------------------------------------------------------- /dvs_sort_events/include/dvs_sort_events/utils.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | 8 | namespace rosbag 9 | { 10 | class Bag; 11 | class MessageInstance; 12 | class View; 13 | } 14 | 15 | namespace dvs_sort_events 16 | { 17 | namespace utils 18 | { 19 | 20 | typedef std::unordered_map topic_eventArray; 21 | 22 | template 23 | bool contains(T element, std::vector my_vector) 24 | { 25 | // returns true if my_vector contains element, otherwise false. 26 | for (auto topic_name : my_vector) 27 | { 28 | if (element == topic_name) 29 | { 30 | return true; 31 | } 32 | } 33 | return false; 34 | } 35 | 36 | bool parse_arguments(int argc, char* argv[], 37 | std::string* path_to_input_rosbag); 38 | 39 | std::string extract_bag_name(const std::string fullname); 40 | 41 | std::string usable_filename(const std::string filename_in); 42 | 43 | bool load_events_from_bag(std::string path_to_input_rosbag, 44 | topic_eventArray& events); 45 | 46 | void load_events(rosbag::View& view, 47 | topic_eventArray& events_by_topic); 48 | 49 | void sort_events(topic_eventArray& events_by_topic); 50 | 51 | bool is_timestamp_in_order(dvs_msgs::Event first, dvs_msgs::Event second); 52 | 53 | 54 | void write_events_to_bag(topic_eventArray& events_by_topic, 55 | const int& max_num_events_per_packet, 56 | const double& max_duration_event_packet, 57 | std::string path_to_output_rosbag); 58 | 59 | } // namespace utils 60 | } // namespace dvs_sort_events 61 | -------------------------------------------------------------------------------- /dvs_sort_events/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | dvs_sort_events 4 | 0.0.0 5 | Sort events in a rosbag by timestamp 6 | Cedric Scheerlinck 7 | GNU GPL 8 | 9 | catkin 10 | catkin_simple 11 | 12 | gflags_catkin 13 | roscpp 14 | rosbag 15 | dvs_msgs 16 | 17 | 18 | -------------------------------------------------------------------------------- /dvs_sort_events/process_folder.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | 3 | # !/usr/bin/python 4 | """ 5 | 6 | """ 7 | 8 | from os.path import join 9 | import os 10 | import subprocess 11 | from glob import glob 12 | import random 13 | import yaml 14 | import argparse 15 | import numpy as np 16 | 17 | 18 | def build_run_command(path_to_input_bag, max_events, max_duration_ms): 19 | # "rosrun dvs_sort_events sort path_to_input --max_events=n --max_duration_ms=t" 20 | rosrun_cmd = ['rosrun', 21 | 'dvs_sort_events', 22 | 'sort', 23 | path_to_input_bag, 24 | f'--max_events={max_events}', 25 | f'--max_duration_ms={max_duration_ms}'] 26 | 27 | return rosrun_cmd 28 | 29 | 30 | if __name__ == "__main__": 31 | 32 | # Parse command-line arguments 33 | parser = argparse.ArgumentParser() 34 | parser.add_argument("--folder", 35 | required=True, 36 | type=str, 37 | help="directory of rosbags") 38 | parser.add_argument("--max_events", 39 | required=True, 40 | type=str, 41 | help="Maximum number of events per output message") 42 | parser.add_argument("--max_duration_ms", 43 | required=True, 44 | type=str, 45 | help="Maximum duration of output message") 46 | args = parser.parse_args() 47 | 48 | for file in os.listdir(args.folder): 49 | if file.endswith(".bag"): 50 | rosrun_cmd = build_run_command(os.path.join(args.folder, file), args.max_events, args.max_duration_ms) 51 | print(subprocess.check_output(rosrun_cmd)) 52 | -------------------------------------------------------------------------------- /dvs_sort_events/src/sort_events.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | #include "../include/dvs_sort_events/utils.h" 20 | 21 | constexpr int INITIAL_MAX_EVENTS = -1; 22 | constexpr double INITIAL_MAX_DURATION_MS = 1e6; 23 | 24 | DEFINE_int32(max_events, INITIAL_MAX_EVENTS, "Maximum number of events per output message"); 25 | DEFINE_double(max_duration_ms, INITIAL_MAX_DURATION_MS, "Maximum duration of output message in milliseconds"); 26 | 27 | int main(int argc, char* argv[]) 28 | { 29 | std::string path_to_input_rosbag; 30 | // parse my arguments first before google gflags 31 | if (!dvs_sort_events::utils::parse_arguments(argc, 32 | argv, 33 | &path_to_input_rosbag 34 | )) 35 | { 36 | return -1; 37 | } 38 | 39 | google::ParseCommandLineFlags(&argc, &argv, true); 40 | 41 | std::string path_to_output_rosbag = path_to_input_rosbag + ".sorted"; 42 | dvs_sort_events::utils::topic_eventArray events_by_topic; 43 | 44 | if (!dvs_sort_events::utils::load_events_from_bag(path_to_input_rosbag, events_by_topic)) 45 | { 46 | return -1; 47 | } 48 | 49 | std::cout << "Sorting... "; 50 | 51 | auto start = std::chrono::high_resolution_clock::now(); 52 | dvs_sort_events::utils::sort_events(events_by_topic); 53 | auto stop = std::chrono::high_resolution_clock::now(); 54 | std::chrono::duration elapsed_seconds = stop-start; 55 | 56 | std::cout << "took " << elapsed_seconds.count() << "s." << std::endl; 57 | 58 | std::cout << "Writing... "; 59 | 60 | const double max_duration_ms = (FLAGS_max_events == INITIAL_MAX_EVENTS 61 | && FLAGS_max_duration_ms == INITIAL_MAX_DURATION_MS) 62 | ? 30 : FLAGS_max_duration_ms; // limit default to 30ms if no flags passed 63 | 64 | dvs_sort_events::utils::write_events_to_bag(events_by_topic, 65 | FLAGS_max_events, 66 | max_duration_ms/1e3, 67 | path_to_output_rosbag); 68 | std::cout << "done!" << std::endl; 69 | 70 | 71 | return 0; 72 | } 73 | -------------------------------------------------------------------------------- /dvs_sort_events/src/utils.cpp: -------------------------------------------------------------------------------- 1 | #include "../include/dvs_sort_events/utils.h" 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | 11 | #define foreach BOOST_FOREACH 12 | 13 | namespace dvs_sort_events { 14 | namespace utils { 15 | //const std::string OUTPUT_FOLDER = "./stats/"; 16 | 17 | bool parse_arguments(int argc, char* argv[], 18 | std::string* path_to_input_rosbag 19 | ) 20 | { 21 | constexpr int expected_num_arguments = 2; 22 | if(argc < expected_num_arguments) 23 | { 24 | std::cerr << "Not enough arguments, "<< argc << " given, " << expected_num_arguments << " expected." << std::endl; 25 | std::cerr << "Usage: rosrun dvs_crop_bag dvs_crop_bag path_to_bag.bag" << std::endl; 26 | return false; 27 | } 28 | 29 | *path_to_input_rosbag = std::string(argv[1]); 30 | 31 | return true; 32 | } 33 | 34 | bool load_events_from_bag(std::string path_to_input_rosbag, 35 | topic_eventArray& events_by_topic) 36 | { 37 | rosbag::Bag input_bag; 38 | try 39 | { 40 | input_bag.open(path_to_input_rosbag, rosbag::bagmode::Read); 41 | } 42 | catch(rosbag::BagIOException e) 43 | { 44 | std::cerr << "Error: could not open rosbag: " << path_to_input_rosbag << std::endl; 45 | return false; 46 | } 47 | 48 | rosbag::View view(input_bag); 49 | 50 | load_events(view, events_by_topic); 51 | 52 | input_bag.close(); 53 | 54 | return true; 55 | } 56 | 57 | void load_events(rosbag::View& view, topic_eventArray& events_by_topic) 58 | { 59 | std::cout << "loading events..." << std::endl; 60 | 61 | std::vector seen_topics; 62 | for(const rosbag::MessageInstance& m : view) 63 | { 64 | if(m.getDataType() == "dvs_msgs/EventArray") 65 | { 66 | const std::string topic_name = m.getTopic(); 67 | // pointer to the message 68 | dvs_msgs::EventArrayConstPtr s = m.instantiate(); 69 | 70 | dvs_msgs::EventArray& event_array = events_by_topic[topic_name]; 71 | 72 | // add to seen topic 73 | if ( !contains(topic_name, seen_topics) ) 74 | { 75 | seen_topics.push_back(topic_name); 76 | event_array.width = s->width; 77 | event_array.height = s->height; 78 | std::cout << "added " << topic_name << " to seen_topics" << std::endl; 79 | } 80 | 81 | for(auto e : s->events) 82 | { 83 | // accumulate events without discrimination 84 | event_array.events.push_back(e); 85 | } 86 | } 87 | } 88 | 89 | std::cout << "...done!" << std::endl; 90 | } 91 | 92 | void sort_events(topic_eventArray& events_by_topic) 93 | { 94 | for (auto& iter: events_by_topic) 95 | { 96 | auto& event_array = iter.second; 97 | std::sort(event_array.events.begin(), event_array.events.end(), is_timestamp_in_order); 98 | } 99 | 100 | } 101 | 102 | bool is_timestamp_in_order(dvs_msgs::Event first, dvs_msgs::Event second) 103 | { 104 | return first.ts.toSec() < second.ts.toSec(); 105 | } 106 | 107 | std::string extract_bag_name(const std::string fullname) 108 | { 109 | int pos = 0; 110 | int len = fullname.length(); 111 | // go from the back to the first forward- or back-slash. 112 | for (int i = len; i > 0; i--) 113 | { 114 | if (fullname[i] == '/' || fullname[i] == '\\') 115 | { 116 | pos = i + 1; 117 | break; 118 | } 119 | } 120 | int count = 4; 121 | // now go from there to the first '.' 122 | for (int i = 0; i < len; i++) 123 | { 124 | if (fullname[pos + i] == '.') 125 | { 126 | count = i; 127 | break; 128 | } 129 | } 130 | std::string bag_name = fullname.substr(pos, count); 131 | return bag_name; 132 | } 133 | 134 | void write_events_to_bag(topic_eventArray& events_by_topic, 135 | const int& max_num_events_per_packet, 136 | const double& max_duration_event_packet, 137 | std::string path_to_output_rosbag) 138 | { 139 | rosbag::Bag output_bag; 140 | output_bag.open(path_to_output_rosbag, rosbag::bagmode::Write); 141 | 142 | for (auto& iter: events_by_topic) 143 | { 144 | std::string topic = iter.first; 145 | dvs_msgs::EventArray& event_array_in = iter.second; 146 | std::vector events_out; 147 | for(auto e : event_array_in.events) 148 | { 149 | events_out.push_back(e); 150 | if(events_out.size() == max_num_events_per_packet 151 | || (events_out.back().ts.toSec() - events_out.front().ts.toSec()) >= max_duration_event_packet) 152 | { 153 | // Write new event array message to output rosbag 154 | dvs_msgs::EventArray event_array_msg; 155 | event_array_msg.events = events_out; 156 | event_array_msg.width = event_array_in.width; 157 | event_array_msg.height = event_array_in.height; 158 | event_array_msg.header.stamp = events_out.back().ts; 159 | 160 | output_bag.write(topic, event_array_msg.header.stamp, event_array_msg); 161 | 162 | // Start new packet of events 163 | events_out.clear(); 164 | } 165 | } 166 | } 167 | output_bag.close(); 168 | } 169 | 170 | 171 | dvs_msgs::EventArray new_event_msg(rosbag::MessageInstance const& m, 172 | const ros::Duration& duration_to_subtract 173 | ) 174 | { 175 | dvs_msgs::EventArray event_array_msg; 176 | 177 | std::vector events; 178 | dvs_msgs::EventArrayConstPtr s = m.instantiate(); 179 | for(auto e : s->events) 180 | { 181 | double new_ts = e.ts.toSec() - duration_to_subtract.toSec(); 182 | if (new_ts > 0.001) // not too close to 'zero' 183 | { 184 | e.ts = ros::Time(new_ts); 185 | events.push_back(e); 186 | } 187 | } 188 | 189 | event_array_msg.events = events; 190 | event_array_msg.width = s->width; 191 | event_array_msg.height = s->height; 192 | event_array_msg.header.stamp = s->header.stamp - duration_to_subtract; 193 | 194 | return event_array_msg; 195 | } 196 | 197 | std::string usable_filename(const std::string filename_in) 198 | { 199 | std::string filename = filename_in; 200 | std::replace( filename.begin(), filename.end(), '/', '_'); // replace all '/' to '_' 201 | std::replace( filename.begin(), filename.end(), '\\', '_'); // replace all '\' to '_' 202 | return filename; 203 | } 204 | 205 | } // namespace utils 206 | } // namespace dvs_sort_events 207 | -------------------------------------------------------------------------------- /script_templates/rosbag_filter_topic.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | for bag in $1/*.bag; do 3 | rosbag filter "$bag" "$bag".filtered "topic == '/dvs/events' or topic == '/dvs/image_raw'" 4 | done 5 | --------------------------------------------------------------------------------