diff --git a/.gitignore b/.gitignore new file mode 100644 index 000000000..dbe9c82b3 --- /dev/null +++ b/.gitignore @@ -0,0 +1 @@ +.vscode/ \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt index 2bd3bcc55..20f088f73 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -11,8 +11,12 @@ find_package(catkin REQUIRED COMPONENTS sensor_msgs roscpp rospy + rosbag std_msgs - tf) + image_transport + cv_bridge + tf +) #find_package(Eigen3 REQUIRED) find_package(PCL REQUIRED) @@ -23,7 +27,8 @@ include_directories( include ${catkin_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS} - ${CERES_INCLUDE_DIRS}) + ${CERES_INCLUDE_DIRS} + ${OpenCV_INCLUDE_DIRS}) catkin_package( CATKIN_DEPENDS geometry_msgs nav_msgs roscpp rospy std_msgs @@ -41,6 +46,9 @@ target_link_libraries(laserOdometry ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${CERES add_executable(laserMapping src/laserMapping.cpp) target_link_libraries(laserMapping ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${CERES_LIBRARIES}) +add_executable(kittiHelper src/kittiHelper.cpp) +target_link_libraries(kittiHelper ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBS}) + diff --git a/README.md b/README.md index f2d2c2c5d..cceaf72bc 100644 --- a/README.md +++ b/README.md @@ -5,7 +5,7 @@ A-LOAM is an Advanced implenmentation of LOAM (J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time), which uses Eigen and Ceres to Solver to simplify code structure. This code is modified from [LOAM_NOTED](https://github.com/cuitaixiang/LOAM_NOTED). -**Authors:** [Tong Qin](http://www.qintonguav.com), [Shaozu Cao](https://github.com/shaozu/LOAM_NOTED), and [Shaojie Shen](http://www.ece.ust.hk/ece.php/profile/facultydetail/eeshaojie) from the [Aerial Robotics Group](http://uav.ust.hk/), [HKUST](https://www.ust.hk/) +**Authors:** [Tong Qin](http://www.qintonguav.com), [Shaozu Cao](https://github.com/shaozu), and [Shaojie Shen](http://www.ece.ust.hk/ece.php/profile/facultydetail/eeshaojie) from the [Aerial Robotics Group](http://uav.ust.hk/), [HKUST](https://www.ust.hk/) ## 1. Prerequisites @@ -42,10 +42,10 @@ Download [NSH indoor outdoor](https://drive.google.com/file/d/1s05tBQOLNEDDurlg4 ## 4. KITTI Example (Velodyne HDL-64) -Download [KITTI Odometry dataset](http://www.cvlibs.net/datasets/kitti/eval_odometry.php) to YOUR_DATASET_FOLDER, and convert it into ROS bag. We take sequences 00 for example. [00](download link) +Download [KITTI Odometry dataset](http://www.cvlibs.net/datasets/kitti/eval_odometry.php) to YOUR_DATASET_FOLDER and set the `dataset_folder` and `sequence_number` parameters in `kitti_helper.launch` file. Note you also convert KITTI dataset to bag file for easy use by setting proper parameters in `kitti_helper.launch`. ``` roslaunch loam_velodyne loam_velodyne_64.launch - rosbag play YOUR_DATASET_FOLDER/00.bag + roslaunch loam_velodyne kitti_helper.launch ``` diff --git a/launch/kitti_helper.launch b/launch/kitti_helper.launch new file mode 100644 index 000000000..cef3048f7 --- /dev/null +++ b/launch/kitti_helper.launch @@ -0,0 +1,11 @@ + + + + + + + + + + + \ No newline at end of file diff --git a/launch/loam_velodyne_16.launch b/launch/loam_velodyne_16.launch index f0e637fd1..f8362488e 100644 --- a/launch/loam_velodyne_16.launch +++ b/launch/loam_velodyne_16.launch @@ -18,9 +18,9 @@ - + - + diff --git a/launch/loam_velodyne_64.launch b/launch/loam_velodyne_64.launch index 013c11042..bda803420 100644 --- a/launch/loam_velodyne_64.launch +++ b/launch/loam_velodyne_64.launch @@ -18,9 +18,9 @@ - + - + diff --git a/package.xml b/package.xml index f36971de9..0382fe84a 100644 --- a/package.xml +++ b/package.xml @@ -21,9 +21,11 @@ nav_msgs roscpp rospy - std_msgs + std_msgs + rosbag sensor_msgs tf + image_transport geometry_msgs nav_msgs @@ -31,7 +33,9 @@ roscpp rospy std_msgs + rosbag tf + image_transport diff --git a/rviz_cfg/loam_velodyne.rviz b/rviz_cfg/loam_velodyne_16.rviz similarity index 100% rename from rviz_cfg/loam_velodyne.rviz rename to rviz_cfg/loam_velodyne_16.rviz diff --git a/rviz_cfg/loam_velodyne_64.rviz b/rviz_cfg/loam_velodyne_64.rviz new file mode 100644 index 000000000..f0753c885 --- /dev/null +++ b/rviz_cfg/loam_velodyne_64.rviz @@ -0,0 +1,742 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /odometry1/odomPath1 + - /odometry1/PointCloud21 + - /Image1 + - /Path1 + Splitter Ratio: 0.5 + Tree Height: 764 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.588679016 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: surround +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.0299999993 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 160 + Reference Frame: + Value: true + - Class: rviz/Axes + Enabled: true + Length: 1 + Name: Axes + Radius: 0.100000001 + Reference Frame: + Value: true + - Class: rviz/Group + Displays: + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 255; 170; 0 + Enabled: false + Head Diameter: 0.300000012 + Head Length: 0.200000003 + Length: 0.300000012 + Line Style: Lines + Line Width: 0.0299999993 + Name: gtPathlc + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.0299999993 + Shaft Diameter: 0.100000001 + Shaft Length: 0.100000001 + Topic: /path_gt + Unreliable: false + Value: false + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 255; 170; 0 + Enabled: true + Head Diameter: 0.300000012 + Head Length: 0.200000003 + Length: 0.300000012 + Line Style: Lines + Line Width: 0.0299999993 + Name: gtPathLidar + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.0299999993 + Shaft Diameter: 0.100000001 + Shaft Length: 0.100000001 + Topic: /path_gt_lidar + Unreliable: false + Value: true + Enabled: false + Name: GT + - Class: rviz/Group + Displays: + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 255; 85; 0 + Enabled: true + Head Diameter: 0.300000012 + Head Length: 0.200000003 + Length: 0.300000012 + Line Style: Lines + Line Width: 0.100000001 + Name: odomPath + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.0299999993 + Shaft Diameter: 0.100000001 + Shaft Length: 0.100000001 + Topic: /laser_odom_path + Unreliable: false + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 63.1004791 + Min Color: 0; 0; 0 + Min Intensity: -0.00678629428 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.0500000007 + Style: Flat Squares + Topic: /velodyne_cloud_2 + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + Enabled: false + Name: odometry + - Class: rviz/Group + Displays: + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 25; 255; 0 + Enabled: true + Head Diameter: 0.300000012 + Head Length: 0.200000003 + Length: 0.300000012 + Line Style: Lines + Line Width: 0.0299999993 + Name: mappingPath + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.0299999993 + Shaft Diameter: 0.100000001 + Shaft Length: 0.100000001 + Topic: /aft_mapped_path + Unreliable: false + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: -999999 + Min Color: 0; 0; 0 + Min Intensity: 999999 + Name: allMapCloud + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.0500000007 + Style: Flat Squares + Topic: /laser_cloud_map + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 15 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: surround + Position Transformer: XYZ + Queue Size: 1 + Selectable: true + Size (Pixels): 1 + Size (m): 0.0500000007 + Style: Squares + Topic: /laser_cloud_surround + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 50.143261 + Min Color: 0; 0; 0 + Min Intensity: -0.0299073197 + Name: currPoints + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 2 + Size (m): 0.00999999978 + Style: Points + Topic: /velodyne_cloud_registered + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 0; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 63.0384331 + Min Color: 255; 0; 0 + Min Intensity: -0.00643126154 + Name: corner + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.200000003 + Style: Flat Squares + Topic: /mapping_corner + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 0; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 63.0818977 + Min Color: 0; 0; 0 + Min Intensity: -0.00524160406 + Name: surf + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.200000003 + Style: Flat Squares + Topic: /mapping_surf + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 0; 255; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 17.0880051 + Min Color: 0; 0; 0 + Min Intensity: -0.00679048989 + Name: used_corner + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.5 + Style: Flat Squares + Topic: /mapping_used_corner + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 0; 255; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 0; 255; 0 + Max Intensity: 63.0818977 + Min Color: 0; 0; 0 + Min Intensity: -0.00524160406 + Name: used_surf + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.5 + Style: Flat Squares + Topic: /mapping_used_surf + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 21.0628471 + Min Color: 0; 0; 0 + Min Intensity: -0.00662572403 + Name: map_corner + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.100000001 + Style: Flat Squares + Topic: /mapping_map_corner + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 63.0839233 + Min Color: 0; 0; 0 + Min Intensity: -0.00876065157 + Name: map_surf + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.100000001 + Style: Flat Squares + Topic: /mapping_map_surf + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 25; 255; 0 + Enabled: false + Head Diameter: 0.300000012 + Head Length: 0.200000003 + Length: 0.300000012 + Line Style: Lines + Line Width: 0.0299999993 + Name: leftcameraPath + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.0299999993 + Shaft Diameter: 0.100000001 + Shaft Length: 0.100000001 + Topic: /lc_path + Unreliable: false + Value: false + Enabled: true + Name: mapping + - Class: rviz/Group + Displays: + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 15 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: sharp + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.00999999978 + Style: Points + Topic: /laser_cloud_sharp + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 63.0398712 + Min Color: 0; 0; 0 + Min Intensity: 0.0263746977 + Name: flat + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.0500000007 + Style: Flat Squares + Topic: /laser_cloud_flat + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 0.99000001 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: raw_data + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.0500000007 + Style: Flat Squares + Topic: /velodyne_points + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 0; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 0.123617291 + Min Color: 0; 0; 0 + Min Intensity: -0.00711920578 + Name: scan + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.200000003 + Style: Flat Squares + Topic: /laser_scanid_63 + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 85; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 62.075676 + Min Color: 0; 0; 0 + Min Intensity: -0.00705419574 + Name: removPoints + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.200000003 + Style: Flat Squares + Topic: /laser_remove_points + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + Enabled: false + Name: scan + - Class: rviz/Image + Enabled: true + Image Topic: /image_left + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 255; 0; 0 + Enabled: true + Head Diameter: 0.300000012 + Head Length: 0.200000003 + Length: 0.300000012 + Line Style: Lines + Line Width: 0.0299999993 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.0299999993 + Shaft Diameter: 0.100000001 + Shaft Length: 0.100000001 + Topic: /path_gt + Unreliable: false + Value: true + Enabled: true + Global Options: + Background Color: 0; 0; 0 + Default Light: true + Fixed Frame: camera_init + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Topic: /initialpose + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/XYOrbit + Distance: 185.440704 + Enable Stereo Rendering: + Stereo Eye Separation: 0.0599999987 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 5.38311291 + Y: 6.32670021 + Z: -1.88172344e-05 + Focal Shape Fixed Size: true + Focal Shape Size: 0.0500000007 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.00999999978 + Pitch: 0.834795713 + Target Frame: + Value: XYOrbit (rviz) + Yaw: 3.24999619 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1410 + Hide Left Dock: false + Hide Right Dock: false + Image: + collapsed: false + QMainWindow State: 000000ff00000000fd000000040000000000000179000004ecfc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006300fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002e00000391000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650100000215000000ca0000000000000000fb0000000a0049006d00610067006501000002e6000000180000000000000000fb0000000a0049006d00610067006501000003c6000001540000001800ffffff0000000100000111000004ecfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002e000004ec000000b700fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000a0000000040fc0100000002fb0000000800540069006d0065010000000000000a000000038300fffffffb0000000800540069006d0065010000000000000450000000000000000000000768000004ec00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 2560 + X: 0 + Y: 30 diff --git a/src/kittiHelper.cpp b/src/kittiHelper.cpp new file mode 100644 index 000000000..130aaf407 --- /dev/null +++ b/src/kittiHelper.cpp @@ -0,0 +1,178 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +std::vector read_lidar_data(const std::string lidar_data_path) +{ + std::ifstream lidar_data_file(lidar_data_path, std::ifstream::in | std::ifstream::binary); + lidar_data_file.seekg(0, std::ios::end); + const size_t num_elements = lidar_data_file.tellg() / sizeof(float); + lidar_data_file.seekg(0, std::ios::beg); + + std::vector lidar_data_buffer(num_elements); + lidar_data_file.read(reinterpret_cast(&lidar_data_buffer[0]), num_elements*sizeof(float)); + return lidar_data_buffer; +} + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "kitti_helper"); + ros::NodeHandle n("~"); + std::string dataset_folder, sequence_number, output_bag_file; + n.getParam("dataset_folder", dataset_folder); + n.getParam("sequence_number", sequence_number); + std::cout << "Reading sequence " << sequence_number << " from " << dataset_folder << '\n'; + bool to_bag; + n.getParam("to_bag", to_bag); + if (to_bag) + n.getParam("output_bag_file", output_bag_file); + int publish_delay; + n.getParam("publish_delay", publish_delay); + publish_delay = publish_delay <= 0 ? 1 : publish_delay; + + ros::Publisher pub_laser_cloud = n.advertise("/velodyne_points", 2); + + image_transport::ImageTransport it(n); + image_transport::Publisher pub_image_left = it.advertise("/image_left", 2); + image_transport::Publisher pub_image_right = it.advertise("/image_right", 2); + + ros::Publisher pubOdomGT = n.advertise ("/odometry_gt", 5); + nav_msgs::Odometry odomGT; + odomGT.header.frame_id = "/camera_init"; + odomGT.child_frame_id = "/ground_truth"; + + ros::Publisher pubPathGT = n.advertise ("/path_gt", 5); + nav_msgs::Path pathGT; + pathGT.header.frame_id = "/camera_init"; + + std::string timestamp_path = "sequences/" + sequence_number + "/times.txt"; + std::ifstream timestamp_file(dataset_folder + timestamp_path, std::ifstream::in); + + std::string ground_truth_path = "results/" + sequence_number + ".txt"; + std::ifstream ground_truth_file(dataset_folder + ground_truth_path, std::ifstream::in); + + rosbag::Bag bag_out; + if (to_bag) + bag_out.open(output_bag_file, rosbag::bagmode::Write); + + Eigen::Matrix3d R_transform; + R_transform << 0, 0, 1, -1, 0, 0, 0, -1, 0; + Eigen::Quaterniond q_transform(R_transform); + + std::string line; + std::size_t line_num = 0; + + ros::Rate r(10.0 / publish_delay); + while (std::getline(timestamp_file, line) && ros::ok()) + { + float timestamp = stof(line); + std::stringstream left_image_path, right_image_path; + left_image_path << dataset_folder << "sequences/" + sequence_number + "/image_0/" << std::setfill('0') << std::setw(6) << line_num << ".png"; + cv::Mat left_image = cv::imread(left_image_path.str(), CV_LOAD_IMAGE_GRAYSCALE); + right_image_path << dataset_folder << "sequences/" + sequence_number + "/image_1/" << std::setfill('0') << std::setw(6) << line_num << ".png"; + cv::Mat right_image = cv::imread(left_image_path.str(), CV_LOAD_IMAGE_GRAYSCALE); + + std::getline(ground_truth_file, line); + std::stringstream pose_stream(line); + std::string s; + Eigen::Matrix gt_pose; + for (std::size_t i = 0; i < 3; ++i) + { + for (std::size_t j = 0; j < 4; ++j) + { + std::getline(pose_stream, s, ' '); + gt_pose(i, j) = stof(s); + } + } + + Eigen::Quaterniond q_w_i(gt_pose.topLeftCorner<3, 3>()); + Eigen::Quaterniond q = q_transform * q_w_i; + q.normalize(); + Eigen::Vector3d t = q_transform * gt_pose.topRightCorner<3, 1>(); + + odomGT.header.stamp = ros::Time().fromSec(timestamp); + odomGT.pose.pose.orientation.x = q.x(); + odomGT.pose.pose.orientation.y = q.y(); + odomGT.pose.pose.orientation.z = q.z(); + odomGT.pose.pose.orientation.w = q.w(); + odomGT.pose.pose.position.x = t(0); + odomGT.pose.pose.position.y = t(1); + odomGT.pose.pose.position.z = t(2); + pubOdomGT.publish(odomGT); + + geometry_msgs::PoseStamped poseGT; + poseGT.header = odomGT.header; + poseGT.pose = odomGT.pose.pose; + pathGT.header.stamp = odomGT.header.stamp; + pathGT.poses.push_back(poseGT); + pubPathGT.publish(pathGT); + + // read lidar point cloud + std::stringstream lidar_data_path; + lidar_data_path << dataset_folder << "velodyne/sequences/" + sequence_number + "/velodyne/" + << std::setfill('0') << std::setw(6) << line_num << ".bin"; + std::vector lidar_data = read_lidar_data(lidar_data_path.str()); + std::cout << "totally " << lidar_data.size() / 4.0 << " points in this lidar frame \n"; + + std::vector lidar_points; + std::vector lidar_intensities; + pcl::PointCloud laser_cloud; + for (std::size_t i = 0; i < lidar_data.size(); i += 4) + { + lidar_points.emplace_back(lidar_data[i], lidar_data[i+1], lidar_data[i+2]); + lidar_intensities.push_back(lidar_data[i+3]); + + pcl::PointXYZI point; + point.x = lidar_data[i]; + point.y = lidar_data[i + 1]; + point.z = lidar_data[i + 2]; + point.intensity = lidar_data[i + 3]; + laser_cloud.push_back(point); + } + + sensor_msgs::PointCloud2 laser_cloud_msg; + pcl::toROSMsg(laser_cloud, laser_cloud_msg); + laser_cloud_msg.header.stamp = ros::Time().fromSec(timestamp); + laser_cloud_msg.header.frame_id = "/camera_init"; + pub_laser_cloud.publish(laser_cloud_msg); + + sensor_msgs::ImagePtr image_left_msg = cv_bridge::CvImage(laser_cloud_msg.header, "mono8", left_image).toImageMsg(); + sensor_msgs::ImagePtr image_right_msg = cv_bridge::CvImage(laser_cloud_msg.header, "mono8", right_image).toImageMsg(); + pub_image_left.publish(image_left_msg); + pub_image_right.publish(image_right_msg); + + if (to_bag) + { + bag_out.write("/image_left", ros::Time::now(), image_left_msg); + bag_out.write("/image_right", ros::Time::now(), image_right_msg); + bag_out.write("/velodyne_points", ros::Time::now(), laser_cloud_msg); + bag_out.write("/path_gt", ros::Time::now(), pathGT); + bag_out.write("/odometry_gt", ros::Time::now(), odomGT); + } + + line_num ++; + r.sleep(); + } + bag_out.close(); + std::cout << "Done \n"; + + + return 0; +} \ No newline at end of file diff --git a/src/scanRegistration.cpp b/src/scanRegistration.cpp index 4c22d98bf..3cc32c355 100755 --- a/src/scanRegistration.cpp +++ b/src/scanRegistration.cpp @@ -36,7 +36,6 @@ #include #include "loam_velodyne/common.h" #include "loam_velodyne/tic_toc.h" -#include #include #include #include