├── .gitignore ├── .images ├── object_visualizer_layout.png └── visualizer_layout.png ├── CMakeLists.txt ├── README.md ├── include ├── common │ ├── rviz_command_button.h │ ├── transform_utils.h │ └── utils.h ├── object_visualizer │ └── object_visualizer.h └── track_visualizer │ └── track_visualizer.h ├── launzh ├── object_visualizer.launch └── track_visualizer.launch ├── package.xml ├── param └── button_layout.yaml ├── rviz ├── object_visualizer.rviz └── track_visualizer.rviz ├── rviz_command_button.xml └── src ├── common └── rviz_command_button.cc ├── object_visualizer ├── object_visualizer.cc └── object_visualizer_node.cc └── track_visualizer ├── track_visualizer.cc └── track_visualizer_node.cc /.gitignore: -------------------------------------------------------------------------------- 1 | data 2 | -------------------------------------------------------------------------------- /.images/object_visualizer_layout.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/xiaoliangabc/kitti_visualizer/46cf7907d7682ecba1d2e5a0929cfbf964d9f804/.images/object_visualizer_layout.png -------------------------------------------------------------------------------- /.images/visualizer_layout.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/xiaoliangabc/kitti_visualizer/46cf7907d7682ecba1d2e5a0929cfbf964d9f804/.images/visualizer_layout.png -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(kitti_visualizer) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | find_package(catkin REQUIRED COMPONENTS 9 | cv_bridge 10 | image_transport 11 | pcl_conversions 12 | pcl_ros 13 | roscpp 14 | rospy 15 | rviz 16 | sensor_msgs 17 | visualization_msgs 18 | ) 19 | 20 | ## The catkin_package macro generates cmake config files for your package 21 | catkin_package( 22 | CATKIN_DEPENDS cv_bridge image_transport pcl_conversions pcl_ros roscpp rospy rviz sensor_msgs visualization_msgs 23 | ) 24 | 25 | ## Specify additional locations of header files 26 | include_directories( 27 | include 28 | ${catkin_INCLUDE_DIRS} 29 | ) 30 | 31 | ## Declare a C++ executable 32 | add_executable(object_visualizer_node src/object_visualizer/object_visualizer.cc src/object_visualizer/object_visualizer_node.cc) 33 | add_executable(track_visualizer_node src/track_visualizer/track_visualizer.cc src/track_visualizer/track_visualizer_node.cc) 34 | 35 | ## Specify libraries to link a library or executable target against 36 | target_link_libraries(object_visualizer_node ${catkin_LIBRARIES}) 37 | target_link_libraries(object_visualizer_node ${OpenCV_LIBRARIES}) 38 | target_link_libraries(track_visualizer_node ${catkin_LIBRARIES}) 39 | target_link_libraries(track_visualizer_node ${OpenCV_LIBRARIES}) 40 | 41 | ## This setting causes Qt's "MOC" generation to happen automatically 42 | set(CMAKE_AUTOMOC ON) 43 | 44 | ## Use the Qt version that rviz used so they are compatible 45 | if(rviz_QT_VERSION VERSION_LESS "5") 46 | message(STATUS "Using Qt4 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}") 47 | find_package(Qt4 ${rviz_QT_VERSION} EXACT REQUIRED QtCore QtGui) 48 | ## pull in all required include dirs, define QT_LIBRARIES, etc 49 | include(${QT_USE_FILE}) 50 | else() 51 | message(STATUS "Using Qt5 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}") 52 | find_package(Qt5 ${rviz_QT_VERSION} EXACT REQUIRED Core Widgets) 53 | ## make target_link_libraries(${QT_LIBRARIES}) pull in all required dependencies 54 | set(QT_LIBRARIES Qt5::Widgets) 55 | endif() 56 | 57 | ## Define QT_NO_KEYWORDS, avoid defining "emit", "slots", etc 58 | add_definitions(-DQT_NO_KEYWORDS) 59 | 60 | ## Specify the list of source files 61 | set(SRC_FILES 62 | src/common/rviz_command_button.cc 63 | ${QT_MOC} 64 | ) 65 | 66 | ## Specify the list of header files 67 | set(HEADER_FILES 68 | include/common/rviz_command_button.h 69 | ) 70 | 71 | ## Declare library 72 | add_library(${PROJECT_NAME}_button ${SRC_FILES} ${HEADER_FILES}) 73 | 74 | ## Specify libraries to link a library or executable target against 75 | target_link_libraries(${PROJECT_NAME}_button ${QT_LIBRARIES} ${catkin_LIBRARIES}) 76 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Visualize KITTI Data Based on ROS and Rviz 2 | 3 | 4 | 5 | This package is used to visualize kitti data using ROS and Rviz. So far, there has the following main features 6 | - [x] [Visualize object data](#Visualize-Object-Data) 7 | - [x] [Visualize track data](#Visualize-Track-Data) 8 | - [ ] Visualize road data 9 | - [ ] Visualize raw data 10 | 11 | ## Dependencies 12 | - [ROS](https://www.ros.org/) 13 | 14 |     All procedures are based on ROS, which is commonly use in robotics and self-driving cars. We tested this package on the `Kinetic` version, but we believe that it can be used on other versions as well. 15 | 16 | - [jsk_recognition_msgs](https://jsk-docs.readthedocs.io/projects/jsk_recognition/en/latest/jsk_recognition_msgs/index.html) 17 | 18 |     This is required for visualizing 3D bounding boxes of objects. 19 | 20 | ## Usage 21 | 22 | ### Installation 23 | 24 | #### 1. ROS 25 | [Ubuntu install of ROS Kinetic](http://wiki.ros.org/kinetic/Installation/Ubuntu) 26 | 27 | #### 2. jsk_recognition_msgs 28 | ```bash 29 | sudo apt-get install ros-kinetic-jsk-recognition-msgs 30 | sudo apt-get install ros-kinetic-jsk-rviz-plugins 31 | ``` 32 | #### 3. kitti_visualizer package 33 | ```bash 34 | cd ros_workspace/src 35 | git clone git@github.com:xiaoliangabc/kitti_visualizer.git 36 | cd ros_workspace 37 | catkin_make 38 | source devel/setup.bash 39 | ``` 40 | 41 | ### Visualize Object Data 42 | 43 | #### Download Object Data 44 | Download object data (velodyne, image_2, calib, label_2) from [KITTI Object Detection Dataset](http://www.cvlibs.net/datasets/kitti/eval_object.php?obj_benchmark=3d) and set the folder structure as following: 45 | ``` 46 | object 47 | testing 48 | calib 49 | image_2 50 | results 51 | velodyne 52 | training 53 | calib 54 | image_2 55 | label_2 56 | velodyne 57 | ``` 58 | 59 | #### Modify Config File 60 | Open [launch/object_visualizer.launch](launch/object_visualizer.launch) file, change the following configs: 61 | - `data_path`: folder that contains object data 62 | - `dataset`: which dataset want to visualize (`training` / `testing`) 63 | - `frame_size`: number of frames for the corresponding dataset (`training: 7481` / `tesing: 7518`) 64 | - `current_frame`: frame index want to start visualization 65 | 66 | #### Launch object_visualizer 67 | Run 68 | ``` 69 | roslaunch kitti_visualizer object_visualizer.launch 70 | ``` 71 | Then `Rviz` will be launched, the layout of `Rviz` is look like 72 | 73 | 74 | 75 | #### Switch Frames 76 | Move the mouse to the bottom left of the screen: 77 | - Click the **Prev** button: switch to the previous frame 78 | - Click the **Next** button: switch to the next frame 79 | - Type frame number to **Frame string** box: jump to the frame you specified 80 | 81 | 82 | ### Visualize Track Data 83 | 84 | #### Download Track Data 85 | Download track data (velodyne, image_2, calib, label_2) from [KITTI Object Tracking Dataset](http://www.cvlibs.net/datasets/kitti/eval_tracking.php) and set the folder structure as following: 86 | ``` 87 | tracking 88 | testing 89 | calib 90 | image_02 91 | results 92 | velodyne 93 | oxts 94 | training 95 | calib 96 | image_02 97 | label_02 98 | velodyne 99 | oxts 100 | ``` 101 | 102 | #### Modify Config File 103 | Open [launch/track_visualizer.launch](launch/track_visualizer.launch) file, change the following configs: 104 | - `data_path`: folder that contains track data 105 | - `dataset`: which dataset want to visualize (`training` / `testing`) 106 | - `scene`: which scene want to visualize (`00xx`) 107 | - `current_frame`: frame index want to start visualization 108 | 109 | #### Launch track_visualizer 110 | Run 111 | ``` 112 | roslaunch kitti_visualizer track_visualizer.launch 113 | ``` 114 | Then `Rviz` will be launched, the layout of `Rviz` is look like 115 | 116 | 117 | 118 | ## Reference 119 | [kitti_object_vis](https://github.com/kuixu/kitti_object_vis) 120 | 121 | [second-ros](https://github.com/lgsvl/second-ros) 122 | -------------------------------------------------------------------------------- /include/common/rviz_command_button.h: -------------------------------------------------------------------------------- 1 | #ifndef KITTI_VISUALIZER_COMMON_RVIZ_COMMAND_BUTTON_H_ 2 | #define KITTI_VISUALIZER_COMMON_RVIZ_COMMAND_BUTTON_H_ 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | #include 10 | 11 | #include 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | #include 21 | 22 | namespace kitti_visualizer { 23 | // Declare Button subclass of rviz::Panel. Every panel which can be added via 24 | // the Panels/Add_New_Panel menu is a subclass of rviz::Panel. 25 | class CommandButton : public rviz::Panel { 26 | // This class uses Qt slots and is a subclass of QObject, so it needs 27 | // the Q_OBJECT macro. 28 | Q_OBJECT 29 | public: 30 | // QWidget subclass constructors usually take a parent widget 31 | // parameter (which usually defaults to 0). At the same time, 32 | // pluginlib::ClassLoader creates instances by calling the default 33 | // constructor (with no arguments). Taking the parameter and giving 34 | // a default of 0 lets the default constructor work and also lets 35 | // someone using the class for something else to pass in a parent 36 | // widget as they normally would with Qt. 37 | CommandButton(QWidget* parent = 0); 38 | 39 | // Declare overrides of rviz::Panel functions for saving and loading data from 40 | // the config file. 41 | virtual void load(const rviz::Config& config); 42 | virtual void save(rviz::Config config) const; 43 | 44 | // Declare some internal slots. 45 | protected Q_SLOTS: 46 | // Call when button is clicked. 47 | void ButtonResponse(QString command); 48 | 49 | // updateTopic() reads the topic name from the QLineEdit 50 | void UpdateFrame(); 51 | 52 | // Protected member variables. 53 | protected: 54 | // The ROS node handle. 55 | ros::NodeHandle nh_; 56 | 57 | // The ROS publisher for the command. 58 | ros::Publisher command_publisher_; 59 | 60 | // One-line text editor for entering the outgoing ROS topic name. 61 | QLineEdit* output_frame_editor_; 62 | }; // class CommandButton 63 | 64 | } // namespace kitti_visualizer 65 | 66 | #endif // KITTI_VISUALIZER_COMMON_RVIZ_COMMAND_BUTTON_H_ 67 | -------------------------------------------------------------------------------- /include/common/transform_utils.h: -------------------------------------------------------------------------------- 1 | #ifndef KITTI_VISUALIZER_COMMON_TRANSFORM_UTILS_H_ 2 | #define KITTI_VISUALIZER_COMMON_TRANSFORM_UTILS_H_ 3 | 4 | #include 5 | 6 | // Part 1: Roatation vector 7 | // Rotation vector to rotation matrix 8 | inline Eigen::Matrix3d RotationVectorToRotationMatrix( 9 | const Eigen::Vector3d& rotation_vector) { 10 | double norm = rotation_vector.norm(); 11 | Eigen::AngleAxisd angle_axis(norm, rotation_vector / norm); 12 | return angle_axis.matrix(); 13 | } 14 | 15 | // Rotation vector to euler angles 16 | inline Eigen::Vector3d RotationVectorToEulerAngles( 17 | const Eigen::Vector3d& rotation_vector) { 18 | double norm = rotation_vector.norm(); 19 | Eigen::AngleAxisd angle_axis(norm, rotation_vector / norm); 20 | return angle_axis.matrix().eulerAngles(2, 1, 0); 21 | } 22 | 23 | // Rotation vector quaternion 24 | inline Eigen::Quaterniond RotationVectorToQuaternion( 25 | const Eigen::Vector3d& rotation_vector) { 26 | double norm = rotation_vector.norm(); 27 | Eigen::AngleAxisd angle_axis(norm, rotation_vector / norm); 28 | Eigen::Quaterniond quaternion; 29 | quaternion = angle_axis; 30 | return quaternion; 31 | } 32 | 33 | // Part 2: Roatation matrix 34 | // Rotation matrix to rotation vector 35 | inline Eigen::Vector3d RotationMatrixToRotationVector( 36 | const Eigen::Matrix3d& rotation_matrix) { 37 | Eigen::AngleAxisd angle_axis(rotation_matrix); 38 | return angle_axis.angle() * angle_axis.axis(); 39 | } 40 | 41 | // Rotation matrix to euler angles 42 | inline Eigen::Vector3d RotationMatrixToEulerAngles( 43 | const Eigen::Matrix3d& rotation_matrix) { 44 | return rotation_matrix.eulerAngles(2, 1, 0); 45 | } 46 | 47 | // Rotation matrix to quaternion 48 | inline Eigen::Quaterniond RotationMatrixToQuaternion( 49 | const Eigen::Matrix3d& rotation_matrix) { 50 | Eigen::Quaterniond quaternion; 51 | quaternion = rotation_matrix; 52 | return quaternion; 53 | } 54 | 55 | // Part 3: Euler angles 56 | // Euler angles to rotation vector 57 | inline Eigen::Vector3d EulerAnglesToRotationVector( 58 | const Eigen::Vector3d& euler_angles) { 59 | Eigen::AngleAxisd roll_angle( 60 | Eigen::AngleAxisd(euler_angles(2), Eigen::Vector3d::UnitX())); 61 | Eigen::AngleAxisd pitch_angle( 62 | Eigen::AngleAxisd(euler_angles(1), Eigen::Vector3d::UnitY())); 63 | Eigen::AngleAxisd yaw_angle( 64 | Eigen::AngleAxisd(euler_angles(0), Eigen::Vector3d::UnitZ())); 65 | Eigen::AngleAxisd angle_axis; 66 | angle_axis = yaw_angle * pitch_angle * roll_angle; 67 | return angle_axis.angle() * angle_axis.axis(); 68 | } 69 | 70 | // Euler angles to rotation matrix 71 | inline Eigen::Matrix3d EulerAnglesToRotationMatrix( 72 | const Eigen::Vector3d& euler_angles) { 73 | Eigen::AngleAxisd roll_angle( 74 | Eigen::AngleAxisd(euler_angles(2), Eigen::Vector3d::UnitX())); 75 | Eigen::AngleAxisd pitch_angle( 76 | Eigen::AngleAxisd(euler_angles(1), Eigen::Vector3d::UnitY())); 77 | Eigen::AngleAxisd yaw_angle( 78 | Eigen::AngleAxisd(euler_angles(0), Eigen::Vector3d::UnitZ())); 79 | Eigen::Matrix3d rotation_matrix; 80 | rotation_matrix = yaw_angle * pitch_angle * roll_angle; 81 | return rotation_matrix; 82 | } 83 | 84 | // Euler angles to quaternion 85 | inline Eigen::Quaterniond EulerAnglesToQuaternion( 86 | const Eigen::Vector3d& euler_angles) { 87 | Eigen::AngleAxisd roll_angle( 88 | Eigen::AngleAxisd(euler_angles(2), Eigen::Vector3d::UnitX())); 89 | Eigen::AngleAxisd pitch_angle( 90 | Eigen::AngleAxisd(euler_angles(1), Eigen::Vector3d::UnitY())); 91 | Eigen::AngleAxisd yaw_angle( 92 | Eigen::AngleAxisd(euler_angles(0), Eigen::Vector3d::UnitZ())); 93 | Eigen::Quaterniond quaternion; 94 | quaternion = yaw_angle * pitch_angle * roll_angle; 95 | return quaternion; 96 | } 97 | 98 | // Part 4: Quaternion 99 | // Quaternion to rotation vector 100 | inline Eigen::Vector3d QuaternionToRotationVector( 101 | const Eigen::Quaterniond& quaternion) { 102 | Eigen::AngleAxisd angle_axis(quaternion); 103 | return angle_axis.angle() * angle_axis.axis(); 104 | } 105 | 106 | // Quaternion to rotation matrix 107 | inline Eigen::Matrix3d QuaternionToRotationMatrix( 108 | const Eigen::Quaterniond& quaternion) { 109 | return quaternion.matrix(); 110 | } 111 | 112 | // Quaternion to euler angles 113 | inline Eigen::Vector3d QuaternionToEulerAngles( 114 | const Eigen::Quaterniond& quaternion) { 115 | return quaternion.matrix().eulerAngles(2, 1, 0); 116 | } 117 | 118 | // Part 5: To Transform affine 119 | // Rotation vector and translation vector to transform affine 120 | inline Eigen::Affine3d RotationVectorToTransformAffine( 121 | const Eigen::Vector3d& rotation_vector, 122 | const Eigen::Vector3d& translation_vector) { 123 | Eigen::Affine3d transform_affine = Eigen::Affine3d::Identity(); 124 | transform_affine.linear() = RotationVectorToRotationMatrix(rotation_vector); 125 | transform_affine.translation() = translation_vector; 126 | return transform_affine; 127 | } 128 | 129 | // Rotation matrix and translation vector to transform affine 130 | inline Eigen::Affine3d RotationMatrixToTransformAffine( 131 | const Eigen::Matrix3d& rotation_matrix, 132 | const Eigen::Vector3d& translation_vector) { 133 | Eigen::Affine3d transform_affine = Eigen::Affine3d::Identity(); 134 | transform_affine.linear() = rotation_matrix; 135 | transform_affine.translation() = translation_vector; 136 | return transform_affine; 137 | } 138 | 139 | // Euler angles and translation vector to transform affine 140 | inline Eigen::Affine3d EulerAnglesToTransformAffine( 141 | const Eigen::Vector3d& euler_angles, 142 | const Eigen::Vector3d& translation_vector) { 143 | Eigen::Affine3d transform_affine = Eigen::Affine3d::Identity(); 144 | transform_affine.linear() = EulerAnglesToRotationMatrix(euler_angles); 145 | transform_affine.translation() = translation_vector; 146 | return transform_affine; 147 | } 148 | 149 | // Quaternion and translation vector to transform affine 150 | inline Eigen::Affine3d QuaternionToTransformAffine( 151 | const Eigen::Quaterniond& quaternion, 152 | const Eigen::Vector3d& translation_vector) { 153 | Eigen::Affine3d transform_affine = Eigen::Affine3d::Identity(); 154 | transform_affine.linear() = QuaternionToRotationMatrix(quaternion); 155 | transform_affine.translation() = translation_vector; 156 | return transform_affine; 157 | } 158 | 159 | // Part 6: From Transform matrix 160 | // Transform matrix to rotation vector and translation vector 161 | inline void TransformMatrixToRotationVector( 162 | const Eigen::Matrix4d& transform_matrix, Eigen::Vector3d& rotation_vector, 163 | Eigen::Vector3d& translation_vector) { 164 | Eigen::Affine3d transform_affine; 165 | transform_affine.matrix() = transform_matrix; 166 | rotation_vector = RotationMatrixToRotationVector(transform_affine.linear()); 167 | translation_vector = transform_affine.translation(); 168 | } 169 | 170 | // Transform matrix to rotation matrix and translation vector 171 | inline void TransformMatrixToRotationMatrix( 172 | const Eigen::Matrix4d& transform_matrix, Eigen::Matrix3d& rotation_matrix, 173 | Eigen::Vector3d& translation_vector) { 174 | Eigen::Affine3d transform_affine; 175 | transform_affine.matrix() = transform_matrix; 176 | rotation_matrix = transform_affine.linear(); 177 | translation_vector = transform_affine.translation(); 178 | } 179 | 180 | // Transform matrix to euler angles and translation vector 181 | inline void TransformMatrixToEulerAngles( 182 | const Eigen::Matrix4d& transform_matrix, Eigen::Vector3d& euler_angles, 183 | Eigen::Vector3d& translation_vector) { 184 | Eigen::Affine3d transform_affine; 185 | transform_affine.matrix() = transform_matrix; 186 | euler_angles = RotationMatrixToEulerAngles(transform_affine.linear()); 187 | translation_vector = transform_affine.translation(); 188 | } 189 | 190 | // Transform matrix to quaternion and translation vector 191 | inline void TransformMatrixToQuaternion(const Eigen::Matrix4d& transform_matrix, 192 | Eigen::Quaterniond& quaternion, 193 | Eigen::Vector3d& translation_vector) { 194 | Eigen::Affine3d transform_affine; 195 | transform_affine.matrix() = transform_matrix; 196 | quaternion = RotationMatrixToQuaternion(transform_affine.linear()); 197 | translation_vector = transform_affine.translation(); 198 | } 199 | 200 | #endif // KITTI_VISUALIZER_COMMON_TRANSFORM_UTILS_H_ 201 | -------------------------------------------------------------------------------- /include/common/utils.h: -------------------------------------------------------------------------------- 1 | #ifndef KITTI_VISUALIZER_COMMON_UTILS_H_ 2 | #define KITTI_VISUALIZER_COMMON_UTILS_H_ 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | #include 11 | 12 | namespace kitti_visualizer { 13 | static int FolderFilesNumber(std::string path) { 14 | DIR *dir; 15 | struct dirent *ent; 16 | int files_number = 0; 17 | if ((dir = opendir(path.c_str())) != NULL) { 18 | // Print all the files and directories within directory 19 | while ((ent = readdir(dir)) != NULL) { 20 | if (strcmp(ent->d_name, ".") == 0 || strcmp(ent->d_name, "..") == 0) 21 | continue; 22 | files_number++; 23 | } 24 | closedir(dir); 25 | } else { 26 | // Could not open directory 27 | ROS_ERROR("Could not open directory: %s", path.c_str()); 28 | ros::shutdown(); 29 | } 30 | 31 | return files_number; 32 | } 33 | 34 | static void ReadPointCloud(const std::string &in_file, 35 | pcl::PointCloud::Ptr raw_cloud) { 36 | // Load point cloud from .bin file 37 | std::fstream input(in_file.c_str(), std::ios::in | std::ios::binary); 38 | if (!input.good()) { 39 | ROS_ERROR("Could not read file: %s", in_file.c_str()); 40 | exit(EXIT_FAILURE); 41 | } 42 | input.seekg(0, std::ios::beg); 43 | 44 | // Transform .bin file to pcl cloud 45 | for (size_t i = 0; input.good() && !input.eof(); i++) { 46 | pcl::PointXYZI pt; 47 | // Read data 48 | input.read((char *)&pt.x, 3 * sizeof(float)); 49 | input.read((char *)&pt.intensity, sizeof(float)); 50 | raw_cloud->points.push_back(pt); 51 | } 52 | input.close(); 53 | } 54 | 55 | // static void ReadCalibMatrix(const std::string &file_path, 56 | // const std::string &matrix_name, 57 | // Eigen::MatrixXd &trans_matrix) { 58 | // // Open calib file 59 | // std::ifstream ifs(file_path); 60 | // if (!ifs) { 61 | // ROS_ERROR("File %s does not exist", file_path.c_str()); 62 | // ros::shutdown(); 63 | // } 64 | 65 | // // Read matrix 66 | // std::string temp_str; 67 | // while (std::getline(ifs, temp_str)) { 68 | // std::istringstream iss(temp_str); 69 | // std::string name; 70 | // iss >> name; 71 | // if (name == matrix_name) { 72 | // float temp_float; 73 | // for (int i = 0; i < trans_matrix.rows(); ++i) { 74 | // for (int j = 0; j < trans_matrix.cols(); ++j) { 75 | // if (iss.rdbuf()->in_avail() != 0) { 76 | // iss >> temp_float; 77 | // trans_matrix(i, j) = temp_float; 78 | // } 79 | // } 80 | // } 81 | // } 82 | // } 83 | // } 84 | 85 | static void ReadCalibMatrix(const std::string &file_path, 86 | const std::string &matrix_name, 87 | Eigen::MatrixXd &trans_matrix) { 88 | // Open calib file 89 | std::ifstream ifs(file_path); 90 | if (!ifs) { 91 | ROS_ERROR("File %s does not exist", file_path.c_str()); 92 | ros::shutdown(); 93 | } 94 | 95 | // Read matrix 96 | std::string temp_str; 97 | while (std::getline(ifs, temp_str)) { 98 | std::istringstream iss(temp_str); 99 | std::string name; 100 | iss >> name; 101 | if (name == matrix_name) { 102 | if (matrix_name == "P2:") { 103 | trans_matrix = Eigen::MatrixXd::Zero(3, 4); 104 | float temp_float; 105 | for (int i = 0; i < 3; ++i) { 106 | for (int j = 0; j < 4; ++j) { 107 | iss >> temp_float; 108 | trans_matrix(i, j) = temp_float; 109 | } 110 | } 111 | return; 112 | } else if (matrix_name == "R0_rect:") { 113 | trans_matrix = Eigen::MatrixXd::Zero(4, 4); 114 | float temp_float; 115 | for (int i = 0; i < 3; ++i) { 116 | for (int j = 0; j < 3; ++j) { 117 | iss >> temp_float; 118 | trans_matrix(i, j) = temp_float; 119 | } 120 | } 121 | trans_matrix(3, 3) = 1.0; 122 | return; 123 | } else if (matrix_name == "R_rect") { 124 | trans_matrix = Eigen::MatrixXd::Zero(4, 4); 125 | float temp_float; 126 | for (int i = 0; i < 3; ++i) { 127 | for (int j = 0; j < 3; ++j) { 128 | iss >> temp_float; 129 | trans_matrix(i, j) = temp_float; 130 | } 131 | } 132 | trans_matrix(3, 3) = 1.0; 133 | return; 134 | } else if (matrix_name == "Tr_velo_to_cam:") { 135 | trans_matrix = Eigen::MatrixXd::Zero(4, 4); 136 | float temp_float; 137 | for (int i = 0; i < 3; ++i) { 138 | for (int j = 0; j < 4; ++j) { 139 | iss >> temp_float; 140 | trans_matrix(i, j) = temp_float; 141 | } 142 | } 143 | trans_matrix(3, 3) = 1.0; 144 | return; 145 | } else if (matrix_name == "Tr_velo_cam") { 146 | trans_matrix = Eigen::MatrixXd::Zero(4, 4); 147 | float temp_float; 148 | for (int i = 0; i < 3; ++i) { 149 | for (int j = 0; j < 4; ++j) { 150 | iss >> temp_float; 151 | trans_matrix(i, j) = temp_float; 152 | } 153 | } 154 | trans_matrix(3, 3) = 1.0; 155 | return; 156 | } else if (matrix_name == "Tr_cam_to_road:") { 157 | trans_matrix = Eigen::MatrixXd::Zero(4, 4); 158 | float temp_float; 159 | for (int i = 0; i < 3; ++i) { 160 | for (int j = 0; j < 4; ++j) { 161 | iss >> temp_float; 162 | trans_matrix(i, j) = temp_float; 163 | } 164 | } 165 | trans_matrix(3, 3) = 1.0; 166 | return; 167 | } 168 | } 169 | } 170 | ROS_ERROR("Transform matrix %s does not exist", matrix_name.c_str()); 171 | ros::shutdown(); 172 | } 173 | 174 | } // namespace kitti_visualizer 175 | 176 | #endif // KITTI_VISUALIZER_COMMON_UTILS_H_ 177 | -------------------------------------------------------------------------------- /include/object_visualizer/object_visualizer.h: -------------------------------------------------------------------------------- 1 | #ifndef KITTI_VISUALIZER_OBJECT_VISUALIZER_OBJECT_VISUALIZER_H_ 2 | #define KITTI_VISUALIZER_OBJECT_VISUALIZER_OBJECT_VISUALIZER_H_ 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | #include 17 | #include 18 | #include 19 | 20 | #include 21 | #include 22 | 23 | #include 24 | 25 | #include 26 | 27 | #include "common/transform_utils.h" 28 | #include "common/utils.h" 29 | 30 | namespace kitti_visualizer { 31 | 32 | class ObjectVisualizer { 33 | public: 34 | ObjectVisualizer(ros::NodeHandle nh, ros::NodeHandle pnh); 35 | 36 | // Visualize object data 37 | void Visualizer(); 38 | 39 | private: 40 | // Visualize point cloud 41 | void PointCloudVisualizer(const std::string& file_prefix, 42 | const ros::Publisher publisher); 43 | 44 | // Visualize image 45 | void ImageVisualizer(const std::string& file_prefix, 46 | const ros::Publisher publisher); 47 | 48 | // Draw 2D bounding boxes in image 49 | void Draw2DBoundingBoxes(const std::string& file_prefix, cv::Mat& raw_image); 50 | 51 | // Visualize 3D bounding boxes 52 | void BoundingBoxesVisualizer(const std::string& file_prefix, 53 | const ros::Publisher publisher); 54 | 55 | // Transform 3D bounding boxes form camera to velodyne 56 | jsk_recognition_msgs::BoundingBoxArray TransformBoundingBoxes( 57 | const std::vector> detections, 58 | const std::string& file_prefix); 59 | 60 | // Parse detections from file 61 | std::vector> ParseDetections( 62 | const std::string& file_prefix); 63 | 64 | // Subscribe command from Rviz 65 | void CommandButtonCallback(const std_msgs::String::ConstPtr& in_command); 66 | 67 | // Judge whether the files number are valid 68 | void AssertFilesNumber(); 69 | 70 | private: 71 | ros::NodeHandle nh_; 72 | ros::NodeHandle pnh_; 73 | 74 | // Subscriber 75 | ros::Subscriber sub_command_button_; 76 | 77 | // Publisher 78 | ros::Publisher pub_point_cloud_; 79 | ros::Publisher pub_image_; 80 | ros::Publisher pub_bounding_boxes_; 81 | 82 | // Object data path 83 | std::string data_path_; 84 | std::string dataset_; 85 | 86 | // Frame 87 | int frame_size_; 88 | int current_frame_; 89 | }; 90 | 91 | } // namespace kitti_visualizer 92 | 93 | #endif // KITTI_VISUALIZER_OBJECT_VISUALIZER_OBJECT_VISUALIZER_H_ 94 | -------------------------------------------------------------------------------- /include/track_visualizer/track_visualizer.h: -------------------------------------------------------------------------------- 1 | #ifndef KITTI_VISUALIZER_TRACK_VISUALIZER_TRACK_VISUALIZER_H_ 2 | #define KITTI_VISUALIZER_TRACK_VISUALIZER_TRACK_VISUALIZER_H_ 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | #include 17 | #include 18 | #include 19 | 20 | #include 21 | #include 22 | 23 | #include 24 | 25 | #include 26 | 27 | #include "common/transform_utils.h" 28 | #include "common/utils.h" 29 | 30 | namespace kitti_visualizer { 31 | 32 | class TrackVisualizer { 33 | public: 34 | TrackVisualizer(ros::NodeHandle nh, ros::NodeHandle pnh); 35 | 36 | // Visualize object data 37 | void Visualizer(const int& frame); 38 | 39 | // Get frame number 40 | int GetFrameNumber(); 41 | 42 | private: 43 | // Visualize point cloud 44 | void PointCloudVisualizer(const std::string& file_prefix, 45 | const ros::Publisher publisher); 46 | 47 | // Visualize image 48 | void ImageVisualizer(const std::string& file_prefix, 49 | const ros::Publisher publisher); 50 | 51 | // Draw 2D bounding boxes in image 52 | void Draw2DBoundingBoxes(const std::string& file_prefix, 53 | const std::string& folder, cv::Mat& raw_image); 54 | 55 | // Visualize 3D bounding boxes 56 | void BoundingBoxesVisualizer(const std::string& file_prefix, 57 | const std::string& folder, 58 | const ros::Publisher publisher); 59 | 60 | // Transform 3D bounding boxes form camera to velodyne 61 | jsk_recognition_msgs::BoundingBoxArray TransformBoundingBoxes( 62 | const std::vector> detections, 63 | const std::string& file_prefix); 64 | 65 | // Parse detections from file 66 | std::vector> ParseTracks(const std::string& file_prefix, 67 | const std::string& folder); 68 | 69 | // Subscribe command from Rviz 70 | void CommandButtonCallback(const std_msgs::String::ConstPtr& in_command); 71 | 72 | private: 73 | ros::NodeHandle nh_; 74 | ros::NodeHandle pnh_; 75 | 76 | // Subscriber 77 | ros::Subscriber sub_command_button_; 78 | 79 | // Publisher 80 | ros::Publisher pub_point_cloud_; 81 | ros::Publisher pub_image_; 82 | ros::Publisher pub_bounding_boxes_; 83 | ros::Publisher pub_tracking_result_; 84 | 85 | // Object data path 86 | std::string data_path_; 87 | std::string dataset_; 88 | 89 | // Scene 90 | std::string scene_; 91 | 92 | // Frame 93 | int frame_size_; 94 | int current_frame_; 95 | }; 96 | 97 | } // namespace kitti_visualizer 98 | 99 | #endif // KITTI_VISUALIZER_TRACK_VISUALIZER_TRACK_VISUALIZER_H_ 100 | -------------------------------------------------------------------------------- /launzh/object_visualizer.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /launzh/track_visualizer.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | kitti_visualizer 4 | 0.0.0 5 | The kitti visualizer package 6 | 7 | wxl 8 | 9 | BSD 10 | 11 | 12 | 13 | wxl 14 | 15 | catkin 16 | cv_bridge 17 | image_transport 18 | pcl_conversions 19 | pcl_ros 20 | roscpp 21 | rospy 22 | rviz 23 | sensor_msgs 24 | visualization_msgs 25 | cv_bridge 26 | image_transport 27 | pcl_conversions 28 | pcl_ros 29 | roscpp 30 | rospy 31 | rviz 32 | sensor_msgs 33 | visualization_msgs 34 | cv_bridge 35 | image_transport 36 | pcl_conversions 37 | pcl_ros 38 | roscpp 39 | rospy 40 | rviz 41 | sensor_msgs 42 | visualization_msgs 43 | 44 | 45 | 46 | 47 | 48 | -------------------------------------------------------------------------------- /param/button_layout.yaml: -------------------------------------------------------------------------------- 1 | - name: 'Prev' 2 | - name: 'Next' 3 | -------------------------------------------------------------------------------- /rviz/object_visualizer.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 0 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: ~ 7 | Splitter Ratio: 0.5 8 | Tree Height: 399 9 | - Class: rviz/Selection 10 | Name: Selection 11 | - Class: rviz/Tool Properties 12 | Expanded: 13 | - /2D Pose Estimate1 14 | - /2D Nav Goal1 15 | - /Publish Point1 16 | Name: Tool Properties 17 | Splitter Ratio: 0.588679016 18 | - Class: rviz/Views 19 | Expanded: 20 | - /Current View1 21 | Name: Views 22 | Splitter Ratio: 0.5 23 | - Class: rviz/Time 24 | Experimental: false 25 | Name: Time 26 | SyncMode: 0 27 | SyncSource: PointCloud2 28 | - Class: kitti_visualizer/Button 29 | Name: Button 30 | - Class: rviz/Selection 31 | Name: Selection 32 | Toolbars: 33 | toolButtonStyle: 2 34 | Visualization Manager: 35 | Class: "" 36 | Displays: 37 | - Class: rviz/Axes 38 | Enabled: true 39 | Length: 2 40 | Name: Axes 41 | Radius: 0.100000001 42 | Reference Frame: 43 | Value: true 44 | - Alpha: 1 45 | Autocompute Intensity Bounds: true 46 | Autocompute Value Bounds: 47 | Max Value: 10 48 | Min Value: -10 49 | Value: true 50 | Axis: Z 51 | Channel Name: intensity 52 | Class: rviz/PointCloud2 53 | Color: 255; 255; 255 54 | Color Transformer: FlatColor 55 | Decay Time: 0 56 | Enabled: true 57 | Invert Rainbow: false 58 | Max Color: 255; 255; 255 59 | Max Intensity: 0.99000001 60 | Min Color: 0; 0; 0 61 | Min Intensity: 0 62 | Name: PointCloud2 63 | Position Transformer: XYZ 64 | Queue Size: 10 65 | Selectable: true 66 | Size (Pixels): 3 67 | Size (m): 0.100000001 68 | Style: Flat Squares 69 | Topic: /kitti_visualizer/object/point_cloud 70 | Unreliable: false 71 | Use Fixed Frame: true 72 | Use rainbow: true 73 | Value: true 74 | - Class: rviz/Image 75 | Enabled: true 76 | Image Topic: /kitti_visualizer/object/image 77 | Max Value: 1 78 | Median window: 5 79 | Min Value: 0 80 | Name: Image 81 | Normalize Range: true 82 | Queue Size: 2 83 | Transport Hint: raw 84 | Unreliable: false 85 | Value: true 86 | - Class: jsk_rviz_plugin/BoundingBoxArray 87 | Enabled: true 88 | Name: BoundingBoxArray 89 | Topic: /kitti_visualizer/object/bounding_boxes 90 | Unreliable: false 91 | Value: true 92 | alpha: 0.800000012 93 | color: 25; 255; 0 94 | coloring: Auto 95 | line width: 0.200000003 96 | only edge: true 97 | show coords: false 98 | Enabled: true 99 | Global Options: 100 | Background Color: 48; 48; 48 101 | Default Light: true 102 | Fixed Frame: base_link 103 | Frame Rate: 30 104 | Name: root 105 | Tools: 106 | - Class: rviz/Interact 107 | Hide Inactive Objects: true 108 | - Class: rviz/MoveCamera 109 | - Class: rviz/Select 110 | - Class: rviz/FocusCamera 111 | - Class: rviz/Measure 112 | - Class: rviz/SetInitialPose 113 | Topic: /initialpose 114 | - Class: rviz/SetGoal 115 | Topic: /move_base_simple/goal 116 | - Class: rviz/PublishPoint 117 | Single click: true 118 | Topic: /clicked_point 119 | Value: true 120 | Views: 121 | Current: 122 | Angle: -1.56999958 123 | Class: rviz/TopDownOrtho 124 | Enable Stereo Rendering: 125 | Stereo Eye Separation: 0.0599999987 126 | Stereo Focal Distance: 1 127 | Swap Stereo Eyes: false 128 | Value: false 129 | Invert Z Axis: false 130 | Name: Current View 131 | Near Clip Distance: 0.00999999978 132 | Scale: 9.54829311 133 | Target Frame: 134 | Value: TopDownOrtho (rviz) 135 | X: 36.4230385 136 | Y: 0.919728637 137 | Saved: ~ 138 | Window Geometry: 139 | Button: 140 | collapsed: false 141 | Displays: 142 | collapsed: false 143 | Height: 1000 144 | Hide Left Dock: false 145 | Hide Right Dock: true 146 | Image: 147 | collapsed: false 148 | QMainWindow State: 000000ff00000000fd0000000400000000000002980000035efc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000001d0000000d700fffffffb0000001200530065006c0065006300740069006f006e00000001b4000000810000006100fffffffb0000000a0049006d00610067006501000001fe0000013f0000001600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c0042007500740074006f006e0100000343000000430000004300ffffff000000010000010f0000035efc0200000002fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007300000000280000035e000000ad00ffffff0000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005ff0000003efc0100000002fb0000000800540069006d00650100000000000005ff0000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000003610000035e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 149 | Selection: 150 | collapsed: false 151 | Time: 152 | collapsed: false 153 | Tool Properties: 154 | collapsed: false 155 | Views: 156 | collapsed: true 157 | Width: 1535 158 | X: 65 159 | Y: 24 160 | -------------------------------------------------------------------------------- /rviz/track_visualizer.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 0 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /BoundingBoxArray2 8 | Splitter Ratio: 0.5 9 | Tree Height: 399 10 | - Class: rviz/Selection 11 | Name: Selection 12 | - Class: rviz/Tool Properties 13 | Expanded: 14 | - /2D Pose Estimate1 15 | - /2D Nav Goal1 16 | - /Publish Point1 17 | Name: Tool Properties 18 | Splitter Ratio: 0.588679016 19 | - Class: rviz/Views 20 | Expanded: 21 | - /Current View1 22 | Name: Views 23 | Splitter Ratio: 0.5 24 | - Class: rviz/Time 25 | Experimental: false 26 | Name: Time 27 | SyncMode: 0 28 | SyncSource: PointCloud2 29 | - Class: kitti_visualizer/Button 30 | Name: Button 31 | - Class: rviz/Selection 32 | Name: Selection 33 | Toolbars: 34 | toolButtonStyle: 2 35 | Visualization Manager: 36 | Class: "" 37 | Displays: 38 | - Class: rviz/Axes 39 | Enabled: true 40 | Length: 2 41 | Name: Axes 42 | Radius: 0.100000001 43 | Reference Frame: 44 | Value: true 45 | - Alpha: 1 46 | Autocompute Intensity Bounds: true 47 | Autocompute Value Bounds: 48 | Max Value: 10 49 | Min Value: -10 50 | Value: true 51 | Axis: Z 52 | Channel Name: intensity 53 | Class: rviz/PointCloud2 54 | Color: 255; 255; 255 55 | Color Transformer: FlatColor 56 | Decay Time: 0 57 | Enabled: true 58 | Invert Rainbow: false 59 | Max Color: 255; 255; 255 60 | Max Intensity: 0.99000001 61 | Min Color: 0; 0; 0 62 | Min Intensity: 0 63 | Name: PointCloud2 64 | Position Transformer: XYZ 65 | Queue Size: 10 66 | Selectable: true 67 | Size (Pixels): 3 68 | Size (m): 0.150000006 69 | Style: Flat Squares 70 | Topic: /kitti_visualizer/object/point_cloud 71 | Unreliable: false 72 | Use Fixed Frame: true 73 | Use rainbow: true 74 | Value: true 75 | - Class: rviz/Image 76 | Enabled: true 77 | Image Topic: /kitti_visualizer/object/image 78 | Max Value: 1 79 | Median window: 5 80 | Min Value: 0 81 | Name: Image 82 | Normalize Range: true 83 | Queue Size: 2 84 | Transport Hint: raw 85 | Unreliable: false 86 | Value: true 87 | - Class: jsk_rviz_plugin/BoundingBoxArray 88 | Enabled: true 89 | Name: BoundingBoxArray 90 | Topic: /kitti_visualizer/object/bounding_boxes 91 | Unreliable: false 92 | Value: true 93 | alpha: 0.800000012 94 | color: 25; 255; 0 95 | coloring: Flat color 96 | line width: 0.300000012 97 | only edge: true 98 | show coords: false 99 | - Class: jsk_rviz_plugin/BoundingBoxArray 100 | Enabled: true 101 | Name: BoundingBoxArray 102 | Topic: /kitti_visualizer/object/tracking_result 103 | Unreliable: false 104 | Value: true 105 | alpha: 0.800000012 106 | color: 25; 255; 0 107 | coloring: Label 108 | line width: 0.00499999989 109 | only edge: false 110 | show coords: false 111 | Enabled: true 112 | Global Options: 113 | Background Color: 48; 48; 48 114 | Default Light: true 115 | Fixed Frame: base_link 116 | Frame Rate: 30 117 | Name: root 118 | Tools: 119 | - Class: rviz/Interact 120 | Hide Inactive Objects: true 121 | - Class: rviz/MoveCamera 122 | - Class: rviz/Select 123 | - Class: rviz/FocusCamera 124 | - Class: rviz/Measure 125 | - Class: rviz/SetInitialPose 126 | Topic: /initialpose 127 | - Class: rviz/SetGoal 128 | Topic: /move_base_simple/goal 129 | - Class: rviz/PublishPoint 130 | Single click: true 131 | Topic: /clicked_point 132 | Value: true 133 | Views: 134 | Current: 135 | Angle: 0.0149983382 136 | Class: rviz/TopDownOrtho 137 | Enable Stereo Rendering: 138 | Stereo Eye Separation: 0.0599999987 139 | Stereo Focal Distance: 1 140 | Swap Stereo Eyes: false 141 | Value: false 142 | Invert Z Axis: false 143 | Name: Current View 144 | Near Clip Distance: 0.00999999978 145 | Scale: 17.8780117 146 | Target Frame: 147 | Value: TopDownOrtho (rviz) 148 | X: 35.8659821 149 | Y: 4.4915905 150 | Saved: ~ 151 | Window Geometry: 152 | Button: 153 | collapsed: false 154 | Displays: 155 | collapsed: false 156 | Height: 1000 157 | Hide Left Dock: false 158 | Hide Right Dock: true 159 | Image: 160 | collapsed: false 161 | QMainWindow State: 000000ff00000000fd0000000400000000000001d30000035efc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000001d0000000d700fffffffb0000001200530065006c0065006300740069006f006e00000001b4000000810000006100fffffffb0000000a0049006d00610067006501000001fe0000013f0000001600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c0042007500740074006f006e0100000343000000430000004300ffffff000000010000010f0000035efc0200000002fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007300000000280000035e000000ad00ffffff0000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005ff0000003efc0100000002fb0000000800540069006d00650100000000000005ff0000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000004260000035e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 162 | Selection: 163 | collapsed: false 164 | Time: 165 | collapsed: false 166 | Tool Properties: 167 | collapsed: false 168 | Views: 169 | collapsed: true 170 | Width: 1535 171 | X: 65 172 | Y: 24 173 | -------------------------------------------------------------------------------- /rviz_command_button.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | A panel widget allowing simple diff-drive style robot base control. 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /src/common/rviz_command_button.cc: -------------------------------------------------------------------------------- 1 | #include "common/rviz_command_button.h" 2 | 3 | namespace kitti_visualizer { 4 | // We start with the constructor, doing the standard Qt thing of 5 | // passing the optional *parent* argument on to the superclass 6 | // constructor, and also zero-ing the velocities we will be 7 | // publishing. 8 | CommandButton::CommandButton(QWidget* parent) : rviz::Panel(parent) { 9 | // Get button parameters 10 | std::string current_path = ros::package::getPath("kitti_visualizer"); 11 | std::string button_layout_file = current_path + "/param/button_layout.yaml"; 12 | YAML::Node node = YAML::LoadFile(button_layout_file); 13 | std::vector button_names; 14 | for (int i = 0; i < node.size(); ++i) { 15 | button_names.push_back(node[i]["name"].as()); 16 | } 17 | 18 | // Lay out buttons using QPushButton in a QHBoxLayout. 19 | QHBoxLayout* layout = new QHBoxLayout; 20 | QSignalMapper* signal_mapper = new QSignalMapper(this); 21 | for (auto button_name : button_names) { 22 | QPushButton* button = new QPushButton(QString::fromStdString(button_name)); 23 | layout->addWidget(button); 24 | button->setEnabled(true); 25 | connect(button, SIGNAL(clicked()), signal_mapper, SLOT(map())); 26 | signal_mapper->setMapping(button, QString::fromStdString(button_name)); 27 | } 28 | connect(signal_mapper, SIGNAL(mapped(QString)), this, 29 | SLOT(ButtonResponse(QString))); 30 | 31 | // Lay out the "frame string" text entry field using a QLabel and a QLineEdit 32 | // in a QHBoxLayout. 33 | QHBoxLayout* frame_layout = new QHBoxLayout; 34 | frame_layout->addWidget(new QLabel("Frame string:")); 35 | output_frame_editor_ = new QLineEdit; 36 | frame_layout->addWidget(output_frame_editor_); 37 | output_frame_editor_->setPlaceholderText("000000"); 38 | output_frame_editor_->setText("000000"); 39 | layout->addLayout(frame_layout); 40 | connect(output_frame_editor_, SIGNAL(returnPressed()), this, 41 | SLOT(UpdateFrame())); 42 | 43 | setLayout(layout); 44 | 45 | // Publisher 46 | command_publisher_ = 47 | nh_.advertise("/kitti_visualizer/command_button", 1); 48 | } 49 | 50 | void CommandButton::ButtonResponse(QString command) { 51 | std_msgs::String command_msg; 52 | command_msg.data = command.toStdString(); 53 | command_publisher_.publish(command_msg); 54 | } 55 | 56 | void CommandButton::UpdateFrame() { 57 | std_msgs::String command_msg; 58 | command_msg.data = output_frame_editor_->text().toStdString(); 59 | command_publisher_.publish(command_msg); 60 | std::cout << output_frame_editor_->text().toStdString() << std::endl; 61 | } 62 | 63 | // Save all configuration data from this panel to the given Config object. 64 | void CommandButton::save(rviz::Config config) const { 65 | rviz::Panel::save(config); 66 | } 67 | 68 | // Load all configuration data for this panel from the given Config object. 69 | void CommandButton::load(const rviz::Config& config) { 70 | rviz::Panel::load(config); 71 | } 72 | 73 | } // kitti_visualizer 74 | 75 | // Tell pluginlib about this class. Every class which should be 76 | // loadable by pluginlib::ClassLoader must have these two lines 77 | // compiled in its .cpp file, outside of any namespace scope. 78 | #include 79 | PLUGINLIB_EXPORT_CLASS(kitti_visualizer::CommandButton, rviz::Panel) 80 | -------------------------------------------------------------------------------- /src/object_visualizer/object_visualizer.cc: -------------------------------------------------------------------------------- 1 | #include "object_visualizer/object_visualizer.h" 2 | 3 | namespace kitti_visualizer { 4 | 5 | ObjectVisualizer::ObjectVisualizer(ros::NodeHandle nh, ros::NodeHandle pnh) 6 | : nh_(nh), pnh_(pnh) { 7 | pnh_.param("data_path", data_path_, ""); 8 | pnh_.param("dataset", dataset_, ""); 9 | pnh_.param("frame_size", frame_size_, 0); 10 | pnh_.param("current_frame", current_frame_, 0); 11 | 12 | // Judge whether the files number are valid 13 | AssertFilesNumber(); 14 | 15 | // Subscriber 16 | sub_command_button_ = 17 | nh_.subscribe("/kitti_visualizer/command_button", 2, 18 | &ObjectVisualizer::CommandButtonCallback, this); 19 | 20 | // Publisher 21 | pub_point_cloud_ = nh_.advertise>( 22 | "kitti_visualizer/object/point_cloud", 2); 23 | pub_image_ = 24 | nh_.advertise("kitti_visualizer/object/image", 2); 25 | pub_bounding_boxes_ = nh_.advertise( 26 | "kitti_visualizer/object/bounding_boxes", 2); 27 | } 28 | 29 | void ObjectVisualizer::Visualizer() { 30 | // Get current file name 31 | std::ostringstream file_prefix; 32 | file_prefix << std::setfill('0') << std::setw(6) << current_frame_; 33 | ROS_INFO("Visualizing frame %s ...", file_prefix.str().c_str()); 34 | 35 | // Visualize point cloud 36 | PointCloudVisualizer(file_prefix.str(), pub_point_cloud_); 37 | 38 | // Visualize image 39 | ImageVisualizer(file_prefix.str(), pub_image_); 40 | 41 | // Visualize 3D bounding boxes 42 | BoundingBoxesVisualizer(file_prefix.str(), pub_bounding_boxes_); 43 | } 44 | 45 | void ObjectVisualizer::PointCloudVisualizer(const std::string& file_prefix, 46 | const ros::Publisher publisher) { 47 | // Read point cloud 48 | std::string cloud_file_name = 49 | data_path_ + dataset_ + "/velodyne/" + file_prefix + ".bin"; 50 | pcl::PointCloud::Ptr raw_cloud( 51 | new pcl::PointCloud); 52 | ReadPointCloud(cloud_file_name, raw_cloud); 53 | 54 | // Publish point cloud 55 | raw_cloud->header.frame_id = "base_link"; 56 | publisher.publish(raw_cloud); 57 | } 58 | 59 | void ObjectVisualizer::ImageVisualizer(const std::string& file_prefix, 60 | const ros::Publisher publisher) { 61 | // Read image 62 | std::string image_file_name = 63 | data_path_ + dataset_ + "/image_2/" + file_prefix + ".png"; 64 | cv::Mat raw_image = cv::imread(image_file_name.c_str()); 65 | 66 | // Draw 2D bounding boxes in image 67 | Draw2DBoundingBoxes(file_prefix, raw_image); 68 | 69 | // Publish image 70 | sensor_msgs::ImagePtr raw_image_msg = 71 | cv_bridge::CvImage(std_msgs::Header(), "bgr8", raw_image).toImageMsg(); 72 | raw_image_msg->header.frame_id = "base_link"; 73 | publisher.publish(raw_image_msg); 74 | } 75 | 76 | void ObjectVisualizer::Draw2DBoundingBoxes(const std::string& file_prefix, 77 | cv::Mat& raw_image) { 78 | // Read bounding boxes data 79 | std::vector> detections = ParseDetections(file_prefix); 80 | 81 | // Draw bounding boxes in image 82 | for (const auto detection : detections) { 83 | cv::rectangle(raw_image, cv::Point(detection[3], detection[4]), 84 | cv::Point(detection[5], detection[6]), cv::Scalar(0, 255, 0), 85 | 2, 8, 0); 86 | } 87 | } 88 | 89 | void ObjectVisualizer::BoundingBoxesVisualizer(const std::string& file_prefix, 90 | const ros::Publisher publisher) { 91 | // Read bounding boxes data 92 | std::vector> detections = ParseDetections(file_prefix); 93 | 94 | // Transform bounding boxes to jsk_recognition_msgs 95 | jsk_recognition_msgs::BoundingBoxArray bounding_box_array = 96 | TransformBoundingBoxes(detections, file_prefix); 97 | 98 | // Publish bounding boxes 99 | bounding_box_array.header.frame_id = "base_link"; 100 | pub_bounding_boxes_.publish(bounding_box_array); 101 | } 102 | 103 | jsk_recognition_msgs::BoundingBoxArray ObjectVisualizer::TransformBoundingBoxes( 104 | const std::vector> detections, 105 | const std::string& file_prefix) { 106 | // Read transform matrixs from calib file 107 | std::string calib_file_name = 108 | data_path_ + dataset_ + "/calib/" + file_prefix + ".txt"; 109 | Eigen::MatrixXd trans_velo_to_cam = Eigen::MatrixXd::Identity(4, 4); 110 | ReadCalibMatrix(calib_file_name, "Tr_velo_to_cam:", trans_velo_to_cam); 111 | Eigen::MatrixXd trans_cam_to_rect = Eigen::MatrixXd::Identity(4, 4); 112 | ReadCalibMatrix(calib_file_name, "R0_rect:", trans_cam_to_rect); 113 | 114 | // Set bounding boxes to jsk_recognition_msgs::BoundingBoxArray 115 | jsk_recognition_msgs::BoundingBoxArray bounding_box_array; 116 | for (const auto detection : detections) { 117 | jsk_recognition_msgs::BoundingBox bounding_box; 118 | // Bounding box position 119 | Eigen::Vector4d rect_position(detection[10], detection[11], detection[12], 120 | 1.0); 121 | Eigen::MatrixXd velo_position = trans_velo_to_cam.inverse() * 122 | trans_cam_to_rect.inverse() * rect_position; 123 | bounding_box.pose.position.x = velo_position(0); 124 | bounding_box.pose.position.y = velo_position(1); 125 | bounding_box.pose.position.z = velo_position(2) + detection[7] / 2.0; 126 | // Bounding box orientation 127 | tf::Quaternion bounding_box_quat = 128 | tf::createQuaternionFromRPY(0.0, 0.0, 0.0 - detection[13]); 129 | tf::quaternionTFToMsg(bounding_box_quat, bounding_box.pose.orientation); 130 | // Bounding box dimensions 131 | bounding_box.dimensions.x = detection[8]; 132 | bounding_box.dimensions.y = detection[9]; 133 | bounding_box.dimensions.z = detection[7]; 134 | // Bounding box header 135 | bounding_box.header.stamp = ros::Time::now(); 136 | bounding_box.header.frame_id = "base_link"; 137 | bounding_box_array.boxes.push_back(bounding_box); 138 | } 139 | 140 | return bounding_box_array; 141 | } 142 | 143 | std::vector> ObjectVisualizer::ParseDetections( 144 | const std::string& file_prefix) { 145 | // Open bounding boxes file 146 | std::string detections_file_name; 147 | if (dataset_ == "training") { 148 | detections_file_name = 149 | data_path_ + dataset_ + "/label_2/" + file_prefix + ".txt"; 150 | } else if (dataset_ == "testing") { 151 | detections_file_name = 152 | data_path_ + dataset_ + "/results/" + file_prefix + ".txt"; 153 | } 154 | std::ifstream detections_file(detections_file_name); 155 | if (!detections_file) { 156 | ROS_ERROR("File %s does not exist", detections_file_name.c_str()); 157 | ros::shutdown(); 158 | } 159 | 160 | // Parse objects data 161 | std::vector> detections; 162 | std::string line_str; 163 | while (getline(detections_file, line_str)) { 164 | // Store std::string into std::stringstream 165 | std::stringstream line_ss(line_str); 166 | // Parse object type 167 | std::string object_type; 168 | getline(line_ss, object_type, ' '); 169 | if (object_type == "DontCare") continue; 170 | // Parse object data 171 | std::vector detection; 172 | std::string str; 173 | while (getline(line_ss, str, ' ')) { 174 | detection.push_back(boost::lexical_cast(str)); 175 | } 176 | detections.push_back(detection); 177 | } 178 | 179 | return detections; 180 | } 181 | 182 | void ObjectVisualizer::CommandButtonCallback( 183 | const std_msgs::String::ConstPtr& in_command) { 184 | // Parse frame number form command 185 | if (in_command->data == "Next") { 186 | current_frame_ = (frame_size_ + current_frame_ + 1) % frame_size_; 187 | } else if (in_command->data == "Prev") { 188 | current_frame_ = (frame_size_ + current_frame_ - 1) % frame_size_; 189 | } else { 190 | int frame = std::stoi(in_command->data); 191 | if (frame >= 0 && frame < frame_size_) 192 | current_frame_ = frame; 193 | else 194 | ROS_ERROR("No frame %s", in_command->data.c_str()); 195 | } 196 | 197 | // Visualize object data 198 | Visualizer(); 199 | } 200 | 201 | void ObjectVisualizer::AssertFilesNumber() { 202 | // Assert velodyne files numbers 203 | ROS_ASSERT(FolderFilesNumber(data_path_ + dataset_ + "/velodyne") == 204 | frame_size_); 205 | // Assert image_2 files numbers 206 | ROS_ASSERT(FolderFilesNumber(data_path_ + dataset_ + "/image_2") == 207 | frame_size_); 208 | // Assert calib files numbers 209 | ROS_ASSERT(FolderFilesNumber(data_path_ + dataset_ + "/calib") == 210 | frame_size_); 211 | if (dataset_ == "training") { 212 | // Assert label_2 files numbers 213 | ROS_ASSERT(FolderFilesNumber(data_path_ + dataset_ + "/label_2") == 214 | frame_size_); 215 | } else if (dataset_ == "testing") { 216 | // Assert results files numbers 217 | ROS_ASSERT(FolderFilesNumber(data_path_ + dataset_ + "/results") == 218 | frame_size_); 219 | } else { 220 | ROS_ERROR("Dataset input error: %s", dataset_.c_str()); 221 | ros::shutdown(); 222 | } 223 | } 224 | } 225 | -------------------------------------------------------------------------------- /src/object_visualizer/object_visualizer_node.cc: -------------------------------------------------------------------------------- 1 | #include "object_visualizer/object_visualizer.h" 2 | 3 | int main(int argc, char *argv[]) { 4 | ros::init(argc, argv, "object_visualizer_node"); 5 | 6 | ros::NodeHandle nh; 7 | ros::NodeHandle pnh("~"); 8 | 9 | kitti_visualizer::ObjectVisualizer object_visualizer(nh, pnh); 10 | 11 | ros::Duration(3).sleep(); 12 | object_visualizer.Visualizer(); 13 | 14 | ros::spin(); 15 | 16 | return 0; 17 | } 18 | -------------------------------------------------------------------------------- /src/track_visualizer/track_visualizer.cc: -------------------------------------------------------------------------------- 1 | #include "track_visualizer/track_visualizer.h" 2 | 3 | namespace kitti_visualizer { 4 | 5 | TrackVisualizer::TrackVisualizer(ros::NodeHandle nh, ros::NodeHandle pnh) 6 | : nh_(nh), pnh_(pnh) { 7 | pnh_.param("data_path", data_path_, ""); 8 | pnh_.param("dataset", dataset_, ""); 9 | pnh_.param("scene", scene_, ""); 10 | pnh_.param("current_frame", current_frame_, 0); 11 | 12 | // Subscriber 13 | sub_command_button_ = 14 | nh_.subscribe("/kitti_visualizer/command_button", 2, 15 | &TrackVisualizer::CommandButtonCallback, this); 16 | 17 | // Publisher 18 | pub_point_cloud_ = nh_.advertise>( 19 | "kitti_visualizer/object/point_cloud", 2); 20 | pub_image_ = 21 | nh_.advertise("kitti_visualizer/object/image", 2); 22 | pub_bounding_boxes_ = nh_.advertise( 23 | "kitti_visualizer/object/bounding_boxes", 2); 24 | pub_tracking_result_ = nh_.advertise( 25 | "kitti_visualizer/object/tracking_result", 2); 26 | } 27 | 28 | void TrackVisualizer::Visualizer(const int& frame) { 29 | // Get current file name 30 | std::ostringstream file_prefix; 31 | file_prefix << std::setfill('0') << std::setw(6) << frame; 32 | ROS_INFO("Visualizing frame %s ...", file_prefix.str().c_str()); 33 | 34 | // Visualize point cloud 35 | PointCloudVisualizer(file_prefix.str(), pub_point_cloud_); 36 | 37 | // Visualize image 38 | ImageVisualizer(file_prefix.str(), pub_image_); 39 | 40 | // Visualize 3D bounding boxes 41 | BoundingBoxesVisualizer(file_prefix.str(), "/label_02/", pub_bounding_boxes_); 42 | 43 | // Visualize tracking result 44 | BoundingBoxesVisualizer(file_prefix.str(), "/AB3DMOT/", pub_tracking_result_); 45 | } 46 | 47 | void TrackVisualizer::PointCloudVisualizer(const std::string& file_prefix, 48 | const ros::Publisher publisher) { 49 | // Read point cloud 50 | std::string cloud_file_name = data_path_ + dataset_ + "/velodyne/" + scene_ + 51 | "/" + file_prefix + ".bin"; 52 | pcl::PointCloud::Ptr raw_cloud( 53 | new pcl::PointCloud); 54 | ReadPointCloud(cloud_file_name, raw_cloud); 55 | 56 | // Publish point cloud 57 | raw_cloud->header.frame_id = "base_link"; 58 | publisher.publish(raw_cloud); 59 | } 60 | 61 | void TrackVisualizer::ImageVisualizer(const std::string& file_prefix, 62 | const ros::Publisher publisher) { 63 | // Read image 64 | std::string image_file_name = data_path_ + dataset_ + "/image_02/" + scene_ + 65 | "/" + file_prefix + ".png"; 66 | cv::Mat raw_image = cv::imread(image_file_name.c_str()); 67 | 68 | // Draw 2D bounding boxes in image 69 | Draw2DBoundingBoxes(file_prefix, "/label_02/", raw_image); 70 | 71 | // Publish image 72 | sensor_msgs::ImagePtr raw_image_msg = 73 | cv_bridge::CvImage(std_msgs::Header(), "bgr8", raw_image).toImageMsg(); 74 | raw_image_msg->header.frame_id = "base_link"; 75 | publisher.publish(raw_image_msg); 76 | } 77 | 78 | void TrackVisualizer::Draw2DBoundingBoxes(const std::string& file_prefix, 79 | const std::string& folder, 80 | cv::Mat& raw_image) { 81 | // Read bounding boxes data 82 | std::vector> tracks = ParseTracks(file_prefix, folder); 83 | 84 | // Draw bounding boxes in image 85 | for (const auto track : tracks) { 86 | cv::rectangle(raw_image, cv::Point(track[4], track[5]), 87 | cv::Point(track[6], track[7]), cv::Scalar(0, 255, 0), 2, 8, 88 | 0); 89 | } 90 | } 91 | 92 | void TrackVisualizer::BoundingBoxesVisualizer(const std::string& file_prefix, 93 | const std::string& folder, 94 | const ros::Publisher publisher) { 95 | // Read bounding boxes data 96 | std::vector> tracks = ParseTracks(file_prefix, folder); 97 | 98 | // Transform bounding boxes to jsk_recognition_msgs 99 | jsk_recognition_msgs::BoundingBoxArray bounding_box_array = 100 | TransformBoundingBoxes(tracks, file_prefix); 101 | 102 | // Publish bounding boxes 103 | bounding_box_array.header.frame_id = "base_link"; 104 | publisher.publish(bounding_box_array); 105 | } 106 | 107 | jsk_recognition_msgs::BoundingBoxArray TrackVisualizer::TransformBoundingBoxes( 108 | const std::vector> tracks, 109 | const std::string& file_prefix) { 110 | // Read transform matrixs from calib file 111 | std::string calib_file_name = 112 | data_path_ + dataset_ + "/calib/" + scene_ + ".txt"; 113 | Eigen::MatrixXd trans_velo_to_cam = Eigen::MatrixXd::Identity(4, 4); 114 | ReadCalibMatrix(calib_file_name, "Tr_velo_cam", trans_velo_to_cam); 115 | Eigen::MatrixXd trans_cam_to_rect = Eigen::MatrixXd::Identity(4, 4); 116 | ReadCalibMatrix(calib_file_name, "R_rect", trans_cam_to_rect); 117 | 118 | // Set bounding boxes to jsk_recognition_msgs::BoundingBoxArray 119 | jsk_recognition_msgs::BoundingBoxArray bounding_box_array; 120 | for (const auto track : tracks) { 121 | jsk_recognition_msgs::BoundingBox bounding_box; 122 | // Bounding box id 123 | bounding_box.label = static_cast(track[0]); 124 | // Bounding box position 125 | Eigen::Vector4d rect_position(track[11], track[12], track[13], 1.0); 126 | Eigen::MatrixXd velo_position = trans_velo_to_cam.inverse() * 127 | trans_cam_to_rect.inverse() * rect_position; 128 | bounding_box.pose.position.x = velo_position(0); 129 | bounding_box.pose.position.y = velo_position(1); 130 | bounding_box.pose.position.z = velo_position(2) + track[8] / 2.0; 131 | // Bounding box orientation 132 | tf::Quaternion bounding_box_quat = 133 | tf::createQuaternionFromRPY(0.0, 0.0, 0.0 - track[14]); 134 | tf::quaternionTFToMsg(bounding_box_quat, bounding_box.pose.orientation); 135 | // Bounding box dimensions 136 | bounding_box.dimensions.x = track[9]; 137 | bounding_box.dimensions.y = track[10]; 138 | bounding_box.dimensions.z = track[8]; 139 | // Bounding box header 140 | bounding_box.header.stamp = ros::Time::now(); 141 | bounding_box.header.frame_id = "base_link"; 142 | bounding_box_array.boxes.push_back(bounding_box); 143 | } 144 | 145 | return bounding_box_array; 146 | } 147 | 148 | std::vector> TrackVisualizer::ParseTracks( 149 | const std::string& file_prefix, const std::string& folder) { 150 | // Open bounding boxes file 151 | std::string tracks_file_name; 152 | if (dataset_ == "training") { 153 | tracks_file_name = data_path_ + dataset_ + folder + scene_ + ".txt"; 154 | } else if (dataset_ == "testing") { 155 | tracks_file_name = 156 | data_path_ + dataset_ + "/results/" + file_prefix + ".txt"; 157 | } 158 | std::ifstream tracks_file(tracks_file_name); 159 | if (!tracks_file) { 160 | ROS_ERROR("File %s does not exist", tracks_file_name.c_str()); 161 | ros::shutdown(); 162 | } 163 | 164 | // Parse tracks data 165 | std::vector> tracks; 166 | std::string line_str; 167 | while (getline(tracks_file, line_str)) { 168 | // Store std::string into std::stringstream 169 | std::stringstream line_ss(line_str); 170 | // Parse frame number 171 | std::string frame; 172 | getline(line_ss, frame, ' '); 173 | if (boost::lexical_cast(frame) != 174 | boost::lexical_cast(file_prefix)) 175 | continue; 176 | // Parse object data 177 | std::vector track; 178 | // Parse object id 179 | std::string object_id; 180 | getline(line_ss, object_id, ' '); 181 | if (boost::lexical_cast(object_id) < 0) continue; 182 | track.push_back(boost::lexical_cast(object_id)); 183 | // Parse object type 184 | std::string object_type; 185 | getline(line_ss, object_type, ' '); 186 | if (object_type == "DontCare") continue; 187 | std::string str; 188 | while (getline(line_ss, str, ' ')) { 189 | track.push_back(boost::lexical_cast(str)); 190 | } 191 | tracks.push_back(track); 192 | } 193 | 194 | return tracks; 195 | } 196 | 197 | void TrackVisualizer::CommandButtonCallback( 198 | const std_msgs::String::ConstPtr& in_command) { 199 | // Parse frame number form command 200 | if (in_command->data == "Next") { 201 | current_frame_ = (frame_size_ + current_frame_ + 1) % frame_size_; 202 | } else if (in_command->data == "Prev") { 203 | current_frame_ = (frame_size_ + current_frame_ - 1) % frame_size_; 204 | } else { 205 | int frame = std::stoi(in_command->data); 206 | if (frame >= 0 && frame < frame_size_) 207 | current_frame_ = frame; 208 | else 209 | ROS_ERROR("No frame %s", in_command->data.c_str()); 210 | } 211 | 212 | // Visualize object data 213 | Visualizer(current_frame_); 214 | } 215 | 216 | int TrackVisualizer::GetFrameNumber() { 217 | // Get velodyne frame number 218 | int velo_frame_num = 219 | FolderFilesNumber(data_path_ + dataset_ + "/velodyne/" + scene_ + "/"); 220 | 221 | // Get image_2 frame number 222 | int image_frame_num = 223 | FolderFilesNumber(data_path_ + dataset_ + "/image_02/" + scene_ + "/"); 224 | 225 | // Assert velodyne and image 226 | ROS_ASSERT(velo_frame_num == image_frame_num); 227 | 228 | // Assign 229 | frame_size_ = velo_frame_num; 230 | 231 | return velo_frame_num; 232 | } 233 | } 234 | -------------------------------------------------------------------------------- /src/track_visualizer/track_visualizer_node.cc: -------------------------------------------------------------------------------- 1 | #include "track_visualizer/track_visualizer.h" 2 | 3 | int main(int argc, char *argv[]) { 4 | ros::init(argc, argv, "track_visualizer_node"); 5 | 6 | ros::NodeHandle nh; 7 | ros::NodeHandle pnh("~"); 8 | 9 | kitti_visualizer::TrackVisualizer track_visualizer(nh, pnh); 10 | 11 | // get frame number 12 | int frame_size = track_visualizer.GetFrameNumber(); 13 | 14 | ros::Rate loop_rate(20); 15 | int frame = 0; 16 | // while (ros::ok()) { 17 | // track_visualizer.Visualizer(frame); 18 | // frame = (++frame) % frame_size; 19 | 20 | // loop_rate.sleep(); 21 | // } 22 | 23 | track_visualizer.Visualizer(frame); 24 | ros::spin(); 25 | 26 | return 0; 27 | } 28 | --------------------------------------------------------------------------------