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