├── CMakeLists.txt ├── README.md ├── cfg └── file_player.cfg ├── include └── file_player │ ├── color.h │ └── datathread.h ├── install_depend_package.sh ├── launch └── file_player.launch ├── msg └── tmp ├── package.xml └── src ├── ROSThread.cpp ├── ROSThread.h ├── color_code.h ├── main.cpp ├── mainwindow.cpp ├── mainwindow.h ├── mainwindow.ui ├── resources.qrc ├── resources ├── fragmentShader.fsh ├── fragmentShaderColor.fsh ├── fragmentShaderOBJ.fsh ├── vertexShader.vsh ├── vertexShaderColor.vsh └── vertexShaderOBJ.vsh └── test.cpp /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(file_player) 3 | 4 | add_definitions(-std=c++11) 5 | 6 | if (NOT CMAKE_BUILD_TYPE) 7 | set (CMAKE_BUILD_TYPE release) 8 | endif () 9 | 10 | find_package(catkin REQUIRED cmake_modules 11 | COMPONENTS 12 | roscpp 13 | rospy 14 | std_msgs 15 | geometry_msgs 16 | message_generation 17 | rosbag 18 | image_transport 19 | cv_bridge 20 | dynamic_reconfigure 21 | pcl_ros 22 | pcl_conversions 23 | pcl_msgs 24 | irp_sen_msgs 25 | camera_info_manager 26 | tf 27 | ) 28 | set(CMAKE_AUTOMOC ON) 29 | 30 | find_package(Eigen REQUIRED) 31 | 32 | #set (CMAKE_PREFIX_PATH /opt/Qt5.6.1/5.6/gcc_64/lib/cmake) 33 | 34 | #find_package(Qt5Core) 35 | find_package(Qt5Widgets) 36 | find_package(Qt5Gui) 37 | find_package(Qt5OpenGL) 38 | 39 | 40 | add_message_files( 41 | DIRECTORY msg 42 | ) 43 | 44 | generate_messages( 45 | DEPENDENCIES 46 | std_msgs 47 | geometry_msgs 48 | ) 49 | 50 | generate_dynamic_reconfigure_options( 51 | cfg/file_player.cfg 52 | #... 53 | ) 54 | 55 | catkin_package( 56 | INCLUDE_DIRS include 57 | LIBRARIES 58 | file_player 59 | CATKIN_DEPENDS 60 | roscpp rospy 61 | std_msgs 62 | geometry_msgs 63 | message_runtime 64 | image_transport 65 | cv_bridge 66 | dynamic_reconfigure 67 | pcl_ros 68 | pcl_conversions 69 | pcl_msgs 70 | irp_sen_msgs 71 | camera_info_manager 72 | tf 73 | 74 | DEPENDS 75 | Eigen 76 | ) 77 | 78 | 79 | set (SRC_DIR ${CMAKE_CURRENT_SOURCE_DIR}/src) 80 | 81 | set (File_Player_QTLib_src ${SRC_DIR}/mainwindow.cpp ${SRC_DIR}/ROSThread.cpp) 82 | set (File_Player_QTLib_hdr ${SRC_DIR}/mainwindow.h ${SRC_DIR}/ROSThread.h) 83 | set (File_Player_QTLib_ui ${SRC_DIR}/mainwindow.ui) 84 | set (File_Player_QTBin_src ${SRC_DIR}/main.cpp) 85 | 86 | 87 | 88 | #find_package(GLEW REQUIRED) 89 | #find_package(GLUT REQUIRED) 90 | 91 | 92 | include_directories( 93 | ${catkin_INCLUDE_DIRS} 94 | ${file_player_INCLUDE_DIRS} 95 | ${SRC_DIR} 96 | ${Qt5Widgets_INCLUDE_DIRS} 97 | ${PROJECT_BINARY_DIR} 98 | include 99 | ${Eigen_INCLUDE_DIRS} 100 | ) 101 | 102 | 103 | #qt5_wrap_cpp(File_Player_QTLib_hdr_moc ${File_Player_QTLib_hdr}) 104 | qt5_wrap_ui (File_Player_QTLib_ui_moc ${File_Player_QTLib_ui}) 105 | qt5_add_resources(SHADER_RSC_ADDED ${SRC_DIR}/resources.qrc) 106 | 107 | 108 | 109 | ########### 110 | ## Build ## 111 | ########### 112 | 113 | 114 | add_executable(file_player ${File_Player_QTLib_src} ${File_Player_QTLib_hdr} ${File_Player_QTBin_src} ${SHADER_RSC_ADDED} ${File_Player_QTLib_ui_moc}) 115 | 116 | add_dependencies(file_player file_player_msgs_generate_messages_cpp ${PROJECT_NAME}_gencfg) 117 | add_dependencies(file_player ${catkin_EXPORTED_TARGETS}) 118 | 119 | target_link_libraries(file_player 120 | 121 | ${catkin_LIBRARIES} 122 | ${QT_LIBRARIES} 123 | ${OPENGL_LIBRARIES} 124 | Qt5::Widgets 125 | Qt5::Gui 126 | Qt5::OpenGL 127 | GL 128 | ${BOOST_CUSTOM_LIBS} 129 | ${Eigen_LIBRARIES} 130 | ) 131 | 132 | 133 | 134 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # File player for stereo thermal dataset 2 | 3 | Maintainer: Seungsang Yun (seungsang@snu.ac.kr) 4 | 5 | ## 1. Obtain dependent package (defined msg) 6 | 7 | ``` 8 | $mkdir ~/catkin_ws 9 | $cd ~/catkin_ws 10 | $mkdir src 11 | $cd src 12 | $git clone https://github.com/rpmsnu/file_player_sthereo_dataset.git 13 | $cd ~/catkin_ws/src/file_player_sthereo_dataset 14 | $bash install_depend_package.sh 15 | ``` 16 | 17 | ## 2. Build workspace 18 | 19 | ``` 20 | $cd ~/catkin_ws 21 | $catkin_make 22 | ``` 23 | 24 | ## 3. Run file player 25 | 26 | ``` 27 | $source devel/setup.bash 28 | $roslaunch file_player file_player.launch 29 | ``` 30 | 31 | ## 4. Load data files and play 32 | 33 | 1. Click 'Load' button. 34 | 2. Choose data set folder including sensor_data folder and calibration folder. 35 | 3. The player button starts publishing data in the ROS message. 36 | 4. The Stop skip button skips data while the vehicle is stationary for convenience. 37 | 5. The loop button resumes when playback is finished. 38 | 39 | ## 5. Contributor 40 | * Jinyong Jeong : The original author 41 | -------------------------------------------------------------------------------- /cfg/file_player.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | PACKAGE = "dynamic_file_player" 3 | 4 | from dynamic_reconfigure.parameter_generator_catkin import * 5 | 6 | gen = ParameterGenerator() 7 | 8 | exit(gen.generate(PACKAGE, "dynamic_file_player", "dynamic_file_player")) 9 | -------------------------------------------------------------------------------- /include/file_player/color.h: -------------------------------------------------------------------------------- 1 | #ifndef _COLORS_ 2 | #define _COLORS_ 3 | 4 | /* FOREGROUND */ 5 | #define RST "\x1B[0m" 6 | #define KRED "\x1B[31m" 7 | #define KGRN "\x1B[32m" 8 | #define KYEL "\x1B[33m" 9 | #define KBLU "\x1B[34m" 10 | #define KMAG "\x1B[35m" 11 | #define KCYN "\x1B[36m" 12 | #define KWHT "\x1B[37m" 13 | 14 | #define FRED(x) KRED x RST 15 | #define FGRN(x) KGRN x RST 16 | #define FYEL(x) KYEL x RST 17 | #define FBLU(x) KBLU x RST 18 | #define FMAG(x) KMAG x RST 19 | #define FCYN(x) KCYN x RST 20 | #define FWHT(x) KWHT x RST 21 | 22 | #define BOLD(x) "\x1B[1m" x RST 23 | #define UNDL(x) "\x1B[4m" x RST 24 | 25 | #endif /* _COLORS_ */ 26 | -------------------------------------------------------------------------------- /include/file_player/datathread.h: -------------------------------------------------------------------------------- 1 | #ifndef DATATHREAD_H 2 | #define DATATHREAD_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | template 10 | struct DataThread{ 11 | 12 | std::mutex mutex_; 13 | std::queue data_queue_; 14 | std::condition_variable cv_; 15 | std::thread thread_; 16 | bool active_; 17 | 18 | DataThread() : active_(true){} 19 | 20 | void push(T data){ 21 | mutex_.lock(); 22 | data_queue_.push(data); 23 | mutex_.unlock(); 24 | } 25 | 26 | T pop(){ 27 | T result; 28 | mutex_.lock(); 29 | result = data_queue_.front(); 30 | data_queue_.pop(); 31 | mutex_.unlock(); 32 | return result; 33 | } 34 | 35 | // virtual void DataProcess(T data){ 36 | 37 | // } 38 | 39 | // void DataProcessThread(){ 40 | // while(1){ 41 | // std::unique_lock ul(mutex_); 42 | // cv_.wait(ul); 43 | // if(active_ == false) return; 44 | // ul.unlock(); 45 | 46 | // while(!data_queue_.empty()){ 47 | // auto data = pop(); 48 | // //process 49 | // DataProcess(data); 50 | // } 51 | // } 52 | // } 53 | 54 | }; 55 | 56 | #endif // DATATHREAD_H 57 | -------------------------------------------------------------------------------- /install_depend_package.sh: -------------------------------------------------------------------------------- 1 | cd .. 2 | git clone https://github.com/irapkaist/irp_sen_msgs.git 3 | git clone https://github.com/swri-robotics/novatel_gps_driver.git 4 | 5 | cd novatel_gps_driver 6 | mv novatel_gps_msgs ../ 7 | 8 | rm -rf novatel_gps_driver 9 | -------------------------------------------------------------------------------- /launch/file_player.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | -------------------------------------------------------------------------------- /msg/tmp: -------------------------------------------------------------------------------- 1 | tmp 2 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | file_player 4 | 0.0.0 5 | file player 6 | 7 | 8 | 9 | 10 | Seungsang Yun 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | http://irap.kaist.ac.kr --> 22 | 23 | 24 | 25 | 26 | Seungsang Yun --> 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | catkin 40 | roscpp 41 | rospy 42 | std_msgs 43 | geometry_msgs 44 | message_generation 45 | image_transport 46 | cv_bridge 47 | dynamic_reconfigure 48 | eigen 49 | pcl_ros 50 | pcl_conversions 51 | pcl_msgs 52 | irp_sen_msgs 53 | cmake_modules 54 | camera_info_manager 55 | tf 56 | 57 | roscpp 58 | rospy 59 | std_msgs 60 | geometry_msgs 61 | message_runtime 62 | image_transport 63 | cv_bridge 64 | dynamic_reconfigure 65 | eigen 66 | pcl_ros 67 | pcl_conversions 68 | pcl_msgs 69 | irp_sen_msgs 70 | cmake_modules 71 | camera_info_manager 72 | tf 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | -------------------------------------------------------------------------------- /src/ROSThread.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "ROSThread.h" 4 | 5 | using namespace std; 6 | 7 | ROSThread::ROSThread(QObject *parent, QMutex *th_mutex) : 8 | QThread(parent), mutex_(th_mutex) 9 | { 10 | processed_stamp_ = 0; 11 | play_rate_ = 1.0; 12 | loop_flag_ = false; 13 | stop_skip_flag_ = true; 14 | stereo_active_ = true; 15 | stereo_thermal_active_ = true; 16 | stereo_thermal_14bit_left_active_ = true; 17 | stereo_thermal_14bit_right_active_ = true; 18 | 19 | search_bound_ = 10; 20 | // search_bound_ = 1000000; 21 | reset_process_stamp_flag_ = false; 22 | auto_start_flag_ = true; 23 | stamp_show_count_ = 0; 24 | imu_data_version_ = 0; 25 | prev_clock_stamp_ = 0; 26 | } 27 | 28 | ROSThread::~ROSThread() 29 | { 30 | data_stamp_thread_.active_ = false; 31 | gps_thread_.active_ = false; 32 | inspva_thread_.active_ = false; 33 | imu_thread_.active_ = false; 34 | ouster_thread_.active_ = false; 35 | stereo_thread_.active_ = false; 36 | stereo_thermal_14bit_left_thread_.active_ = false; 37 | stereo_thermal_14bit_right_thread_.active_ = false; 38 | 39 | usleep(100000); 40 | 41 | data_stamp_thread_.cv_.notify_all(); 42 | if(data_stamp_thread_.thread_.joinable()) data_stamp_thread_.thread_.join(); 43 | 44 | gps_thread_.cv_.notify_all(); 45 | if(gps_thread_.thread_.joinable()) gps_thread_.thread_.join(); 46 | 47 | inspva_thread_.cv_.notify_all(); 48 | if(inspva_thread_.thread_.joinable()) inspva_thread_.thread_.join(); 49 | 50 | imu_thread_.cv_.notify_all(); 51 | if(imu_thread_.thread_.joinable()) imu_thread_.thread_.join(); 52 | 53 | ouster_thread_.cv_.notify_all(); 54 | if(ouster_thread_.thread_.joinable()) ouster_thread_.thread_.join(); 55 | 56 | stereo_thread_.cv_.notify_all(); 57 | if(stereo_thread_.thread_.joinable()) stereo_thread_.thread_.join(); 58 | 59 | stereo_thermal_14bit_left_thread_.cv_.notify_all(); 60 | if(stereo_thermal_14bit_left_thread_.thread_.joinable()) stereo_thermal_14bit_left_thread_.thread_.join(); 61 | 62 | stereo_thermal_14bit_right_thread_.cv_.notify_all(); 63 | if(stereo_thermal_14bit_right_thread_.thread_.joinable()) stereo_thermal_14bit_right_thread_.thread_.join(); 64 | } 65 | 66 | void ROSThread::ros_initialize(ros::NodeHandle &n) 67 | { 68 | nh_ = n; 69 | 70 | pre_timer_stamp_ = ros::Time::now().toNSec(); 71 | timer_ = nh_.createTimer(ros::Duration(0.0001), boost::bind(&ROSThread::TimerCallback, this, _1)); 72 | 73 | start_sub_ = nh_.subscribe("/file_player_start", 1, boost::bind(&ROSThread::FilePlayerStart, this, _1)); 74 | stop_sub_ = nh_.subscribe("/file_player_stop", 1, boost::bind(&ROSThread::FilePlayerStop, this, _1)); 75 | 76 | gps_pub_ = nh_.advertise("/gps/fix", 1000); 77 | inspva_pub_ = nh_.advertise("/inspva", 1000); 78 | 79 | // imu_origin_pub_ = nh_.advertise("/imu/data", 1000); 80 | imu_origin_pub_ = nh_.advertise("/xsens_imu_data", 1000); 81 | imu_pub_ = nh_.advertise("/imu/data_raw", 1000); 82 | magnet_pub_ = nh_.advertise("/imu/mag", 1000); 83 | ouster_pub_ = nh_.advertise("/os_cloud_node/points", 10000); 84 | 85 | stereo_left_pub_ = nh_.advertise("/stereo/left/image_raw", 10); 86 | stereo_right_pub_ = nh_.advertise("/stereo/right/image_raw", 10); 87 | stereo_thermal_14bit_left_pub_ = nh_.advertise("/thermal_14bit/left/image_raw", 10); 88 | stereo_thermal_14bit_right_pub_ = nh_.advertise("/thermal_14bit/right/image_raw", 10); 89 | 90 | stereo_left_info_pub_ = nh_.advertise("/stereo/left/camera_info", 10); 91 | stereo_right_info_pub_ = nh_.advertise("/stereo/right/camera_info", 10); 92 | stereo_thermal_14bit_left_info_pub_ = nh_.advertise("/thermal_14bit/left/camera_info", 10); 93 | stereo_thermal_14bit_right_info_pub_ = nh_.advertise("/thermal_14bit/right/camera_info", 10); 94 | 95 | clock_pub_ = nh_.advertise("/clock", 1); 96 | } 97 | 98 | void ROSThread::run() 99 | { 100 | ros::AsyncSpinner spinner(0); 101 | spinner.start(); 102 | ros::waitForShutdown(); 103 | } 104 | 105 | void ROSThread::Ready() 106 | { 107 | data_stamp_thread_.active_ = false; 108 | data_stamp_thread_.cv_.notify_all(); 109 | if(data_stamp_thread_.thread_.joinable()) data_stamp_thread_.thread_.join(); 110 | gps_thread_.active_ = false; 111 | gps_thread_.cv_.notify_all(); 112 | if(gps_thread_.thread_.joinable()) gps_thread_.thread_.join(); 113 | inspva_thread_.active_ = false; 114 | inspva_thread_.cv_.notify_all(); 115 | if(inspva_thread_.thread_.joinable()) inspva_thread_.thread_.join(); 116 | imu_thread_.active_ = false; 117 | imu_thread_.cv_.notify_all(); 118 | if(imu_thread_.thread_.joinable()) imu_thread_.thread_.join(); 119 | ouster_thread_.active_ = false; 120 | ouster_thread_.cv_.notify_all(); 121 | if(ouster_thread_.thread_.joinable()) ouster_thread_.thread_.join(); 122 | stereo_thread_.active_ = false; 123 | stereo_thread_.cv_.notify_all(); 124 | if(stereo_thread_.thread_.joinable()) stereo_thread_.thread_.join(); 125 | stereo_thermal_14bit_left_thread_.active_ = false; 126 | stereo_thermal_14bit_left_thread_.cv_.notify_all(); 127 | if(stereo_thermal_14bit_left_thread_.thread_.joinable()) stereo_thermal_14bit_left_thread_.thread_.join(); 128 | stereo_thermal_14bit_right_thread_.active_ = false; 129 | stereo_thermal_14bit_right_thread_.cv_.notify_all(); 130 | if(stereo_thermal_14bit_right_thread_.thread_.joinable()) stereo_thermal_14bit_right_thread_.thread_.join(); 131 | 132 | //check path is right or not 133 | ifstream f((data_folder_path_+"/sensor_data/data_stamp.csv").c_str()); 134 | if(!f.good()){ 135 | cout << "Please check file path. Input path is wrong" << endl; 136 | return; 137 | } 138 | f.close(); 139 | 140 | //Read CSV file and make map 141 | FILE *fp; 142 | int64_t stamp; 143 | //data stamp data load 144 | 145 | fp = fopen((data_folder_path_+"/sensor_data/data_stamp.csv").c_str(),"r"); 146 | char data_name[50]; 147 | data_stamp_.clear(); 148 | while(fscanf(fp,"%ld,%s\n",&stamp,data_name) == 2){ 149 | // data_stamp_[stamp] = data_name; 150 | data_stamp_.insert( multimap::value_type(stamp, data_name)); 151 | } 152 | cout << "Stamp data are loaded" << endl; 153 | fclose(fp); 154 | 155 | initial_data_stamp_ = data_stamp_.begin()->first - 1; 156 | last_data_stamp_ = prev(data_stamp_.end(),1)->first - 1; 157 | 158 | //Read gps data 159 | fp = fopen((data_folder_path_+"/sensor_data/gps.csv").c_str(),"r"); 160 | double latitude, longitude, altitude, altitude_orthometric; 161 | double cov[9]; 162 | sensor_msgs::NavSatFix gps_data; 163 | gps_data_.clear(); 164 | while( fscanf(fp,"%ld,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf\n", 165 | &stamp,&latitude,&longitude,&altitude,&cov[0],&cov[1],&cov[2],&cov[3],&cov[4],&cov[5],&cov[6],&cov[7],&cov[8]) 166 | == 13 167 | ) 168 | { 169 | gps_data.header.stamp.fromNSec(stamp); 170 | gps_data.header.frame_id = "gps"; 171 | gps_data.latitude = latitude; 172 | gps_data.longitude = longitude; 173 | gps_data.altitude = altitude; 174 | for(int i = 0 ; i < 9 ; i ++) gps_data.position_covariance[i] = cov[i]; 175 | gps_data_[stamp] = gps_data; 176 | } 177 | cout << "Gps data are loaded" << endl; 178 | 179 | fclose(fp); 180 | 181 | 182 | 183 | 184 | //Read inspva data 185 | fp = fopen((data_folder_path_+"/sensor_data/inspva.csv").c_str(),"r"); 186 | // double latitude, longitude, altitude, altitude_orthometric; 187 | double height, north_velocity, east_velocity, up_velocity, roll, pitch, azimuth; 188 | // string status; 189 | char status[17]; 190 | novatel_gps_msgs::Inspva inspva_data; 191 | inspva_data_.clear(); 192 | while(fscanf(fp,"%ld,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%s",&stamp,&latitude,&longitude,&height,&north_velocity,&east_velocity,&up_velocity,&roll,&pitch,&azimuth,status) == 11){ 193 | //17%19[^\n] %29[^\n] 194 | inspva_data.header.stamp.fromNSec(stamp); 195 | inspva_data.header.frame_id = "inspva"; 196 | inspva_data.latitude = latitude; 197 | inspva_data.longitude = longitude; 198 | inspva_data.height = height; 199 | inspva_data.north_velocity = north_velocity; 200 | inspva_data.east_velocity = east_velocity; 201 | inspva_data.up_velocity = up_velocity; 202 | inspva_data.roll = roll; 203 | inspva_data.pitch = pitch; 204 | inspva_data.azimuth = azimuth; 205 | inspva_data.status = status; 206 | inspva_data_[stamp] = inspva_data; 207 | } 208 | cout << "Inspva data are loaded" << endl; 209 | fclose(fp); 210 | 211 | //Read IMU data 212 | fp = fopen((data_folder_path_+"/sensor_data/xsens_imu.csv").c_str(),"r"); 213 | double q_x,q_y,q_z,q_w,x,y,z,g_x,g_y,g_z,a_x,a_y,a_z,m_x,m_y,m_z; 214 | irp_sen_msgs::imu imu_data_origin; 215 | sensor_msgs::Imu imu_data; 216 | sensor_msgs::MagneticField mag_data; 217 | imu_data_.clear(); 218 | mag_data_.clear(); 219 | 220 | while(1){ 221 | int length = fscanf(fp,"%ld,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf\n",&stamp,&q_x,&q_y,&q_z,&q_w,&x,&y,&z,&g_x,&g_y,&g_z,&a_x,&a_y,&a_z,&m_x,&m_y,&m_z); 222 | if(length != 8 && length != 17) break; 223 | if(length == 8){ 224 | imu_data.header.stamp.fromNSec(stamp); 225 | imu_data.header.frame_id = "imu"; 226 | imu_data.orientation.x = q_x; 227 | imu_data.orientation.y = q_y; 228 | imu_data.orientation.z = q_z; 229 | imu_data.orientation.w = q_w; 230 | 231 | imu_data_[stamp] = imu_data; 232 | imu_data_version_ = 1; 233 | 234 | imu_data_origin.header.stamp.fromNSec(stamp); 235 | imu_data_origin.header.frame_id = "imu"; 236 | imu_data_origin.quaternion_data.x = q_x; 237 | imu_data_origin.quaternion_data.y = q_y; 238 | imu_data_origin.quaternion_data.z = q_z; 239 | imu_data_origin.quaternion_data.w = q_w; 240 | imu_data_origin.eular_data.x = x; 241 | imu_data_origin.eular_data.y = y; 242 | imu_data_origin.eular_data.z = z; 243 | imu_data_origin_[stamp] = imu_data_origin; 244 | 245 | }else if(length == 17){ 246 | imu_data.header.stamp.fromNSec(stamp); 247 | imu_data.header.frame_id = "imu"; 248 | imu_data.orientation.x = q_x; 249 | imu_data.orientation.y = q_y; 250 | imu_data.orientation.z = q_z; 251 | imu_data.orientation.w = q_w; 252 | imu_data.angular_velocity.x = g_x; 253 | imu_data.angular_velocity.y = g_y; 254 | imu_data.angular_velocity.z = g_z; 255 | imu_data.linear_acceleration.x = a_x; 256 | imu_data.linear_acceleration.y = a_y; 257 | imu_data.linear_acceleration.z = a_z; 258 | 259 | imu_data.orientation_covariance[0] = 3; 260 | imu_data.orientation_covariance[4] = 3; 261 | imu_data.orientation_covariance[8] = 3; 262 | imu_data.angular_velocity_covariance[0] = 3; 263 | imu_data.angular_velocity_covariance[4] = 3; 264 | imu_data.angular_velocity_covariance[8] = 3; 265 | imu_data.linear_acceleration_covariance[0] = 3; 266 | imu_data.linear_acceleration_covariance[4] = 3; 267 | imu_data.linear_acceleration_covariance[8] = 3; 268 | 269 | 270 | imu_data_[stamp] = imu_data; 271 | mag_data.header.stamp.fromNSec(stamp); 272 | mag_data.header.frame_id = "imu"; 273 | mag_data.magnetic_field.x = m_x; 274 | mag_data.magnetic_field.y = m_y; 275 | mag_data.magnetic_field.z = m_z; 276 | mag_data_[stamp] = mag_data; 277 | imu_data_version_ = 2; 278 | 279 | 280 | imu_data_origin.header.stamp.fromNSec(stamp); 281 | imu_data_origin.header.frame_id = "imu"; 282 | imu_data_origin.quaternion_data.x = q_x; 283 | imu_data_origin.quaternion_data.y = q_y; 284 | imu_data_origin.quaternion_data.z = q_z; 285 | imu_data_origin.quaternion_data.w = q_w; 286 | imu_data_origin.eular_data.x = x; 287 | imu_data_origin.eular_data.y = y; 288 | imu_data_origin.eular_data.z = z; 289 | imu_data_origin.gyro_data.x = g_x; 290 | imu_data_origin.gyro_data.y = g_y; 291 | imu_data_origin.gyro_data.z = g_z; 292 | imu_data_origin.acceleration_data.x = a_x; 293 | imu_data_origin.acceleration_data.y = a_y; 294 | imu_data_origin.acceleration_data.z = a_z; 295 | imu_data_origin.magneticfield_data.x = m_x; 296 | imu_data_origin.magneticfield_data.y = m_y; 297 | imu_data_origin.magneticfield_data.z = m_z; 298 | imu_data_origin_[stamp] = imu_data_origin; 299 | 300 | } 301 | } 302 | cout << "IMU data are loaded" << endl; 303 | fclose(fp); 304 | 305 | ouster_file_list_.clear(); 306 | stereo_thermal_14bit_left_file_list_.clear(); 307 | stereo_thermal_14bit_right_file_list_.clear(); 308 | 309 | GetDirList(data_folder_path_ + "/sensor_data/ouster",ouster_file_list_); 310 | GetDirList(data_folder_path_ + "/image/stereo_left",stereo_file_list_); 311 | GetDirList(data_folder_path_ + "/image/stereo_thermal_14_left",stereo_thermal_14bit_left_file_list_); 312 | GetDirList(data_folder_path_ + "/image/stereo_thermal_14_right",stereo_thermal_14bit_right_file_list_); 313 | 314 | // GetDirList(data_folder_path_ + "/omni/cam0",omni_file_list_); 315 | 316 | //load camera info 317 | 318 | left_camera_nh_ = ros::NodeHandle(nh_,"left"); 319 | right_camera_nh_ = ros::NodeHandle(nh_,"right"); 320 | 321 | left_cinfo_ = boost::shared_ptr(new camera_info_manager::CameraInfoManager(left_camera_nh_,"/stereo/left")); 322 | right_cinfo_ = boost::shared_ptr(new camera_info_manager::CameraInfoManager(right_camera_nh_,"/stereo/right")); 323 | 324 | 325 | string left_yaml_file_path = "file://" + data_folder_path_ + "/calibration/left.yaml"; 326 | string right_yaml_file_path = "file://" + data_folder_path_ + "/calibration/right.yaml"; 327 | 328 | 329 | if(left_cinfo_->validateURL(left_yaml_file_path)){ 330 | left_cinfo_->loadCameraInfo(left_yaml_file_path); 331 | cout << "Success to load camera info" << endl; 332 | stereo_left_info_ = left_cinfo_->getCameraInfo(); 333 | } 334 | 335 | 336 | if(right_cinfo_->validateURL(right_yaml_file_path)){ 337 | right_cinfo_->loadCameraInfo(right_yaml_file_path); 338 | cout << "Success to load camera info" << endl; 339 | stereo_right_info_ = right_cinfo_->getCameraInfo(); 340 | } 341 | 342 | //load 14bit thermal camera info 343 | 344 | thermal_14bit_left_camera_nh_ = ros::NodeHandle(nh_,"thermal_14bit_left"); 345 | thermal_14bit_right_camera_nh_ = ros::NodeHandle(nh_,"thermal_14bit_right"); 346 | 347 | thermal_14bit_left_cinfo_ = boost::shared_ptr(new camera_info_manager::CameraInfoManager(left_camera_nh_,"/stereo_thermal_14bit/left")); 348 | thermal_14bit_right_cinfo_ = boost::shared_ptr(new camera_info_manager::CameraInfoManager(right_camera_nh_,"/stereo_thermal_14bit/right")); 349 | 350 | string thermal_14bit_left_yaml_file_path = "file://" + data_folder_path_ + "/calibration/thermal_14bit_left.yaml"; 351 | string thermal_14bit_right_yaml_file_path = "file://" + data_folder_path_ + "/calibration/thermal_14bit_right.yaml"; 352 | 353 | if(thermal_14bit_left_cinfo_->validateURL(thermal_14bit_left_yaml_file_path)){ 354 | thermal_14bit_left_cinfo_->loadCameraInfo(thermal_14bit_left_yaml_file_path); 355 | cout << "Success to load thermal camera info" << endl; 356 | stereo_thermal_14bit_left_info_ = thermal_14bit_left_cinfo_->getCameraInfo(); 357 | } 358 | 359 | if(thermal_14bit_right_cinfo_->validateURL(thermal_14bit_right_yaml_file_path)){ 360 | thermal_14bit_right_cinfo_->loadCameraInfo(thermal_14bit_right_yaml_file_path); 361 | cout << "Success to load thermal camera info" << endl; 362 | stereo_thermal_14bit_right_info_ = thermal_14bit_right_cinfo_->getCameraInfo(); 363 | } 364 | 365 | data_stamp_thread_.active_ = true; 366 | gps_thread_.active_ = true; 367 | inspva_thread_.active_ = true; 368 | imu_thread_.active_ = true; 369 | ouster_thread_.active_ = true; 370 | 371 | stereo_thread_.active_ = true; 372 | stereo_thermal_14bit_left_thread_.active_ = true; 373 | stereo_thermal_14bit_right_thread_.active_ = true; 374 | 375 | data_stamp_thread_.thread_ = std::thread(&ROSThread::DataStampThread,this); 376 | gps_thread_.thread_ = std::thread(&ROSThread::GpsThread,this); 377 | inspva_thread_.thread_ = std::thread(&ROSThread::InspvaThread,this); 378 | imu_thread_.thread_ = std::thread(&ROSThread::ImuThread,this); 379 | ouster_thread_.thread_ = std::thread(&ROSThread::OusterThread,this); 380 | stereo_thread_.thread_ = std::thread(&ROSThread::StereoThread,this); 381 | stereo_thermal_14bit_left_thread_.thread_ = std::thread(&ROSThread::StereoThermal14BitLeftThread,this); 382 | stereo_thermal_14bit_right_thread_.thread_ = std::thread(&ROSThread::StereoThermal14BitRightThread,this); 383 | } 384 | 385 | void ROSThread::DataStampThread() 386 | { 387 | auto stop_region_iter = stop_period_.begin(); 388 | 389 | for(auto iter = data_stamp_.begin() ; iter != data_stamp_.end() ; iter ++){ 390 | auto stamp = iter->first; 391 | 392 | while((stamp > (initial_data_stamp_+processed_stamp_))&&(data_stamp_thread_.active_ == true)){ 393 | if(processed_stamp_ == 0){ 394 | iter = data_stamp_.begin(); 395 | stop_region_iter = stop_period_.begin(); 396 | stamp = iter->first; 397 | } 398 | usleep(1); 399 | if(reset_process_stamp_flag_ == true) break; 400 | //wait for data publish 401 | } 402 | 403 | if(reset_process_stamp_flag_ == true){ 404 | auto target_stamp = processed_stamp_ + initial_data_stamp_; 405 | //set iter 406 | iter = data_stamp_.lower_bound(target_stamp); 407 | iter = prev(iter,1); 408 | //set stop region order 409 | auto new_stamp = iter->first; 410 | stop_region_iter = stop_period_.upper_bound(new_stamp); 411 | 412 | reset_process_stamp_flag_ = false; 413 | continue; 414 | } 415 | 416 | 417 | //check whether stop region or not 418 | if(stamp == stop_region_iter->first){ 419 | if(stop_skip_flag_ == true){ 420 | cout << "Skip stop section!!" << endl; 421 | iter = data_stamp_.find(stop_region_iter->second); //find stop region end 422 | iter = prev(iter,1); 423 | processed_stamp_ = stop_region_iter->second - initial_data_stamp_; 424 | } 425 | stop_region_iter++; 426 | if(stop_skip_flag_ == true){ 427 | continue; 428 | } 429 | } 430 | 431 | if(data_stamp_thread_.active_ == false) return; 432 | if(iter->second.compare("gps") == 0){ 433 | gps_thread_.push(stamp); 434 | gps_thread_.cv_.notify_all(); 435 | }else if(iter->second.compare("gps") == 0) 436 | { 437 | gps_thread_.push(stamp); 438 | gps_thread_.cv_.notify_all(); 439 | } 440 | else if(iter->second.compare("inspva") == 0){ 441 | inspva_thread_.push(stamp); 442 | inspva_thread_.cv_.notify_all(); 443 | }else if(iter->second.compare("imu") == 0){ 444 | imu_thread_.push(stamp); 445 | imu_thread_.cv_.notify_all(); 446 | }else if(iter->second.compare("ouster") == 0){ 447 | ouster_thread_.push(stamp); 448 | ouster_thread_.cv_.notify_all(); 449 | }else if(iter->second.compare("stereo") == 0 && stereo_active_ == true){ 450 | stereo_thread_.push(stamp); 451 | stereo_thread_.cv_.notify_all(); 452 | }else if(iter->second.compare("stereo_thermal_14_left") == 0 && stereo_thermal_14bit_left_active_ == true){ 453 | stereo_thermal_14bit_left_thread_.push(stamp); 454 | stereo_thermal_14bit_left_thread_.cv_.notify_all(); 455 | }else if(iter->second.compare("stereo_thermal_14_right") == 0 && stereo_thermal_14bit_right_active_ == true){ 456 | stereo_thermal_14bit_right_thread_.push(stamp); 457 | stereo_thermal_14bit_right_thread_.cv_.notify_all(); 458 | } 459 | stamp_show_count_++; 460 | if(stamp_show_count_ > 100){ 461 | stamp_show_count_ = 0; 462 | emit StampShow(stamp); 463 | } 464 | 465 | if(prev_clock_stamp_ == 0 || (stamp - prev_clock_stamp_) > 10000000){ 466 | rosgraph_msgs::Clock clock; 467 | clock.clock.fromNSec(stamp); 468 | clock_pub_.publish(clock); 469 | prev_clock_stamp_ = stamp; 470 | } 471 | 472 | if(loop_flag_ == true && iter == prev(data_stamp_.end(),1)){ 473 | iter = data_stamp_.begin(); 474 | stop_region_iter = stop_period_.begin(); 475 | processed_stamp_ = 0; 476 | } 477 | if(loop_flag_ == false && iter == prev(data_stamp_.end(),1)){ 478 | play_flag_ = false; 479 | while(!play_flag_){ 480 | iter = data_stamp_.begin(); 481 | stop_region_iter = stop_period_.begin(); 482 | processed_stamp_ = 0; 483 | usleep(10000); 484 | } 485 | } 486 | } 487 | cout << "Data publish complete" << endl; 488 | 489 | } 490 | 491 | void ROSThread::GpsThread() 492 | { 493 | while(1){ 494 | std::unique_lock ul(gps_thread_.mutex_); 495 | gps_thread_.cv_.wait(ul); 496 | if(gps_thread_.active_ == false) return; 497 | ul.unlock(); 498 | 499 | while(!gps_thread_.data_queue_.empty()){ 500 | auto data = gps_thread_.pop(); 501 | //process 502 | if(gps_data_.find(data) != gps_data_.end()){ 503 | gps_pub_.publish(gps_data_[data]); 504 | } 505 | } 506 | if(gps_thread_.active_ == false) return; 507 | } 508 | } 509 | void ROSThread::InspvaThread() 510 | { 511 | while(1){ 512 | std::unique_lock ul(inspva_thread_.mutex_); 513 | inspva_thread_.cv_.wait(ul); 514 | if(inspva_thread_.active_ == false) return; 515 | ul.unlock(); 516 | 517 | while(!inspva_thread_.data_queue_.empty()){ 518 | auto data = inspva_thread_.pop(); 519 | //process 520 | if(inspva_data_.find(data) != inspva_data_.end()){ 521 | inspva_pub_.publish(inspva_data_[data]); 522 | } 523 | 524 | } 525 | if(inspva_thread_.active_ == false) return; 526 | } 527 | } 528 | 529 | void ROSThread::ImuThread() 530 | { 531 | while(1){ 532 | std::unique_lock ul(imu_thread_.mutex_); 533 | imu_thread_.cv_.wait(ul); 534 | if(imu_thread_.active_ == false) return; 535 | ul.unlock(); 536 | 537 | while(!imu_thread_.data_queue_.empty()){ 538 | auto data = imu_thread_.pop(); 539 | //process 540 | if(imu_data_.find(data) != imu_data_.end()){ 541 | imu_pub_.publish(imu_data_[data]); 542 | imu_origin_pub_.publish(imu_data_origin_[data]); 543 | if(imu_data_version_ == 2){ 544 | magnet_pub_.publish(mag_data_[data]); 545 | } 546 | } 547 | 548 | } 549 | if(imu_thread_.active_ == false) return; 550 | } 551 | } 552 | 553 | void ROSThread::TimerCallback(const ros::TimerEvent&) 554 | { 555 | int64_t current_stamp = ros::Time::now().toNSec(); 556 | if(play_flag_ == true && pause_flag_ == false){ 557 | processed_stamp_ += static_cast(static_cast(current_stamp - pre_timer_stamp_) * play_rate_); 558 | } 559 | pre_timer_stamp_ = current_stamp; 560 | 561 | if(play_flag_ == false){ 562 | processed_stamp_ = 0; //reset 563 | prev_clock_stamp_ = 0; 564 | } 565 | } 566 | 567 | //ouster 568 | void ROSThread::OusterThread() 569 | { 570 | int current_file_index = 0; 571 | int previous_file_index = 0; 572 | while(1){ 573 | std::unique_lock ul(ouster_thread_.mutex_); 574 | ouster_thread_.cv_.wait(ul); 575 | if(ouster_thread_.active_ == false) return; 576 | ul.unlock(); 577 | 578 | while(!ouster_thread_.data_queue_.empty()){ 579 | auto data = ouster_thread_.pop(); 580 | //process 581 | 582 | //publish data 583 | if(to_string(data) + ".bin" == ouster_next_.first){ 584 | //publish 585 | ouster_next_.second.header.stamp.fromNSec(data); 586 | ouster_next_.second.header.frame_id = "ouster"; 587 | ouster_pub_.publish(ouster_next_.second); 588 | 589 | }else{ 590 | // cout << "Re-load right ouster from path" << endl; 591 | //load current data 592 | pcl::PointCloud cloud; 593 | cloud.clear(); 594 | sensor_msgs::PointCloud2 publish_cloud; 595 | string current_file_name = data_folder_path_ + "/sensor_data/ouster" +"/"+ to_string(data) + ".bin"; 596 | if(find(next(ouster_file_list_.begin(),max(0,previous_file_index-search_bound_)),ouster_file_list_.end(),to_string(data)+".bin") != ouster_file_list_.end()){ 597 | ifstream file; 598 | file.open(current_file_name, ios::in|ios::binary); 599 | while(!file.eof()){ 600 | pcl::PointXYZI point; 601 | file.read(reinterpret_cast(&point.x), sizeof(float)); 602 | file.read(reinterpret_cast(&point.y), sizeof(float)); 603 | file.read(reinterpret_cast(&point.z), sizeof(float)); 604 | file.read(reinterpret_cast(&point.intensity), sizeof(float)); 605 | cloud.points.push_back (point); 606 | } 607 | file.close(); 608 | 609 | pcl::toROSMsg(cloud, publish_cloud); 610 | publish_cloud.header.stamp.fromNSec(data); 611 | publish_cloud.header.frame_id = "ouster"; 612 | ouster_pub_.publish(publish_cloud); 613 | 614 | } 615 | previous_file_index = 0; 616 | } 617 | 618 | //load next data 619 | pcl::PointCloud cloud; 620 | cloud.clear(); 621 | sensor_msgs::PointCloud2 publish_cloud; 622 | current_file_index = find(next(ouster_file_list_.begin(),max(0,previous_file_index-search_bound_)),ouster_file_list_.end(),to_string(data)+".bin") - ouster_file_list_.begin(); 623 | if(find(next(ouster_file_list_.begin(),max(0,previous_file_index-search_bound_)),ouster_file_list_.end(),ouster_file_list_[current_file_index+1]) != ouster_file_list_.end()){ 624 | string next_file_name = data_folder_path_ + "/sensor_data/ouster" +"/"+ ouster_file_list_[current_file_index+1]; 625 | 626 | ifstream file; 627 | file.open(next_file_name, ios::in|ios::binary); 628 | while(!file.eof()){ 629 | pcl::PointXYZI point; 630 | file.read(reinterpret_cast(&point.x), sizeof(float)); 631 | file.read(reinterpret_cast(&point.y), sizeof(float)); 632 | file.read(reinterpret_cast(&point.z), sizeof(float)); 633 | file.read(reinterpret_cast(&point.intensity), sizeof(float)); 634 | cloud.points.push_back (point); 635 | } 636 | file.close(); 637 | pcl::toROSMsg(cloud, publish_cloud); 638 | ouster_next_ = make_pair(ouster_file_list_[current_file_index+1], publish_cloud); 639 | } 640 | 641 | previous_file_index = current_file_index; 642 | } 643 | if(ouster_thread_.active_ == false) return; 644 | } 645 | } //end ouster 646 | 647 | void ROSThread::StereoThread() 648 | { 649 | int current_img_index = 0; 650 | int previous_img_index = 0; 651 | 652 | while(1){ 653 | std::unique_lock ul(stereo_thread_.mutex_); 654 | stereo_thread_.cv_.wait(ul); 655 | if(stereo_thread_.active_ == false) return; 656 | ul.unlock(); 657 | 658 | while(!stereo_thread_.data_queue_.empty()){ 659 | auto data = stereo_thread_.pop(); 660 | //process 661 | if(stereo_file_list_.size() == 0) continue; 662 | 663 | //publish 664 | if(to_string(data)+".png" == stereo_left_next_img_.first && !stereo_left_next_img_.second.empty() && !stereo_right_next_img_.second.empty()){ 665 | cv_bridge::CvImage left_out_msg; 666 | left_out_msg.header.stamp.fromNSec(data); 667 | left_out_msg.header.frame_id = "stereo_left"; 668 | left_out_msg.encoding = sensor_msgs::image_encodings::BAYER_BGGR8; 669 | left_out_msg.image = stereo_left_next_img_.second; 670 | 671 | cv_bridge::CvImage right_out_msg; 672 | right_out_msg.header.stamp.fromNSec(data); 673 | right_out_msg.header.frame_id = "stereo_right"; 674 | right_out_msg.encoding = sensor_msgs::image_encodings::BAYER_BGGR8; 675 | right_out_msg.image = stereo_right_next_img_.second; 676 | 677 | stereo_left_info_.header.stamp.fromNSec(data); 678 | stereo_left_info_.header.frame_id = "/stereo/left"; 679 | stereo_right_info_.header.stamp.fromNSec(data); 680 | stereo_right_info_.header.frame_id = "/stereo/right"; 681 | 682 | stereo_left_pub_.publish(left_out_msg.toImageMsg()); 683 | stereo_right_pub_.publish(right_out_msg.toImageMsg()); 684 | 685 | stereo_left_info_pub_.publish(stereo_left_info_); 686 | stereo_right_info_pub_.publish(stereo_right_info_); 687 | 688 | }else{ 689 | // cout << "Re-load stereo image from image path" << endl; 690 | 691 | string current_stereo_left_name = data_folder_path_ + "/image/stereo_left" +"/"+ to_string(data)+".png"; 692 | string current_stereo_right_name = data_folder_path_ + "/image/stereo_right" +"/"+ to_string(data)+".png"; 693 | cv::Mat current_left_image; 694 | cv::Mat current_right_image; 695 | current_left_image = imread(current_stereo_left_name, CV_LOAD_IMAGE_ANYDEPTH); 696 | current_right_image = imread(current_stereo_right_name, CV_LOAD_IMAGE_ANYDEPTH); 697 | 698 | if(!current_left_image.empty() && !current_right_image.empty()){ 699 | 700 | cv_bridge::CvImage left_out_msg; 701 | left_out_msg.header.stamp.fromNSec(data); 702 | left_out_msg.header.frame_id = "stereo_left"; 703 | left_out_msg.encoding = sensor_msgs::image_encodings::BAYER_BGGR8; 704 | left_out_msg.image = current_left_image; 705 | 706 | cv_bridge::CvImage right_out_msg; 707 | right_out_msg.header.stamp.fromNSec(data); 708 | right_out_msg.header.frame_id = "stereo_right"; 709 | right_out_msg.encoding = sensor_msgs::image_encodings::BAYER_BGGR8; 710 | right_out_msg.image = current_right_image; 711 | 712 | stereo_left_info_.header.stamp.fromNSec(data); 713 | stereo_left_info_.header.frame_id = "/stereo/left"; 714 | stereo_right_info_.header.stamp.fromNSec(data); 715 | stereo_right_info_.header.frame_id = "/stereo/right"; 716 | 717 | stereo_left_pub_.publish(left_out_msg.toImageMsg()); 718 | stereo_right_pub_.publish(right_out_msg.toImageMsg()); 719 | 720 | stereo_left_info_pub_.publish(stereo_left_info_); 721 | stereo_right_info_pub_.publish(stereo_right_info_); 722 | } 723 | previous_img_index = 0; 724 | 725 | 726 | } 727 | 728 | //load next image 729 | current_img_index = find(next(stereo_file_list_.begin(), max(0,previous_img_index - search_bound_)),stereo_file_list_.end(),to_string(data)+".png") - stereo_file_list_.begin(); 730 | if(current_img_index < stereo_file_list_.size()-2){ 731 | 732 | string next_stereo_left_name = data_folder_path_ + "/image/stereo_left" +"/"+ stereo_file_list_[current_img_index+1]; 733 | string next_stereo_right_name = data_folder_path_ + "/image/stereo_right" +"/"+ stereo_file_list_[current_img_index+1]; 734 | cv::Mat next_left_image; 735 | cv::Mat next_right_image; 736 | next_left_image = imread(next_stereo_left_name, CV_LOAD_IMAGE_ANYDEPTH); 737 | next_right_image = imread(next_stereo_right_name, CV_LOAD_IMAGE_ANYDEPTH); 738 | if(!next_left_image.empty() && !next_right_image.empty()){ 739 | stereo_left_next_img_ = make_pair(stereo_file_list_[current_img_index+1], next_left_image); 740 | stereo_right_next_img_ = make_pair(stereo_file_list_[current_img_index+1], next_right_image); 741 | } 742 | 743 | } 744 | previous_img_index = current_img_index; 745 | } 746 | if(stereo_thread_.active_ == false) return; 747 | } 748 | } 749 | 750 | //14bit thermal Left Thread 751 | void ROSThread::StereoThermal14BitLeftThread() 752 | { 753 | int current_img_index = 0; 754 | int previous_img_index = 0; 755 | 756 | while(1){ 757 | std::unique_lock ul(stereo_thermal_14bit_left_thread_.mutex_); 758 | stereo_thermal_14bit_left_thread_.cv_.wait(ul); 759 | if(stereo_thermal_14bit_left_thread_.active_ == false) return; 760 | ul.unlock(); 761 | 762 | while(!stereo_thermal_14bit_left_thread_.data_queue_.empty()){ 763 | auto data = stereo_thermal_14bit_left_thread_.pop(); 764 | //process 765 | if(stereo_thermal_14bit_left_file_list_.size() == 0) continue; 766 | 767 | //publish 768 | if(to_string(data)+".png" == stereo_thermal_14bit_left_next_img_.first && !stereo_thermal_14bit_left_next_img_.second.empty()){ 769 | cv_bridge::CvImage left_out_msg; 770 | left_out_msg.header.stamp.fromNSec(data); 771 | left_out_msg.header.frame_id = "stereo_thermal_14bit_left"; 772 | left_out_msg.encoding = sensor_msgs::image_encodings::MONO16; 773 | left_out_msg.image = stereo_thermal_14bit_left_next_img_.second; 774 | 775 | stereo_thermal_14bit_left_info_.header.stamp.fromNSec(data); 776 | stereo_thermal_14bit_left_info_.header.frame_id = "/stereo_thermal_14bit/left"; 777 | 778 | 779 | 780 | stereo_thermal_14bit_left_pub_.publish(left_out_msg.toImageMsg()); 781 | stereo_thermal_14bit_left_info_pub_.publish(stereo_thermal_14bit_left_info_); 782 | 783 | }else{ 784 | // cout << "Re-load stereo image from image path" << endl; 785 | 786 | string current_stereo_thermal_14bit_left_name = data_folder_path_ + "/image/stereo_thermal_14_left" +"/"+ to_string(data)+".png"; 787 | cv::Mat current_left_image; 788 | current_left_image = imread(current_stereo_thermal_14bit_left_name, CV_LOAD_IMAGE_ANYDEPTH); 789 | 790 | if(!current_left_image.empty()){ 791 | 792 | cv_bridge::CvImage left_out_msg; 793 | left_out_msg.header.stamp.fromNSec(data); 794 | left_out_msg.header.frame_id = "stereo_thermal_14bit_left"; 795 | left_out_msg.encoding = sensor_msgs::image_encodings::MONO16; 796 | left_out_msg.image = current_left_image; 797 | 798 | stereo_thermal_14bit_left_info_.header.stamp.fromNSec(data); 799 | stereo_thermal_14bit_left_info_.header.frame_id = "/stereo_thermal_14bit/left"; 800 | stereo_thermal_14bit_left_pub_.publish(left_out_msg.toImageMsg()); 801 | stereo_thermal_14bit_left_info_pub_.publish(stereo_thermal_14bit_left_info_); 802 | } 803 | previous_img_index = 0; 804 | } 805 | 806 | //load next image 807 | current_img_index = find(next(stereo_thermal_14bit_left_file_list_.begin(), max(0,previous_img_index - search_bound_)),stereo_thermal_14bit_left_file_list_.end(),to_string(data)+".png") - stereo_thermal_14bit_left_file_list_.begin(); 808 | if(current_img_index < stereo_file_list_.size()-2){ 809 | string next_stereo_thermal_14bit_left_name = data_folder_path_ + "/image/stereo_thermal_14_left" +"/"+ stereo_thermal_14bit_left_file_list_[current_img_index+1]; 810 | cv::Mat next_left_image; 811 | next_left_image = imread(next_stereo_thermal_14bit_left_name, CV_LOAD_IMAGE_ANYDEPTH); 812 | if(!next_left_image.empty()){ 813 | stereo_thermal_14bit_left_next_img_ = make_pair(stereo_thermal_14bit_left_file_list_[current_img_index+1], next_left_image); 814 | } 815 | } 816 | previous_img_index = current_img_index; 817 | } 818 | if(stereo_thermal_14bit_left_thread_.active_ == false) return; 819 | } 820 | } 821 | //end thermal Left Thread 822 | 823 | //14bit thermal Right Thread 824 | void ROSThread::StereoThermal14BitRightThread() 825 | { 826 | int current_img_index = 0; 827 | int previous_img_index = 0; 828 | 829 | while(1){ 830 | std::unique_lock ul(stereo_thermal_14bit_right_thread_.mutex_); 831 | stereo_thermal_14bit_right_thread_.cv_.wait(ul); 832 | if(stereo_thermal_14bit_right_thread_.active_ == false) return; 833 | ul.unlock(); 834 | 835 | while(!stereo_thermal_14bit_right_thread_.data_queue_.empty()){ 836 | auto data = stereo_thermal_14bit_right_thread_.pop(); 837 | //process 838 | if(stereo_thermal_14bit_right_file_list_.size() == 0) continue; 839 | 840 | //publish 841 | if(to_string(data)+".png" == stereo_thermal_14bit_right_next_img_.first && !stereo_thermal_14bit_right_next_img_.second.empty()){ 842 | cv_bridge::CvImage right_out_msg; 843 | right_out_msg.header.stamp.fromNSec(data); 844 | right_out_msg.header.frame_id = "stereo_thermal_14bit_right"; 845 | right_out_msg.encoding = sensor_msgs::image_encodings::MONO16; 846 | right_out_msg.image = stereo_thermal_14bit_right_next_img_.second; 847 | 848 | stereo_thermal_14bit_right_info_.header.stamp.fromNSec(data); 849 | stereo_thermal_14bit_right_info_.header.frame_id = "/stereo_thermal_14bit/right"; 850 | 851 | stereo_thermal_14bit_right_pub_.publish(right_out_msg.toImageMsg()); 852 | stereo_thermal_14bit_right_info_pub_.publish(stereo_thermal_14bit_right_info_); 853 | 854 | }else{ 855 | // cout << "Re-load stereo image from image path" << endl; 856 | 857 | string current_stereo_thermal_14bit_right_name = data_folder_path_ + "/image/stereo_thermal_14_right" +"/"+ to_string(data)+".png"; 858 | cv::Mat current_right_image; 859 | current_right_image = imread(current_stereo_thermal_14bit_right_name, CV_LOAD_IMAGE_ANYDEPTH); 860 | 861 | if(!current_right_image.empty()){ 862 | 863 | cv_bridge::CvImage right_out_msg; 864 | right_out_msg.header.stamp.fromNSec(data); 865 | right_out_msg.header.frame_id = "stereo_thermal_14bit_right"; 866 | right_out_msg.encoding = sensor_msgs::image_encodings::MONO16; 867 | right_out_msg.image = current_right_image; 868 | 869 | stereo_thermal_14bit_right_info_.header.stamp.fromNSec(data); 870 | stereo_thermal_14bit_right_info_.header.frame_id = "/stereo_thermal_14bit/right"; 871 | stereo_thermal_14bit_right_pub_.publish(right_out_msg.toImageMsg()); 872 | stereo_thermal_14bit_right_info_pub_.publish(stereo_thermal_14bit_right_info_); 873 | } 874 | previous_img_index = 0; 875 | } 876 | 877 | //load next image 878 | current_img_index = find(next(stereo_thermal_14bit_right_file_list_.begin(), max(0,previous_img_index - search_bound_)),stereo_thermal_14bit_right_file_list_.end(),to_string(data)+".png") - stereo_thermal_14bit_right_file_list_.begin(); 879 | if(current_img_index < stereo_file_list_.size()-2){ 880 | string next_stereo_thermal_14bit_right_name = data_folder_path_ + "/image/stereo_thermal_14_right" +"/"+ stereo_thermal_14bit_right_file_list_[current_img_index+1]; 881 | cv::Mat next_right_image; 882 | next_right_image = imread(next_stereo_thermal_14bit_right_name, CV_LOAD_IMAGE_ANYDEPTH); 883 | if(!next_right_image.empty()){ 884 | stereo_thermal_14bit_right_next_img_ = make_pair(stereo_thermal_14bit_right_file_list_[current_img_index+1], next_right_image); 885 | } 886 | } 887 | previous_img_index = current_img_index; 888 | } 889 | if(stereo_thermal_14bit_right_thread_.active_ == false) return; 890 | } 891 | } 892 | //end thermal Right Thread 893 | 894 | int ROSThread::GetDirList(string dir, vector &files) 895 | { 896 | 897 | vector tmp_files; 898 | struct dirent **namelist; 899 | int n; 900 | n = scandir(dir.c_str(),&namelist, 0 , alphasort); 901 | if (n < 0) 902 | perror("scandir"); 903 | else { 904 | while (n--) { 905 | if(string(namelist[n]->d_name) != "." && string(namelist[n]->d_name) != ".."){ 906 | tmp_files.push_back(string(namelist[n]->d_name)); 907 | } 908 | free(namelist[n]); 909 | } 910 | free(namelist); 911 | } 912 | 913 | for(auto iter = tmp_files.rbegin() ; iter!= tmp_files.rend() ; iter++){ 914 | files.push_back(*iter); 915 | } 916 | return 0; 917 | } 918 | 919 | void ROSThread::FilePlayerStart(const std_msgs::BoolConstPtr& msg) 920 | { 921 | if(auto_start_flag_ == true){ 922 | cout << "File player auto start" << endl; 923 | usleep(1000000); 924 | play_flag_ = false; 925 | emit StartSignal(); 926 | } 927 | } 928 | 929 | void ROSThread::FilePlayerStop(const std_msgs::BoolConstPtr& msg) 930 | { 931 | cout << "File player auto stop" << endl; 932 | play_flag_ = true; 933 | emit StartSignal(); 934 | } 935 | void ROSThread::ResetProcessStamp(int position) 936 | { 937 | if(position > 0 && position < 10000){ 938 | processed_stamp_ = static_cast(static_cast(last_data_stamp_ - initial_data_stamp_)*static_cast(position)/static_cast(10000)); 939 | reset_process_stamp_flag_ = true; 940 | } 941 | } 942 | -------------------------------------------------------------------------------- /src/ROSThread.h: -------------------------------------------------------------------------------- 1 | #ifndef VIEWER_ROS_H 2 | #define VIEWER_ROS_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | 28 | #include 29 | #include 30 | #include 31 | #include 32 | 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | #include 48 | #include 49 | #include 50 | #include 51 | #include 52 | 53 | #include 54 | #include 55 | #include 56 | #include 57 | #include 58 | 59 | 60 | #include 61 | #include 62 | #include 63 | #include 64 | #include 65 | #include 66 | 67 | //pcl 68 | #include 69 | #include 70 | #include 71 | 72 | #include "file_player/color.h" 73 | #include "rosbag/bag.h" 74 | #include 75 | #include "file_player/datathread.h" 76 | #include 77 | 78 | #include 79 | #include 80 | #include 81 | #include 82 | #include 83 | #include 84 | #include 85 | #include 86 | 87 | 88 | 89 | using namespace std; 90 | using namespace cv; 91 | 92 | class ROSThread : public QThread 93 | { 94 | Q_OBJECT 95 | 96 | public: 97 | explicit ROSThread(QObject *parent = 0, QMutex *th_mutex = 0); 98 | ~ROSThread(); 99 | void ros_initialize(ros::NodeHandle &n); 100 | void run(); 101 | QMutex *mutex_; 102 | ros::NodeHandle nh_; 103 | ros::NodeHandle left_camera_nh_; 104 | ros::NodeHandle right_camera_nh_; 105 | 106 | ros::NodeHandle thermal_left_camera_nh_; 107 | ros::NodeHandle thermal_right_camera_nh_; 108 | 109 | ros::NodeHandle thermal_14bit_left_camera_nh_; 110 | ros::NodeHandle thermal_14bit_right_camera_nh_; 111 | 112 | boost::shared_ptr left_cinfo_; 113 | boost::shared_ptr right_cinfo_; 114 | 115 | boost::shared_ptr thermal_left_cinfo_; 116 | boost::shared_ptr thermal_right_cinfo_; 117 | 118 | boost::shared_ptr thermal_14bit_left_cinfo_; 119 | boost::shared_ptr thermal_14bit_right_cinfo_; 120 | 121 | int64_t initial_data_stamp_; 122 | int64_t last_data_stamp_; 123 | 124 | bool auto_start_flag_; 125 | int stamp_show_count_; 126 | 127 | bool play_flag_; 128 | bool pause_flag_; 129 | bool loop_flag_; 130 | bool stop_skip_flag_; 131 | double play_rate_; 132 | string data_folder_path_; 133 | 134 | int imu_data_version_; 135 | 136 | void Ready(); 137 | void ResetProcessStamp(int position); 138 | 139 | signals: 140 | void StampShow(quint64 stamp); 141 | void StartSignal(); 142 | 143 | private: 144 | 145 | int search_bound_; 146 | 147 | bool stereo_active_; 148 | bool stereo_thermal_active_; 149 | bool stereo_thermal_14bit_left_active_; 150 | bool stereo_thermal_14bit_right_active_; 151 | 152 | bool omni_active_; 153 | 154 | ros::Subscriber start_sub_; 155 | ros::Subscriber stop_sub_; 156 | 157 | ros::Publisher gps_pub_; 158 | ros::Publisher inspva_pub_; 159 | 160 | ros::Publisher gps_odometry_pub_; 161 | ros::Publisher imu_origin_pub_; 162 | ros::Publisher imu_pub_; 163 | ros::Publisher magnet_pub_; 164 | ros::Publisher ouster_pub_; 165 | 166 | ros::Publisher stereo_left_pub_; 167 | ros::Publisher stereo_right_pub_; 168 | ros::Publisher stereo_thermal_14bit_left_pub_; 169 | ros::Publisher stereo_thermal_14bit_right_pub_; 170 | 171 | ros::Publisher stereo_left_info_pub_; 172 | ros::Publisher stereo_right_info_pub_; 173 | 174 | ros::Publisher stereo_thermal_left_info_pub_; 175 | ros::Publisher stereo_thermal_right_info_pub_; 176 | ros::Publisher stereo_thermal_14bit_left_info_pub_; 177 | ros::Publisher stereo_thermal_14bit_right_info_pub_; 178 | 179 | ros::Publisher clock_pub_; 180 | 181 | int64_t prev_clock_stamp_; 182 | 183 | multimap data_stamp_; 184 | map odometry_data_; 185 | map gps_data_; 186 | map inspva_data_; 187 | map gps_odometry_data_; 188 | 189 | map imu_data_origin_; 190 | map imu_data_; 191 | map mag_data_; 192 | 193 | DataThread data_stamp_thread_; 194 | DataThread gps_thread_; 195 | DataThread inspva_thread_; 196 | DataThread imu_thread_; 197 | DataThread stereo_thread_; 198 | DataThread stereo_thermal_14bit_left_thread_; 199 | DataThread stereo_thermal_14bit_right_thread_; 200 | DataThread ouster_thread_; 201 | 202 | map stop_period_; //start and stop stamp 203 | 204 | void DataStampThread(); 205 | void GpsThread(); 206 | void InspvaThread(); 207 | void ImuThread(); 208 | void OusterThread(); 209 | 210 | void StereoThread(); 211 | void StereoThermal14BitLeftThread(); 212 | void StereoThermal14BitRightThread(); 213 | 214 | void FilePlayerStart(const std_msgs::BoolConstPtr& msg); 215 | void FilePlayerStop(const std_msgs::BoolConstPtr& msg); 216 | 217 | vector ouster_file_list_; 218 | vector stereo_file_list_; 219 | vector stereo_thermal_14bit_left_file_list_; 220 | vector stereo_thermal_14bit_right_file_list_; 221 | 222 | ros::Timer timer_; 223 | void TimerCallback(const ros::TimerEvent&); 224 | int64_t processed_stamp_; 225 | int64_t pre_timer_stamp_; 226 | bool reset_process_stamp_flag_; 227 | 228 | pair ouster_next_; 229 | 230 | pair stereo_left_next_img_; 231 | pair stereo_right_next_img_; 232 | pair stereo_thermal_14bit_left_next_img_; 233 | pair stereo_thermal_14bit_right_next_img_; 234 | 235 | sensor_msgs::CameraInfo stereo_left_info_; 236 | sensor_msgs::CameraInfo stereo_right_info_; 237 | sensor_msgs::CameraInfo stereo_thermal_14bit_left_info_; 238 | sensor_msgs::CameraInfo stereo_thermal_14bit_right_info_; 239 | 240 | int GetDirList(string dir, vector &files); 241 | 242 | 243 | public slots: 244 | 245 | }; 246 | 247 | #endif // VIEWER_LCM_H 248 | -------------------------------------------------------------------------------- /src/color_code.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | float hsv[64][3]= 4 | {{1 ,0 ,0 }, 5 | {1 ,0.09375,0 }, 6 | {1 ,0.18750,0 }, 7 | {1 ,0.28125,0 }, 8 | {1 ,0.37500,0 }, 9 | {1 ,0.46875,0 }, 10 | {1 ,0.56250,0 }, 11 | {1 ,0.65625,0 }, 12 | {1 ,0.75000,0 }, 13 | {1 ,0.84375,0 }, 14 | {1 ,0.93750,0 }, 15 | {0.96875,1 ,0 }, 16 | {0.87500,1 ,0 }, 17 | {0.78125,1 ,0 }, 18 | {0.68750,1 ,0 }, 19 | {0.59375,1 ,0 }, 20 | {0.50000,1 ,0 }, 21 | {0.40625,1 ,0 }, 22 | {0.31250,1 ,0 }, 23 | {0.21875,1 ,0 }, 24 | {0.12500,1 ,0 }, 25 | {0.03125,1 ,0 }, 26 | {0 ,1 ,0.06250}, 27 | {0 ,1 ,0.15625}, 28 | {0 ,1 ,0.25000}, 29 | {0 ,1 ,0.34375}, 30 | {0 ,1 ,0.43750}, 31 | {0 ,1 ,0.53125}, 32 | {0 ,1 ,0.62500}, 33 | {0 ,1 ,0.71875}, 34 | {0 ,1 ,0.81250}, 35 | {0 ,1 ,0.90625}, 36 | {0 ,1 ,1 }, 37 | {0 ,0.90625,1 }, 38 | {0 ,0.81250,1 }, 39 | {0 ,0.71875,1 }, 40 | {0 ,0.62500,1 }, 41 | {0 ,0.53125,1 }, 42 | {0 ,0.43750,1 }, 43 | {0 ,0.34375,1 }, 44 | {0 ,0.25000,1 }, 45 | {0 ,0.15625,1 }, 46 | {0 ,0.06250,1 }, 47 | {0.03125,0 ,1 }, 48 | {0.12500,0 ,1 }, 49 | {0.21875,0 ,1 }, 50 | {0.31250,0 ,1 }, 51 | {0.40625,0 ,1 }, 52 | {0.50000,0 ,1 }, 53 | {0.59375,0 ,1 }, 54 | {0.68750,0 ,1 }, 55 | {0.78125,0 ,1 }, 56 | {0.87500,0 ,1 }, 57 | {0.96875,0 ,1 }, 58 | {1 ,0 ,0.93750}, 59 | {1 ,0 ,0.84375}, 60 | {1 ,0 ,0.75000}, 61 | {1 ,0 ,0.65625}, 62 | {1 ,0 ,0.56250}, 63 | {1 ,0 ,0.46875}, 64 | {1 ,0 ,0.37500}, 65 | {1 ,0 ,0.28125}, 66 | {1 ,0 ,0.18750}, 67 | {1 ,0 ,0.09375}}; 68 | 69 | float jet[64][3]= 70 | {{0 ,0 ,0.5625}, 71 | {0 ,0 ,0.6250}, 72 | {0 ,0 ,0.6875}, 73 | {0 ,0 ,0.7500}, 74 | {0 ,0 ,0.8125}, 75 | {0 ,0 ,0.8750}, 76 | {0 ,0 ,0.9375}, 77 | {0 ,0 ,1.0000}, 78 | {0 ,0.0625 ,1.0000}, 79 | {0 ,0.1250 ,1.0000}, 80 | {0 ,0.1875 ,1.0000}, 81 | {0 ,0.2500 ,1.0000}, 82 | {0 ,0.3125 ,1.0000}, 83 | {0 ,0.3750 ,1.0000}, 84 | {0 ,0.4375 ,1.0000}, 85 | {0 ,0.5000 ,1.0000}, 86 | {0 ,0.5625 ,1.0000}, 87 | {0 ,0.6250 ,1.0000}, 88 | {0 ,0.6875 ,1.0000}, 89 | {0 ,0.7500 ,1.0000}, 90 | {0 ,0.8125 ,1.0000}, 91 | {0 ,0.8750 ,1.0000}, 92 | {0 ,0.9375 ,1.0000}, 93 | {0 ,1.0000 ,1.0000}, 94 | {0.0625 ,1.0000 ,0.9375}, 95 | {0.1250 ,1.0000 ,0.8750}, 96 | {0.1875 ,1.0000 ,0.8125}, 97 | {0.2500 ,1.0000 ,0.7500}, 98 | {0.3125 ,1.0000 ,0.6875}, 99 | {0.3750 ,1.0000 ,0.6250}, 100 | {0.4375 ,1.0000 ,0.5625}, 101 | {0.5000 ,1.0000 ,0.5000}, 102 | {0.5625 ,1.0000 ,0.4375}, 103 | {0.6250 ,1.0000 ,0.3750}, 104 | {0.6875 ,1.0000 ,0.3125}, 105 | {0.7500 ,1.0000 ,0.2500}, 106 | {0.8125 ,1.0000 ,0.1875}, 107 | {0.8750 ,1.0000 ,0.1250}, 108 | {0.9375 ,1.0000 ,0.0625}, 109 | {1.0000 ,1.0000 ,0 }, 110 | {1.0000 ,0.9375 ,0 }, 111 | {1.0000 ,0.8750 ,0 }, 112 | {1.0000 ,0.8125 ,0 }, 113 | {1.0000 ,0.7500 ,0 }, 114 | {1.0000 ,0.6875 ,0 }, 115 | {1.0000 ,0.6250 ,0 }, 116 | {1.0000 ,0.5625 ,0 }, 117 | {1.0000 ,0.5000 ,0 }, 118 | {1.0000 ,0.4375 ,0 }, 119 | {1.0000 ,0.3750 ,0 }, 120 | {1.0000 ,0.3125 ,0 }, 121 | {1.0000 ,0.2500 ,0 }, 122 | {1.0000 ,0.1875 ,0 }, 123 | {1.0000 ,0.1250 ,0 }, 124 | {1.0000 ,0.0625 ,0 }, 125 | {1.0000 ,0 ,0 }, 126 | {0.9375 ,0 ,0 }, 127 | {0.8750 ,0 ,0 }, 128 | {0.8125 ,0 ,0 }, 129 | {0.7500 ,0 ,0 }, 130 | {0.6875 ,0 ,0 }, 131 | {0.6250 ,0 ,0 }, 132 | {0.5625 ,0 ,0 }, 133 | {0.5000 ,0 ,0 }}; 134 | 135 | float randCol[64][3]= 136 | {{0 ,0 ,0.5625}, 137 | {0 ,0 ,0.6250}, 138 | {0 ,0.5625 ,1.0000}, 139 | {0.0625 ,1.0000 ,0.9375}, 140 | {0.6250 ,1.0000 ,0.3750}, 141 | {0.5000 ,1.0000 ,0.5000}, 142 | {1.0000 ,0.3750 ,0 }, 143 | {1.0000 ,0.9375 ,0 }, 144 | {0.8125 ,1.0000 ,0.1875}, 145 | {0.6875 ,1.0000 ,0.3125}, 146 | {0.9375 ,1.0000 ,0.0625}, 147 | {0.2500 ,1.0000 ,0.7500}, 148 | {0 ,0.3750 ,1.0000}, 149 | {0 ,0 ,0.6875}, 150 | {0.1250 ,1.0000 ,0.8750}, 151 | {0.5625 ,1.0000 ,0.4375}, 152 | {1.0000 ,0.7500 ,0 }, 153 | {1.0000 ,0 ,0 }, 154 | {0 ,0.4375 ,1.0000}, 155 | {0.8750 ,1.0000 ,0.1250}, 156 | {0.3125 ,1.0000 ,0.6875}, 157 | {0.4375 ,1.0000 ,0.5625}, 158 | {0 ,0 ,0.8750}, 159 | {0.1875 ,1.0000 ,0.8125}, 160 | {0.6250 ,0 ,0 }, 161 | {1.0000 ,0.3125 ,0 }, 162 | {0 ,0 ,0.9375}, 163 | {0 ,0 ,1.0000}, 164 | {1.0000 ,0.8750 ,0 }, 165 | {0 ,0.5000 ,1.0000}, 166 | {0 ,0.6875 ,1.0000}, 167 | {1.0000 ,0.6250 ,0 }, 168 | {1.0000 ,0.5000 ,0 }, 169 | {0 ,0.6250 ,1.0000}, 170 | {1.0000 ,0.6875 ,0 }, 171 | {0 ,0.1250 ,1.0000}, 172 | {1.0000 ,0.5625 ,0 }, 173 | {0.9375 ,0 ,0 }, 174 | {1.0000 ,1.0000 ,0 }, 175 | {0 ,0.1875 ,1.0000}, 176 | {0 ,0.2500 ,1.0000}, 177 | {0 ,0.3125 ,1.0000}, 178 | {1.0000 ,0.4375 ,0 }, 179 | {0.8750 ,0 ,0 }, 180 | {0.8125 ,0 ,0 }, 181 | {0.7500 ,0 ,0 }, 182 | {0.7500 ,1.0000 ,0.2500}, 183 | {0 ,0.0625 ,1.0000}, 184 | {0 ,0.7500 ,1.0000}, 185 | {1.0000 ,0.8125 ,0 }, 186 | {0 ,0.8125 ,1.0000}, 187 | {0 ,0.8750 ,1.0000}, 188 | {0 ,0.9375 ,1.0000}, 189 | {0 ,1.0000 ,1.0000}, 190 | {0.3750 ,1.0000 ,0.6250}, 191 | {1.0000 ,0.1875 ,0 }, 192 | {0.6875 ,0 ,0 }, 193 | {1.0000 ,0.2500 ,0 }, 194 | {0 ,0 ,0.7500}, 195 | {1.0000 ,0.1250 ,0 }, 196 | {0 ,0 ,0.8125}, 197 | {1.0000 ,0.0625 ,0 }, 198 | {0.5625 ,0 ,0 }, 199 | {0.5000 ,0 ,0 }}; 200 | -------------------------------------------------------------------------------- /src/main.cpp: -------------------------------------------------------------------------------- 1 | #include "mainwindow.h" 2 | #include 3 | //#include 4 | #include 5 | #include 6 | #include 7 | //#include 8 | #include 9 | #include 10 | 11 | int main(int argc, char *argv[]) 12 | { 13 | ros::init(argc, argv, "file_player"); 14 | ros::NodeHandle nh; 15 | 16 | QApplication a(argc, argv); 17 | MainWindow w; 18 | w.RosInit(nh); 19 | w.show(); 20 | 21 | return a.exec(); 22 | } 23 | -------------------------------------------------------------------------------- /src/mainwindow.cpp: -------------------------------------------------------------------------------- 1 | #include "mainwindow.h" 2 | #include "ui_mainwindow.h" 3 | 4 | using namespace std; 5 | 6 | MainWindow::MainWindow(QWidget *parent) : 7 | QMainWindow(parent), 8 | ui_(new Ui::MainWindow) 9 | { 10 | my_ros_ = new ROSThread(this, &mutex); 11 | ui_->setupUi(this); 12 | my_ros_->start(); 13 | 14 | slider_checker_ = false; 15 | play_flag_ = false; 16 | pause_flag_ = false; 17 | loop_flag_ = false; 18 | stop_skip_flag_ = true; 19 | 20 | connect(my_ros_, SIGNAL(StampShow(quint64)), this, SLOT(SetStamp(quint64))); 21 | connect(my_ros_, SIGNAL(StartSignal()), this, SLOT(Play())); 22 | 23 | connect(ui_->quitButton, SIGNAL(pressed()), this, SLOT(TryClose())); 24 | connect(ui_->pushButton, SIGNAL(pressed()), this, SLOT(FilePathSet())); 25 | connect(ui_->pushButton_2, SIGNAL(pressed()), this, SLOT(Play())); 26 | connect(ui_->pushButton_3, SIGNAL(pressed()), this, SLOT(Pause())); 27 | 28 | connect(ui_->doubleSpinBox, SIGNAL(valueChanged(double)), this, SLOT(PlaySpeedChange(double))); 29 | ui_->doubleSpinBox->setRange(0.01,20.0); 30 | ui_->doubleSpinBox->setValue(1.0); 31 | ui_->doubleSpinBox->setSingleStep(0.01); 32 | connect(ui_->checkBox, SIGNAL(stateChanged(int)), this, SLOT (LoopFlagChange(int))); 33 | if(loop_flag_ == true){ 34 | ui_->checkBox->setCheckState(Qt::Checked); 35 | }else{ 36 | ui_->checkBox->setCheckState(Qt::Unchecked); 37 | } 38 | connect(ui_->checkBox_2, SIGNAL(stateChanged(int)), this, SLOT (StopSkipFlagChange(int))); 39 | if(stop_skip_flag_ == true){ 40 | ui_->checkBox_2->setCheckState(Qt::Checked); 41 | }else{ 42 | ui_->checkBox_2->setCheckState(Qt::Unchecked); 43 | } 44 | connect(ui_->checkBox_3, SIGNAL(stateChanged(int)), this, SLOT (AutoStartFlagChange(int))); 45 | if(my_ros_->auto_start_flag_ == true){ 46 | ui_->checkBox_3->setCheckState(Qt::Checked); 47 | }else{ 48 | ui_->checkBox_3->setCheckState(Qt::Unchecked); 49 | } 50 | 51 | connect(ui_->horizontalSlider, SIGNAL(sliderPressed()), this, SLOT(SliderPressed())); 52 | connect(ui_->horizontalSlider, SIGNAL(valueChanged(int)), this, SLOT(SliderValueChange(int))); 53 | connect(ui_->horizontalSlider, SIGNAL(sliderReleased()), this, SLOT(SliderValueApply())); 54 | 55 | ui_->horizontalSlider->setRange(0,10000); 56 | ui_->horizontalSlider->setValue(0); 57 | slider_value_ = 0; 58 | 59 | } 60 | 61 | MainWindow::~MainWindow() 62 | { 63 | emit setThreadFinished(true); //Tell the thread to finish 64 | delete ui_; 65 | my_ros_->quit(); 66 | if(!my_ros_->wait(500)) //Wait until it actually has terminated (max. 3 sec) 67 | { 68 | my_ros_->terminate(); //Thread didn't exit in time, probably deadlocked, terminate it! 69 | my_ros_->wait(); //We have to wait again here! 70 | } 71 | } 72 | 73 | 74 | void MainWindow::RosInit(ros::NodeHandle &n) 75 | { 76 | my_ros_->ros_initialize(n); 77 | } 78 | 79 | 80 | void MainWindow::TryClose() 81 | { 82 | close(); 83 | } 84 | 85 | 86 | void MainWindow::FilePathSet() 87 | { 88 | play_flag_ = false; 89 | my_ros_->play_flag_ = false; 90 | this->ui_->pushButton_2->setText(QString::fromStdString("Play")); 91 | 92 | pause_flag_ = false; 93 | my_ros_->pause_flag_ = false; 94 | this->ui_->pushButton_3->setText(QString::fromStdString("Pause")); 95 | 96 | QFileDialog dialog; 97 | this->ui_->label->setText("Data is beging loaded....."); 98 | data_folder_path_ = dialog.getExistingDirectory(); 99 | my_ros_->data_folder_path_ = data_folder_path_.toUtf8().constData(); 100 | 101 | my_ros_->Ready(); 102 | 103 | this->ui_->label->setText(data_folder_path_); 104 | 105 | } 106 | 107 | void MainWindow::SetStamp(quint64 stamp) 108 | { 109 | this->ui_->label_2->setText(QString::number(stamp)); 110 | 111 | //set slide bar 112 | if(slider_checker_ == false){ 113 | ui_->horizontalSlider->setValue(static_cast(static_cast(stamp - my_ros_->initial_data_stamp_)/static_cast(my_ros_->last_data_stamp_ - my_ros_->initial_data_stamp_)*10000)); 114 | } 115 | } 116 | 117 | void MainWindow::Play() 118 | { 119 | if(my_ros_->play_flag_ == false){ 120 | play_flag_ = true; 121 | my_ros_->play_flag_ = true; 122 | this->ui_->pushButton_2->setText(QString::fromStdString("End")); 123 | 124 | pause_flag_ = false; 125 | my_ros_->pause_flag_ = false; 126 | this->ui_->pushButton_3->setText(QString::fromStdString("Pause")); 127 | }else{ 128 | play_flag_ = false; 129 | my_ros_->play_flag_ = false; 130 | this->ui_->pushButton_2->setText(QString::fromStdString("Play")); 131 | } 132 | } 133 | 134 | void MainWindow::Pause() 135 | { 136 | if(pause_flag_ == false){ 137 | pause_flag_ = true; 138 | my_ros_->pause_flag_ = true; 139 | this->ui_->pushButton_3->setText(QString::fromStdString("Resume")); 140 | }else{ 141 | pause_flag_ = false; 142 | my_ros_->pause_flag_ = false; 143 | this->ui_->pushButton_3->setText(QString::fromStdString("Pause")); 144 | } 145 | } 146 | 147 | 148 | void MainWindow::PlaySpeedChange(double value) 149 | { 150 | my_ros_->play_rate_ = value; 151 | } 152 | 153 | void MainWindow::LoopFlagChange(int value) 154 | { 155 | if(value == 2){ 156 | loop_flag_ = true; 157 | my_ros_->loop_flag_ = true; 158 | }else if(value == 0){ 159 | loop_flag_ = false; 160 | my_ros_->loop_flag_ = false; 161 | } 162 | } 163 | void MainWindow::StopSkipFlagChange(int value) 164 | { 165 | if(value == 2){ 166 | stop_skip_flag_ = true; 167 | my_ros_->stop_skip_flag_ = true; 168 | }else if(value == 0){ 169 | stop_skip_flag_ = false; 170 | my_ros_->stop_skip_flag_ = false; 171 | } 172 | } 173 | void MainWindow::AutoStartFlagChange(int value) 174 | { 175 | if(value == 2){ 176 | my_ros_->auto_start_flag_ = true; 177 | }else if(value == 0){ 178 | my_ros_->auto_start_flag_ = false; 179 | } 180 | } 181 | void MainWindow::SliderValueChange(int value) 182 | { 183 | slider_value_ = value; 184 | } 185 | 186 | void MainWindow::SliderPressed() 187 | { 188 | slider_checker_ = true; 189 | } 190 | 191 | void MainWindow::SliderValueApply() 192 | { 193 | my_ros_->ResetProcessStamp(slider_value_); 194 | slider_checker_ = false; 195 | } 196 | -------------------------------------------------------------------------------- /src/mainwindow.h: -------------------------------------------------------------------------------- 1 | #ifndef MAINWINDOW_H 2 | #define MAINWINDOW_H 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include "ROSThread.h" 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #define R2D 180/PI 26 | #define D2R PI/180 27 | #define POWER_CTR_DELAY 200000 28 | #define INTENSITY_MIN 0.0 29 | #define INTENSITY_MAX 100.0 30 | #define INTENSITY_COLOR_MIN 0.0 31 | #define INTENSITY_COLOR_MAX 1.0 32 | 33 | using namespace std; 34 | 35 | extern QMutex mutex; 36 | 37 | namespace Ui { 38 | class MainWindow; 39 | } 40 | 41 | class MainWindow : public QMainWindow 42 | { 43 | Q_OBJECT 44 | 45 | public: 46 | 47 | explicit MainWindow(QWidget *parent = 0); 48 | ~MainWindow(); 49 | void RosInit(ros::NodeHandle &n); 50 | 51 | private slots: 52 | void TryClose(); 53 | void FilePathSet(); 54 | void Play(); 55 | void Pause(); 56 | void PlaySpeedChange(double value); 57 | void LoopFlagChange(int value); 58 | void StopSkipFlagChange(int value); 59 | void AutoStartFlagChange(int value); 60 | void SetStamp(quint64 stamp); 61 | void SliderValueChange(int value); 62 | void SliderPressed(); 63 | void SliderValueApply(); 64 | 65 | signals: 66 | void setThreadFinished(bool); 67 | private: 68 | QMutex mutex; 69 | ROSThread *my_ros_; 70 | Ui::MainWindow *ui_; 71 | QString data_folder_path_; 72 | bool play_flag_; 73 | bool pause_flag_; 74 | bool loop_flag_; 75 | bool stop_skip_flag_; 76 | int slider_value_; 77 | 78 | int slider_checker_; 79 | 80 | }; 81 | 82 | #endif // MAINWINDOW_H 83 | -------------------------------------------------------------------------------- /src/mainwindow.ui: -------------------------------------------------------------------------------- 1 | 2 | 3 | MainWindow 4 | 5 | 6 | 7 | 0 8 | 0 9 | 670 10 | 189 11 | 12 | 13 | 14 | File Player 15 | 16 | 17 | 2 18 | 19 | 20 | Qt::LeftToRight 21 | 22 | 23 | QMainWindow{ 24 | background-color: rgb(50, 50, 50) 25 | } 26 | 27 | QMenuBar{ 28 | background-color: rgb(50, 50, 50) 29 | } 30 | 31 | QPushButton:pressed { 32 | background-color: rgb(90, 90, 90); 33 | color: rgb(210,210,210); 34 | } 35 | QPushButton { 36 | background-color: rgb(76, 76, 76); 37 | color: rgb(210,210,210); 38 | } 39 | 40 | QPushButton:disabled { 41 | background-color: rgb(76, 76, 76); 42 | color: rgb(150,150,150); 43 | } 44 | 45 | QLabel{ 46 | color: rgb(210,210,210); 47 | } 48 | 49 | QComboBox { 50 | background-color: rgb(150,150,150) 51 | } 52 | 53 | QCheckBox { 54 | border: none; 55 | color: rgb(210,210,210); 56 | } 57 | 58 | QFrame#line,#line_2,#line_3,#line_4,#line_5,#line_6,#line_7,#line_8,#line_9,#line_10{ 59 | background-color: rgb(70,70,70); 60 | } 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 0 73 | 0 74 | 75 | 76 | 77 | 78 | 300 79 | 15 80 | 81 | 82 | 83 | Urban Mapping File Player 84 | 85 | 86 | 87 | 88 | 89 | 90 | Qt::Horizontal 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 0 118 | 0 119 | 120 | 121 | 122 | 123 | 100 124 | 25 125 | 126 | 127 | 128 | Play 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 0 137 | 0 138 | 139 | 140 | 141 | 142 | 100 143 | 16777215 144 | 145 | 146 | 147 | Pause 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 0 156 | 0 157 | 158 | 159 | 160 | 161 | 205 162 | 25 163 | 164 | 165 | 166 | Load 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | Qt::Horizontal 176 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | Loop 185 | 186 | 187 | 188 | 189 | 190 | 191 | Skip stop section 192 | 193 | 194 | 195 | 196 | 197 | 198 | Auto start 199 | 200 | 201 | 202 | 203 | 204 | 205 | Qt::Horizontal 206 | 207 | 208 | QSizePolicy::Fixed 209 | 210 | 211 | 212 | 200 213 | 20 214 | 215 | 216 | 217 | 218 | 219 | 220 | 221 | Speed 222 | 223 | 224 | 225 | 226 | 227 | 228 | 229 | 230 | 231 | 232 | 233 | Qt::Horizontal 234 | 235 | 236 | 237 | 238 | 239 | 240 | 241 | 242 | 243 | 0 244 | 0 245 | 246 | 247 | 248 | 249 | 521 250 | 0 251 | 252 | 253 | 254 | Qt::Horizontal 255 | 256 | 257 | 258 | 259 | 260 | 261 | Qt::Horizontal 262 | 263 | 264 | QSizePolicy::Preferred 265 | 266 | 267 | 268 | 1000 269 | 20 270 | 271 | 272 | 273 | 274 | 275 | 276 | 277 | 278 | 0 279 | 0 280 | 281 | 282 | 283 | 284 | 100 285 | 16777215 286 | 287 | 288 | 289 | Quit 290 | 291 | 292 | 293 | 294 | 295 | 296 | 297 | 298 | 299 | TopToolBarArea 300 | 301 | 302 | false 303 | 304 | 305 | 306 | 307 | 308 | 309 | 310 | -------------------------------------------------------------------------------- /src/resources.qrc: -------------------------------------------------------------------------------- 1 | 2 | 3 | resources/fragmentShader.fsh 4 | resources/fragmentShaderColor.fsh 5 | resources/fragmentShaderOBJ.fsh 6 | resources/vertexShader.vsh 7 | resources/vertexShaderColor.vsh 8 | resources/vertexShaderOBJ.vsh 9 | 10 | 11 | -------------------------------------------------------------------------------- /src/resources/fragmentShader.fsh: -------------------------------------------------------------------------------- 1 | #version 130 2 | 3 | //! [0] 4 | uniform vec4 color; 5 | 6 | out vec4 fragColor; 7 | 8 | void main(void) 9 | { 10 | fragColor = color; 11 | } 12 | //! [0] 13 | -------------------------------------------------------------------------------- /src/resources/fragmentShaderColor.fsh: -------------------------------------------------------------------------------- 1 | #version 130 2 | 3 | //! [0] 4 | in vec4 varyingColor; 5 | 6 | out vec4 fragColor; 7 | 8 | void main(void) 9 | { 10 | fragColor = varyingColor; 11 | } 12 | //! [0] 13 | -------------------------------------------------------------------------------- /src/resources/fragmentShaderOBJ.fsh: -------------------------------------------------------------------------------- 1 | #version 130 2 | 3 | //! [0] 4 | uniform sampler2D texture; 5 | 6 | in vec2 varyingTextureCoordinate; 7 | 8 | out vec4 fragColor; 9 | 10 | void main(void) 11 | { 12 | fragColor = texture2D(texture, varyingTextureCoordinate); 13 | } 14 | //! [0] 15 | -------------------------------------------------------------------------------- /src/resources/vertexShader.vsh: -------------------------------------------------------------------------------- 1 | #version 130 2 | 3 | //! [0] 4 | uniform mat4 mvpMatrix; 5 | 6 | in vec4 vertex; 7 | 8 | void main(void) 9 | { 10 | gl_Position = mvpMatrix * vertex; 11 | 12 | } 13 | //! [0] 14 | -------------------------------------------------------------------------------- /src/resources/vertexShaderColor.vsh: -------------------------------------------------------------------------------- 1 | #version 130 2 | 3 | //! [0] 4 | uniform mat4 mvpMatrix; 5 | 6 | in vec4 vertex; 7 | in vec4 color; 8 | 9 | out vec4 varyingColor; 10 | 11 | void main(void) 12 | { 13 | varyingColor = color; 14 | gl_Position = mvpMatrix * vertex; 15 | } 16 | //! [0] 17 | -------------------------------------------------------------------------------- /src/resources/vertexShaderOBJ.vsh: -------------------------------------------------------------------------------- 1 | #version 130 2 | 3 | //! [0] 4 | uniform mat4 mvpMatrix; 5 | 6 | in vec4 vertex; 7 | in vec2 textureCoordinate; 8 | 9 | out vec2 varyingTextureCoordinate; 10 | 11 | void main(void) 12 | { 13 | varyingTextureCoordinate = textureCoordinate; 14 | gl_Position = mvpMatrix * vertex; 15 | } 16 | //! [0] 17 | -------------------------------------------------------------------------------- /src/test.cpp: -------------------------------------------------------------------------------- 1 | // Example 19-3. Stereo calibration, rectification, and correspondence 2 | #pragma warning(disable : 4996) 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | using namespace std; 11 | 12 | void help(char *argv[]) { 13 | cout 14 | << "\n\nExample 19-3. Stereo calibration, rectification, and " 15 | "correspondence" 16 | << "\n Reads in list of locations of a sequence of checkerboard " 17 | "calibration" 18 | << "\n objects from a left,right stereo camera pair. Calibrates, " 19 | "rectifies and then" 20 | << "\n does stereo correspondence." 21 | << "\n" 22 | << "\n This program will run on default parameters assuming you " 23 | "created a build directory" 24 | << "\n directly below the Learning-OpenCV-3 directory and are " 25 | "running programs there. NOTE: the list_of_stereo_pairs> must" 26 | << "\n give the full path name to the left right images, in " 27 | "alternating" 28 | << "\n lines: left image, right image, one path/filename per line, see" 29 | << "\n stereoData/example_19-03_list.txt file, you can comment out " 30 | "lines" 31 | << "\n there by starting them with #." 32 | << "\n" 33 | << "\nDefault Call (with parameters: board_w = 9, board_h = 6, list = " 34 | "../stereoData_19-03_list.txt):" 35 | << "\n" << argv[0] << "\n" 36 | << "\nManual call:" 37 | << "\n" << argv[0] << " [ ]" 38 | << "\n\n PRESS ANY KEY TO STEP THROUGH RESULTS AT EACH STAGE." 39 | << "\n" << endl; 40 | } 41 | 42 | static void StereoCalib(const char *imageList, int nx, int ny, 43 | bool useUncalibrated) { 44 | bool displayCorners = true; 45 | bool showUndistorted = true; 46 | bool isVerticalStereo = false; // horiz or vert cams 47 | const int maxScale = 1; 48 | const float squareSize = 1.f; 49 | 50 | // actual square size 51 | FILE *f = fopen(imageList, "rt"); 52 | int i, j, lr; 53 | int N = nx * ny; 54 | cv::Size board_sz = cv::Size(nx, ny); 55 | vector imageNames[2]; 56 | vector boardModel; 57 | vector > objectPoints; 58 | vector > points[2]; 59 | vector corners[2]; 60 | bool found[2] = {false, false}; 61 | cv::Size imageSize; 62 | 63 | // READ IN THE LIST OF CIRCLE GRIDS: 64 | // 65 | if (!f) { 66 | cout << "Cannot open file " << imageList << endl; 67 | return; 68 | } 69 | for (i = 0; i < ny; i++) 70 | for (j = 0; j < nx; j++) 71 | boardModel.push_back( 72 | cv::Point3f((float)(i * squareSize), (float)(j * squareSize), 0.f)); 73 | i = 0; 74 | for (;;) { 75 | char buf[1024]; 76 | lr = i % 2; 77 | if (lr == 0) 78 | found[0] = found[1] = false; 79 | if (!fgets(buf, sizeof(buf) - 3, f)) 80 | break; 81 | size_t len = strlen(buf); 82 | while (len > 0 && isspace(buf[len - 1])) 83 | buf[--len] = '\0'; 84 | if (buf[0] == '#') 85 | continue; 86 | cv::Mat img = cv::imread(buf, 0); 87 | if (img.empty()) 88 | break; 89 | imageSize = img.size(); 90 | imageNames[lr].push_back(buf); 91 | i++; 92 | 93 | // If we did not find board on the left image, 94 | // it does not make sense to find it on the right. 95 | // 96 | if (lr == 1 && !found[0]) 97 | continue; 98 | 99 | // Find circle grids and centers therein: 100 | for (int s = 1; s <= maxScale; s++) { 101 | cv::Mat timg = img; 102 | if (s > 1) 103 | resize(img, timg, cv::Size(), s, s, cv::INTER_CUBIC); 104 | // Just as example, this would be the call if you had circle calibration 105 | // boards ... 106 | // found[lr] = cv::findCirclesGrid(timg, cv::Size(nx, ny), 107 | // corners[lr], 108 | // cv::CALIB_CB_ASYMMETRIC_GRID | 109 | // cv::CALIB_CB_CLUSTERING); 110 | //...but we have chessboards in our images 111 | found[lr] = cv::findChessboardCorners(timg, board_sz, corners[lr]); 112 | 113 | if (found[lr] || s == maxScale) { 114 | cv::Mat mcorners(corners[lr]); 115 | mcorners *= (1. / s); 116 | } 117 | if (found[lr]) 118 | break; 119 | } 120 | if (displayCorners) { 121 | cout << buf << endl; 122 | cv::Mat cimg; 123 | cv::cvtColor(img, cimg, cv::COLOR_GRAY2BGR); 124 | 125 | // draw chessboard corners works for circle grids too 126 | cv::drawChessboardCorners(cimg, cv::Size(nx, ny), corners[lr], found[lr]); 127 | cv::imshow("Corners", cimg); 128 | if ((cv::waitKey(0) & 255) == 27) // Allow ESC to quit 129 | exit(-1); 130 | } else 131 | cout << '.'; 132 | if (lr == 1 && found[0] && found[1]) { 133 | objectPoints.push_back(boardModel); 134 | points[0].push_back(corners[0]); 135 | points[1].push_back(corners[1]); 136 | } 137 | } 138 | fclose(f); 139 | 140 | // CALIBRATE THE STEREO CAMERAS 141 | cv::Mat M1 = cv::Mat::eye(3, 3, CV_64F); 142 | cv::Mat M2 = cv::Mat::eye(3, 3, CV_64F); 143 | cv::Mat D1, D2, R, T, E, F; 144 | cout << "\nRunning stereo calibration ...\n"; 145 | cv::stereoCalibrate( 146 | objectPoints, points[0], points[1], M1, D1, M2, D2, imageSize, R, T, E, F, 147 | cv::CALIB_FIX_ASPECT_RATIO | cv::CALIB_ZERO_TANGENT_DIST | 148 | cv::CALIB_SAME_FOCAL_LENGTH, 149 | cv::TermCriteria(cv::TermCriteria::COUNT | cv::TermCriteria::EPS, 100, 150 | 1e-5)); 151 | cout << "Done! Press any key to step through images, ESC to exit\n\n"; 152 | 153 | // CALIBRATION QUALITY CHECK 154 | // because the output fundamental matrix implicitly 155 | // includes all the output information, 156 | // we can check the quality of calibration using the 157 | // epipolar geometry constraint: m2^t*F*m1=0 158 | vector lines[2]; 159 | double avgErr = 0; 160 | int nframes = (int)objectPoints.size(); 161 | for (i = 0; i < nframes; i++) { 162 | vector &pt0 = points[0][i]; 163 | vector &pt1 = points[1][i]; 164 | cv::undistortPoints(pt0, pt0, M1, D1, cv::Mat(), M1); 165 | cv::undistortPoints(pt1, pt1, M2, D2, cv::Mat(), M2); 166 | cv::computeCorrespondEpilines(pt0, 1, F, lines[0]); 167 | cv::computeCorrespondEpilines(pt1, 2, F, lines[1]); 168 | 169 | for (j = 0; j < N; j++) { 170 | double err = fabs(pt0[j].x * lines[1][j].x + pt0[j].y * lines[1][j].y + 171 | lines[1][j].z) + 172 | fabs(pt1[j].x * lines[0][j].x + pt1[j].y * lines[0][j].y + 173 | lines[0][j].z); 174 | avgErr += err; 175 | } 176 | } 177 | cout << "avg err = " << avgErr / (nframes * N) << endl; 178 | 179 | // COMPUTE AND DISPLAY RECTIFICATION 180 | // 181 | if (showUndistorted) { 182 | cv::Mat R1, R2, P1, P2, map11, map12, map21, map22; 183 | 184 | // IF BY CALIBRATED (BOUGUET'S METHOD) 185 | // 186 | if (!useUncalibrated) { 187 | stereoRectify(M1, D1, M2, D2, imageSize, R, T, R1, R2, P1, P2, 188 | cv::noArray(), 0); 189 | isVerticalStereo = fabs(P2.at(1, 3)) > fabs(P2.at(0, 3)); 190 | // Precompute maps for cvRemap() 191 | initUndistortRectifyMap(M1, D1, R1, P1, imageSize, CV_16SC2, map11, 192 | map12); 193 | initUndistortRectifyMap(M2, D2, R2, P2, imageSize, CV_16SC2, map21, 194 | map22); 195 | } 196 | 197 | // OR ELSE HARTLEY'S METHOD 198 | // 199 | else { 200 | 201 | // use intrinsic parameters of each camera, but 202 | // compute the rectification transformation directly 203 | // from the fundamental matrix 204 | vector allpoints[2]; 205 | for (i = 0; i < nframes; i++) { 206 | copy(points[0][i].begin(), points[0][i].end(), 207 | back_inserter(allpoints[0])); 208 | copy(points[1][i].begin(), points[1][i].end(), 209 | back_inserter(allpoints[1])); 210 | } 211 | cv::Mat F = findFundamentalMat(allpoints[0], allpoints[1], cv::FM_8POINT); 212 | cv::Mat H1, H2; 213 | cv::stereoRectifyUncalibrated(allpoints[0], allpoints[1], F, imageSize, 214 | H1, H2, 3); 215 | R1 = M1.inv() * H1 * M1; 216 | R2 = M2.inv() * H2 * M2; 217 | 218 | // Precompute map for cvRemap() 219 | // 220 | cv::initUndistortRectifyMap(M1, D1, R1, P1, imageSize, CV_16SC2, map11, 221 | map12); 222 | cv::initUndistortRectifyMap(M2, D2, R2, P2, imageSize, CV_16SC2, map21, 223 | map22); 224 | } 225 | 226 | // RECTIFY THE IMAGES AND FIND DISPARITY MAPS 227 | // 228 | cv::Mat pair; 229 | if (!isVerticalStereo) 230 | pair.create(imageSize.height, imageSize.width * 2, CV_8UC3); 231 | else 232 | pair.create(imageSize.height * 2, imageSize.width, CV_8UC3); 233 | 234 | // Setup for finding stereo corrrespondences 235 | // 236 | cv::Ptr stereo = cv::StereoSGBM::create( 237 | -64, 128, 11, 100, 1000, 32, 0, 15, 1000, 16, cv::StereoSGBM::MODE_HH); 238 | 239 | for (i = 0; i < nframes; i++) { 240 | cv::Mat img1 = cv::imread(imageNames[0][i].c_str(), 0); 241 | cv::Mat img2 = cv::imread(imageNames[1][i].c_str(), 0); 242 | cv::Mat img1r, img2r, disp, vdisp; 243 | if (img1.empty() || img2.empty()) 244 | continue; 245 | cv::remap(img1, img1r, map11, map12, cv::INTER_LINEAR); 246 | cv::remap(img2, img2r, map21, map22, cv::INTER_LINEAR); 247 | if (!isVerticalStereo || !useUncalibrated) { 248 | 249 | // When the stereo camera is oriented vertically, 250 | // Hartley method does not transpose the 251 | // image, so the epipolar lines in the rectified 252 | // images are vertical. Stereo correspondence 253 | // function does not support such a case. 254 | stereo->compute(img1r, img2r, disp); 255 | cv::normalize(disp, vdisp, 0, 256, cv::NORM_MINMAX, CV_8U); 256 | cv::imshow("disparity", vdisp); 257 | } 258 | if (!isVerticalStereo) { 259 | cv::Mat part = pair.colRange(0, imageSize.width); 260 | cvtColor(img1r, part, cv::COLOR_GRAY2BGR); 261 | part = pair.colRange(imageSize.width, imageSize.width * 2); 262 | cvtColor(img2r, part, cv::COLOR_GRAY2BGR); 263 | for (j = 0; j < imageSize.height; j += 16) 264 | cv::line(pair, cv::Point(0, j), cv::Point(imageSize.width * 2, j), 265 | cv::Scalar(0, 255, 0)); 266 | } else { 267 | cv::Mat part = pair.rowRange(0, imageSize.height); 268 | cv::cvtColor(img1r, part, cv::COLOR_GRAY2BGR); 269 | part = pair.rowRange(imageSize.height, imageSize.height * 2); 270 | cv::cvtColor(img2r, part, cv::COLOR_GRAY2BGR); 271 | for (j = 0; j < imageSize.width; j += 16) 272 | line(pair, cv::Point(j, 0), cv::Point(j, imageSize.height * 2), 273 | cv::Scalar(0, 255, 0)); 274 | } 275 | cv::imshow("rectified", pair); 276 | if ((cv::waitKey() & 255) == 27) 277 | break; 278 | } 279 | } 280 | } 281 | 282 | // 283 | //Default Call (with parameters: board_w = 9, board_h = 6, list = 284 | // ../stereoData_19-03_list.txt): 285 | //./example_19-03 286 | // 287 | //Manual call: 288 | //./example_19-03 [ ] 289 | // 290 | // Press any key to step through results, ESC to exit 291 | // 292 | 293 | 294 | int main(int argc, char **argv) { 295 | help(argv); 296 | int board_w = 9, board_h = 6; 297 | const char *board_list = "../stereoData/example_19-03_list.txt"; 298 | if (argc == 4) { 299 | board_list = argv[1]; 300 | board_w = atoi(argv[2]); 301 | board_h = atoi(argv[3]); 302 | } 303 | StereoCalib(board_list, board_w, board_h, true); 304 | return 0; 305 | } --------------------------------------------------------------------------------