Skip to content

Commit 49f1659

Browse files
AaronsweiAaronswei
committedSep 25, 2021
add lidar_imu_fusion
1 parent 4557ac6 commit 49f1659

25 files changed

+7217
-0
lines changed
 

‎launch/hs5.launch

+23
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,23 @@
1+
<launch>
2+
<arg name="config_path" default = "$(find feature_tracker)/../config/hs5_config.yaml" />
3+
<arg name="vins_path" default = "$(find feature_tracker)/../config/../" />
4+
5+
<node name="feature_tracker" pkg="feature_tracker" type="feature_tracker" output="log">
6+
<param name="config_file" type="string" value="$(arg config_path)" />
7+
<param name="vins_folder" type="string" value="$(arg vins_path)" />
8+
</node>
9+
10+
<node name="vins_estimator" pkg="vins_estimator" type="vins_estimator" output="screen">
11+
<param name="config_file" type="string" value="$(arg config_path)" />
12+
<param name="vins_folder" type="string" value="$(arg vins_path)" />
13+
</node>
14+
15+
<node name="pose_graph" pkg="pose_graph" type="pose_graph" output="screen">
16+
<param name="config_file" type="string" value="$(arg config_path)" />
17+
<param name="visualization_shift_x" type="int" value="0" />
18+
<param name="visualization_shift_y" type="int" value="0" />
19+
<param name="skip_cnt" type="int" value="0" />
20+
<param name="skip_dis" type="double" value="0" />
21+
</node>
22+
23+
</launch>

‎launch/module_robot_state_publisher.launch

+11
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,11 @@
1+
<launch>
2+
3+
<arg name="project" default="lidar_imu_fusion"/>
4+
5+
<param name="robot_description" command="$(find xacro)/xacro $(find lidar_imu_fusion)/config/robot.urdf.xacro --inorder" />
6+
7+
<node pkg="robot_state_publisher" type="robot_state_publisher" name="$(arg project)_robot_state_publisher" respawn="true">
8+
<!-- <param name="tf_prefix" value="$(env ROS_HOSTNAME)"/> -->
9+
</node>
10+
11+
</launch>

‎launch/module_rviz.launch

+8
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,8 @@
1+
<launch>
2+
3+
<arg name="project" default="lidar_imu_fusion"/>
4+
5+
<!--- Run Rviz-->
6+
<node pkg="rviz" type="rviz" name="$(arg project)_rviz" args="-d $(find lidar_imu_fusion)/config/rviz.rviz" />
7+
8+
</launch>

‎launch/module_sam.launch

+19
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,19 @@
1+
<launch>
2+
3+
<arg name="project" default="lidar_imu_fusion"/>
4+
<!-- Lidar odometry param -->
5+
<rosparam file="$(find lidar_imu_fusion)/config/params_lidar_hs5.yaml" command="load" />
6+
<!-- VINS config -->
7+
<param name="vins_config_file" type="string" value="$(find lidar_imu_fusion)/config/params_camera_hs5.yaml" />
8+
9+
<!-- Lidar odometry -->
10+
<node pkg="$(arg project)" type="$(arg project)_imuPreintegration" name="$(arg project)_imuPreintegration" output="screen" respawn="true"/>
11+
<node pkg="$(arg project)" type="$(arg project)_imageProjection" name="$(arg project)_imageProjection" output="screen" respawn="true"/>
12+
<node pkg="$(arg project)" type="$(arg project)_featureExtraction" name="$(arg project)_featureExtraction" output="screen" respawn="true"/>
13+
<node pkg="$(arg project)" type="$(arg project)_mapOptmization" name="$(arg project)_mapOptmization" output="screen" respawn="true"/>
14+
<node pkg="$(arg project)" type="$(arg project)_gpsConverter" name="$(arg project)_gpsConverter" output="screen" respawn="true"/>
15+
<!-- <node pkg="$(arg project)" type="$(arg project)_imuTool" name="$(arg project)_imuTool" output="screen" respawn="true"/> -->
16+
<!-- <node pkg="$(arg project)" type="$(arg project)_lidarTool" name="$(arg project)_lidarTool" output="screen" respawn="true"/> -->
17+
<!-- <node pkg="$(arg project)" type="$(arg project)_lidarObstacle" name="$(arg project)_lidarObstacle" output="screen" respawn="true"/> -->
18+
19+
</launch>

‎launch/run_lidar_imu_fusion.launch

+12
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,12 @@
1+
<launch>
2+
3+
<!--- Robot State TF -->
4+
<include file="$(find lidar_imu_fusion)/../../launch/module_robot_state_publisher.launch" />
5+
6+
<!--- Run Rviz-->
7+
<include file="$(find lidar_imu_fusion)/../../launch/module_rviz.launch" />
8+
9+
<!--- SAM -->
10+
<include file="$(find lidar_imu_fusion)/../../launch/module_sam.launch" />
11+
12+
</launch>

‎scripts/kitti2bag.py

+326
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,326 @@
1+
#!env python
2+
# -*- coding: utf-8 -*-
3+
4+
import sys
5+
6+
try:
7+
import pykitti
8+
except ImportError as e:
9+
print('Could not load module \'pykitti\'. Please run `pip install pykitti`')
10+
sys.exit(1)
11+
12+
import tf
13+
import os
14+
import cv2
15+
import rospy
16+
import rosbag
17+
import progressbar
18+
from tf2_msgs.msg import TFMessage
19+
from datetime import datetime
20+
from std_msgs.msg import Header
21+
from sensor_msgs.msg import CameraInfo, Imu, PointField, NavSatFix
22+
import sensor_msgs.point_cloud2 as pcl2
23+
from geometry_msgs.msg import TransformStamped, TwistStamped, Transform
24+
from cv_bridge import CvBridge
25+
import numpy as np
26+
import argparse
27+
28+
def save_imu_data(bag, kitti, imu_frame_id, topic):
29+
print("Exporting IMU")
30+
for timestamp, oxts in zip(kitti.timestamps, kitti.oxts):
31+
q = tf.transformations.quaternion_from_euler(oxts.packet.roll, oxts.packet.pitch, oxts.packet.yaw)
32+
imu = Imu()
33+
imu.header.frame_id = imu_frame_id
34+
imu.header.stamp = rospy.Time.from_sec(float(timestamp.strftime("%s.%f")))
35+
imu.orientation.x = q[0]
36+
imu.orientation.y = q[1]
37+
imu.orientation.z = q[2]
38+
imu.orientation.w = q[3]
39+
imu.linear_acceleration.x = oxts.packet.af
40+
imu.linear_acceleration.y = oxts.packet.al
41+
imu.linear_acceleration.z = oxts.packet.au
42+
imu.angular_velocity.x = oxts.packet.wf
43+
imu.angular_velocity.y = oxts.packet.wl
44+
imu.angular_velocity.z = oxts.packet.wu
45+
bag.write(topic, imu, t=imu.header.stamp)
46+
47+
48+
def save_dynamic_tf(bag, kitti, kitti_type, initial_time):
49+
print("Exporting time dependent transformations")
50+
if kitti_type.find("raw") != -1:
51+
for timestamp, oxts in zip(kitti.timestamps, kitti.oxts):
52+
tf_oxts_msg = TFMessage()
53+
tf_oxts_transform = TransformStamped()
54+
tf_oxts_transform.header.stamp = rospy.Time.from_sec(float(timestamp.strftime("%s.%f")))
55+
tf_oxts_transform.header.frame_id = 'world'
56+
tf_oxts_transform.child_frame_id = 'base_link'
57+
58+
transform = (oxts.T_w_imu)
59+
t = transform[0:3, 3]
60+
q = tf.transformations.quaternion_from_matrix(transform)
61+
oxts_tf = Transform()
62+
63+
oxts_tf.translation.x = t[0]
64+
oxts_tf.translation.y = t[1]
65+
oxts_tf.translation.z = t[2]
66+
67+
oxts_tf.rotation.x = q[0]
68+
oxts_tf.rotation.y = q[1]
69+
oxts_tf.rotation.z = q[2]
70+
oxts_tf.rotation.w = q[3]
71+
72+
tf_oxts_transform.transform = oxts_tf
73+
tf_oxts_msg.transforms.append(tf_oxts_transform)
74+
75+
bag.write('/tf', tf_oxts_msg, tf_oxts_msg.transforms[0].header.stamp)
76+
77+
elif kitti_type.find("odom") != -1:
78+
timestamps = map(lambda x: initial_time + x.total_seconds(), kitti.timestamps)
79+
for timestamp, tf_matrix in zip(timestamps, kitti.T_w_cam0):
80+
tf_msg = TFMessage()
81+
tf_stamped = TransformStamped()
82+
tf_stamped.header.stamp = rospy.Time.from_sec(timestamp)
83+
tf_stamped.header.frame_id = 'world'
84+
tf_stamped.child_frame_id = 'camera_left'
85+
86+
t = tf_matrix[0:3, 3]
87+
q = tf.transformations.quaternion_from_matrix(tf_matrix)
88+
transform = Transform()
89+
90+
transform.translation.x = t[0]
91+
transform.translation.y = t[1]
92+
transform.translation.z = t[2]
93+
94+
transform.rotation.x = q[0]
95+
transform.rotation.y = q[1]
96+
transform.rotation.z = q[2]
97+
transform.rotation.w = q[3]
98+
99+
tf_stamped.transform = transform
100+
tf_msg.transforms.append(tf_stamped)
101+
102+
bag.write('/tf', tf_msg, tf_msg.transforms[0].header.stamp)
103+
104+
105+
def save_camera_data(bag, kitti_type, kitti, util, bridge, camera, camera_frame_id, topic, initial_time):
106+
print("Exporting camera {}".format(camera))
107+
if kitti_type.find("raw") != -1:
108+
camera_pad = '{0:02d}'.format(camera)
109+
image_dir = os.path.join(kitti.data_path, 'image_{}'.format(camera_pad))
110+
image_path = os.path.join(image_dir, 'data')
111+
image_filenames = sorted(os.listdir(image_path))
112+
with open(os.path.join(image_dir, 'timestamps.txt')) as f:
113+
image_datetimes = map(lambda x: datetime.strptime(x[:-4], '%Y-%m-%d %H:%M:%S.%f'), f.readlines())
114+
115+
calib = CameraInfo()
116+
calib.header.frame_id = camera_frame_id
117+
calib.width, calib.height = tuple(util['S_rect_{}'.format(camera_pad)].tolist())
118+
calib.distortion_model = 'plumb_bob'
119+
calib.K = util['K_{}'.format(camera_pad)]
120+
calib.R = util['R_rect_{}'.format(camera_pad)]
121+
calib.D = util['D_{}'.format(camera_pad)]
122+
calib.P = util['P_rect_{}'.format(camera_pad)]
123+
124+
elif kitti_type.find("odom") != -1:
125+
camera_pad = '{0:01d}'.format(camera)
126+
image_path = os.path.join(kitti.sequence_path, 'image_{}'.format(camera_pad))
127+
image_filenames = sorted(os.listdir(image_path))
128+
image_datetimes = map(lambda x: initial_time + x.total_seconds(), kitti.timestamps)
129+
130+
calib = CameraInfo()
131+
calib.header.frame_id = camera_frame_id
132+
calib.P = util['P{}'.format(camera_pad)]
133+
134+
iterable = zip(image_datetimes, image_filenames)
135+
bar = progressbar.ProgressBar()
136+
for dt, filename in bar(iterable):
137+
image_filename = os.path.join(image_path, filename)
138+
cv_image = cv2.imread(image_filename)
139+
calib.height, calib.width = cv_image.shape[:2]
140+
if camera in (0, 1):
141+
cv_image = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY)
142+
encoding = "mono8" if camera in (0, 1) else "bgr8"
143+
image_message = bridge.cv2_to_imgmsg(cv_image, encoding=encoding)
144+
image_message.header.frame_id = camera_frame_id
145+
if kitti_type.find("raw") != -1:
146+
image_message.header.stamp = rospy.Time.from_sec(float(datetime.strftime(dt, "%s.%f")))
147+
topic_ext = "/image_raw"
148+
elif kitti_type.find("odom") != -1:
149+
image_message.header.stamp = rospy.Time.from_sec(dt)
150+
topic_ext = "/image_rect"
151+
calib.header.stamp = image_message.header.stamp
152+
bag.write(topic + topic_ext, image_message, t = image_message.header.stamp)
153+
bag.write(topic + '/camera_info', calib, t = calib.header.stamp)
154+
155+
def save_velo_data(bag, kitti, velo_frame_id, topic):
156+
print("Exporting velodyne data")
157+
velo_path = os.path.join(kitti.data_path, 'velodyne_points')
158+
velo_data_dir = os.path.join(velo_path, 'data')
159+
velo_filenames = sorted(os.listdir(velo_data_dir))
160+
with open(os.path.join(velo_path, 'timestamps.txt')) as f:
161+
lines = f.readlines()
162+
velo_datetimes = []
163+
for line in lines:
164+
if len(line) == 1:
165+
continue
166+
dt = datetime.strptime(line[:-4], '%Y-%m-%d %H:%M:%S.%f')
167+
velo_datetimes.append(dt)
168+
169+
iterable = zip(velo_datetimes, velo_filenames)
170+
bar = progressbar.ProgressBar()
171+
for dt, filename in bar(iterable):
172+
if dt is None:
173+
continue
174+
175+
velo_filename = os.path.join(velo_data_dir, filename)
176+
177+
# read binary data
178+
scan = (np.fromfile(velo_filename, dtype=np.float32)).reshape(-1, 4)
179+
180+
# create header
181+
header = Header()
182+
header.frame_id = velo_frame_id
183+
header.stamp = rospy.Time.from_sec(float(datetime.strftime(dt, "%s.%f")))
184+
185+
# fill pcl msg
186+
fields = [PointField('x', 0, PointField.FLOAT32, 1),
187+
PointField('y', 4, PointField.FLOAT32, 1),
188+
PointField('z', 8, PointField.FLOAT32, 1),
189+
PointField('i', 12, PointField.FLOAT32, 1)]
190+
pcl_msg = pcl2.create_cloud(header, fields, scan)
191+
192+
bag.write(topic + '/pointcloud', pcl_msg, t=pcl_msg.header.stamp)
193+
194+
195+
def get_static_transform(from_frame_id, to_frame_id, transform):
196+
t = transform[0:3, 3]
197+
q = tf.transformations.quaternion_from_matrix(transform)
198+
tf_msg = TransformStamped()
199+
tf_msg.header.frame_id = from_frame_id
200+
tf_msg.child_frame_id = to_frame_id
201+
tf_msg.transform.translation.x = float(t[0])
202+
tf_msg.transform.translation.y = float(t[1])
203+
tf_msg.transform.translation.z = float(t[2])
204+
tf_msg.transform.rotation.x = float(q[0])
205+
tf_msg.transform.rotation.y = float(q[1])
206+
tf_msg.transform.rotation.z = float(q[2])
207+
tf_msg.transform.rotation.w = float(q[3])
208+
return tf_msg
209+
210+
211+
def inv(transform):
212+
"Invert rigid body transformation matrix"
213+
R = transform[0:3, 0:3]
214+
t = transform[0:3, 3]
215+
t_inv = -1 * R.T.dot(t)
216+
transform_inv = np.eye(4)
217+
transform_inv[0:3, 0:3] = R.T
218+
transform_inv[0:3, 3] = t_inv
219+
return transform_inv
220+
221+
222+
def save_static_transforms(bag, transforms, timestamps):
223+
print("Exporting static transformations")
224+
tfm = TFMessage()
225+
for transform in transforms:
226+
t = get_static_transform(from_frame_id=transform[0], to_frame_id=transform[1], transform=transform[2])
227+
tfm.transforms.append(t)
228+
for timestamp in timestamps:
229+
time = rospy.Time.from_sec(float(timestamp.strftime("%s.%f")))
230+
for i in range(len(tfm.transforms)):
231+
tfm.transforms[i].header.stamp = time
232+
bag.write('/tf_static', tfm, t=time)
233+
234+
235+
def save_gps_fix_data(bag, kitti, gps_frame_id, topic):
236+
for timestamp, oxts in zip(kitti.timestamps, kitti.oxts):
237+
navsatfix_msg = NavSatFix()
238+
navsatfix_msg.header.frame_id = gps_frame_id
239+
navsatfix_msg.header.stamp = rospy.Time.from_sec(float(timestamp.strftime("%s.%f")))
240+
navsatfix_msg.latitude = oxts.packet.lat
241+
navsatfix_msg.longitude = oxts.packet.lon
242+
navsatfix_msg.altitude = oxts.packet.alt
243+
navsatfix_msg.status.service = 1
244+
bag.write(topic, navsatfix_msg, t=navsatfix_msg.header.stamp)
245+
246+
247+
def save_gps_vel_data(bag, kitti, gps_frame_id, topic):
248+
for timestamp, oxts in zip(kitti.timestamps, kitti.oxts):
249+
twist_msg = TwistStamped()
250+
twist_msg.header.frame_id = gps_frame_id
251+
twist_msg.header.stamp = rospy.Time.from_sec(float(timestamp.strftime("%s.%f")))
252+
twist_msg.twist.linear.x = oxts.packet.vf
253+
twist_msg.twist.linear.y = oxts.packet.vl
254+
twist_msg.twist.linear.z = oxts.packet.vu
255+
twist_msg.twist.angular.x = oxts.packet.wf
256+
twist_msg.twist.angular.y = oxts.packet.wl
257+
twist_msg.twist.angular.z = oxts.packet.wu
258+
bag.write(topic, twist_msg, t=twist_msg.header.stamp)
259+
260+
261+
def run_kitti2bag(scenario, args_dir):
262+
263+
264+
bridge = CvBridge()
265+
compression = rosbag.Compression.NONE
266+
267+
# CAMERAS
268+
cameras = [
269+
(0, 'camera_gray_left', '/kitti/camera_gray_left'),
270+
(1, 'camera_gray_right', '/kitti/camera_gray_right'),
271+
(2, 'camera_color_left', '/kitti/camera_color_left'),
272+
(3, 'camera_color_right', '/kitti/camera_color_right')
273+
]
274+
275+
bag = rosbag.Bag("kitti_{}.bag".format(scenario), 'w', compression=compression)
276+
args_date = "2011_09_26"
277+
278+
kitti = pykitti.raw(args_dir, args_date, scenario)
279+
return
280+
281+
if not os.path.exists(kitti.data_path):
282+
print('Path {} does not exists. Exiting.'.format(kitti.data_path))
283+
sys.exit(1)
284+
285+
if len(kitti.timestamps) == 0:
286+
print('Dataset is empty? Exiting.')
287+
sys.exit(1)
288+
289+
try:
290+
# IMU
291+
imu_frame_id = 'imu_link'
292+
imu_topic = '/kitti/oxts/imu'
293+
gps_fix_topic = '/kitti/oxts/gps/fix'
294+
gps_vel_topic = '/kitti/oxts/gps/vel'
295+
velo_frame_id = 'velo_link'
296+
velo_topic = '/kitti/velo'
297+
298+
T_base_link_to_imu = np.eye(4, 4)
299+
T_base_link_to_imu[0:3, 3] = [-2.71/2.0-0.05, 0.32, 0.93]
300+
301+
# tf_static
302+
transforms = [
303+
('base_link', imu_frame_id, T_base_link_to_imu),
304+
(imu_frame_id, velo_frame_id, inv(kitti.calib.T_velo_imu)),
305+
(imu_frame_id, cameras[0][1], inv(kitti.calib.T_cam0_imu)),
306+
(imu_frame_id, cameras[1][1], inv(kitti.calib.T_cam1_imu)),
307+
(imu_frame_id, cameras[2][1], inv(kitti.calib.T_cam2_imu)),
308+
(imu_frame_id, cameras[3][1], inv(kitti.calib.T_cam3_imu))
309+
]
310+
311+
util = pykitti.utils.read_calib_file(os.path.join(kitti.calib_path, 'calib_cam_to_cam.txt'))
312+
313+
# Export
314+
save_static_transforms(bag, transforms, kitti.timestamps)
315+
save_dynamic_tf(bag, kitti, args.kitti_type, initial_time=None)
316+
save_imu_data(bag, kitti, imu_frame_id, imu_topic)
317+
save_gps_fix_data(bag, kitti, imu_frame_id, gps_fix_topic)
318+
save_gps_vel_data(bag, kitti, imu_frame_id, gps_vel_topic)
319+
for camera in cameras:
320+
save_camera_data(bag, args.kitti_type, kitti, util, bridge, camera=camera[0], camera_frame_id=camera[1], topic=camera[2], initial_time=None)
321+
save_velo_data(bag, kitti, velo_frame_id, velo_topic)
322+
323+
finally:
324+
print("## OVERVIEW ##")
325+
print(bag)
326+
bag.close()

‎scripts/run.sh

+3
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,3 @@
1+
roslaunch sensor_processing sensor_processing.launch home_dir:=/disk3/sensor_fusion/camera_imu_time_sync/datasets
2+
roslaunch detection detection.launch home_dir:=/disk3/sensor_fusion/camera_imu_time_sync/datasets
3+
roslaunch tracking tracking.launch home_dir:=/disk3/sensor_fusion/camera_imu_time_sync/datasets

‎scripts/sync_rosbag.py

+23
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,23 @@
1+
import rosbag
2+
import sys
3+
4+
file = sys.argv[1]
5+
6+
with rosbag.Bag('synchronized_data.bag', 'w') as outbag:
7+
for topic, msg, t in rosbag.Bag(sys.argv[1]).read_messages():
8+
if topic == "/kitti/velo/pointcloud":
9+
buffer = t
10+
outbag.write(topic,msg, msg.header.stamp)
11+
12+
elif topic == "/tf" and msg.transforms:
13+
for i in range(len(msg.transforms)):
14+
msg.transforms[i].header.stamp = buffer
15+
outbag.write(topic, msg, msg.transforms[i].header.stamp)
16+
17+
elif topic == "/tf_static" and msg.transforms:
18+
for i in range(len(msg.transforms)):
19+
msg.transforms[i].header.stamp = buffer
20+
outbag.write(topic, msg, msg.transforms[i].header.stamp)
21+
else:
22+
msg.header.stamp = buffer
23+
outbag.write(topic, msg, msg.header.stamp)

‎src/lidar_imu_fusion/CMakeLists.txt

+105
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,105 @@
1+
cmake_minimum_required(VERSION 3.0.2)
2+
project(lidar_imu_fusion)
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+
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
9+
## is used, also find other catkin packages
10+
find_package(catkin REQUIRED COMPONENTS
11+
cv_bridge
12+
geometry_msgs
13+
image_transport
14+
message_generation
15+
nav_msgs
16+
pcl_conversions
17+
roscpp
18+
roslib
19+
rospy
20+
sensor_msgs
21+
std_msgs
22+
tf
23+
)
24+
25+
## System dependencies are found with CMake's conventions
26+
# find_package(Boost REQUIRED COMPONENTS system)
27+
find_package(OpenMP REQUIRED)
28+
find_package(PCL REQUIRED)
29+
find_package(OpenCV REQUIRED)
30+
find_package(Ceres REQUIRED)
31+
find_package(GTSAM REQUIRED QUIET)
32+
find_package(Boost REQUIRED COMPONENTS filesystem program_options system)
33+
34+
################################################
35+
## Declare ROS messages, services and actions ##
36+
################################################
37+
## Generate messages in the 'msg' folder
38+
add_message_files(
39+
DIRECTORY msg
40+
FILES
41+
cloud_info.msg
42+
cloud_img.msg
43+
)
44+
45+
## Generate services in the 'srv' folder
46+
add_service_files(
47+
DIRECTORY srv
48+
FILES
49+
reset_lidar_odometry.srv
50+
)
51+
52+
generate_messages(
53+
DEPENDENCIES
54+
geometry_msgs std_msgs nav_msgs sensor_msgs
55+
)
56+
57+
58+
###################################
59+
## catkin specific configuration ##
60+
###################################
61+
## The catkin_package macro generates cmake config files for your package
62+
## Declare things to be passed to dependent projects
63+
## INCLUDE_DIRS: uncomment this if your package contains header files
64+
## LIBRARIES: libraries you create in this project that dependent projects also need
65+
## CATKIN_DEPENDS: catkin_packages dependent projects also need
66+
## DEPENDS: system dependencies of this project that dependent projects also need
67+
catkin_package(
68+
#INCLUDE_DIRS include
69+
#LIBRARIES lidar_imu_fusion
70+
CATKIN_DEPENDS cv_bridge geometry_msgs image_transport message_runtime message_generation nav_msgs pcl_conversions roscpp roslib rospy sensor_msgs std_msgs tf
71+
#DEPENDS system_lib
72+
)
73+
74+
###########
75+
## Build ##
76+
###########
77+
78+
## Specify additional locations of header files
79+
## Your package locations should be listed before other locations
80+
include_directories(
81+
include
82+
${catkin_INCLUDE_DIRS}
83+
)
84+
85+
86+
# IMU Preintegration
87+
add_executable(${PROJECT_NAME}_imuPreintegration src/imuPreintegration.cpp)
88+
target_link_libraries(${PROJECT_NAME}_imuPreintegration ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES} gtsam)
89+
# Range Image Projection
90+
add_executable(${PROJECT_NAME}_imageProjection src/imageProjection.cpp)
91+
add_dependencies(${PROJECT_NAME}_imageProjection ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_generate_messages_cpp)
92+
target_link_libraries(${PROJECT_NAME}_imageProjection ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES})
93+
# Feature Association
94+
add_executable(${PROJECT_NAME}_featureExtraction src/featureExtraction.cpp)
95+
add_dependencies(${PROJECT_NAME}_featureExtraction ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_generate_messages_cpp)
96+
target_link_libraries(${PROJECT_NAME}_featureExtraction ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES})
97+
# Mapping Optimization
98+
add_executable(${PROJECT_NAME}_mapOptmization src/mapOptmization.cpp)
99+
add_dependencies(${PROJECT_NAME}_mapOptmization ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_generate_messages_cpp)
100+
target_compile_options(${PROJECT_NAME}_mapOptmization PRIVATE ${OpenMP_CXX_FLAGS})
101+
target_link_libraries(${PROJECT_NAME}_mapOptmization PRIVATE ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES} ${OpenMP_CXX_FLAGS} gtsam)
102+
103+
add_executable(${PROJECT_NAME}_gpsConverter src/gpsConverter.cpp)
104+
target_link_libraries(${PROJECT_NAME}_gpsConverter ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES} gtsam)
105+

‎src/lidar_imu_fusion/config/brief_k10L6.bin

57.2 MB
Binary file not shown.

‎src/lidar_imu_fusion/config/brief_pattern.yml

+1,029
Large diffs are not rendered by default.

‎src/lidar_imu_fusion/config/params_camera_hs5.yaml

+123
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,123 @@
1+
%YAML:1.0
2+
3+
# Project
4+
project_name: "lvi_sam"
5+
6+
#common parameters
7+
8+
point_cloud_topic: "lvi_sam/lidar/deskew/cloud_deskewed"
9+
imu_topic: "/imu_correct"
10+
image_topic: "/raw_image"
11+
odomResetTopic: "/reset_lidar_osometry"
12+
imu_rate: 100
13+
save_cloud: true
14+
num_bins: 3600
15+
16+
# Lidar Params
17+
use_lidar: 1 # whether use depth info from lidar or not
18+
lidar_skip: 0 # skip this amount of scans
19+
align_camera_lidar_estimation: 0 # align camera and lidar estimation for visualization
20+
21+
#HS5
22+
model_type: PINHOLE
23+
camera_name: camera
24+
image_width: 1920
25+
image_height: 1080
26+
distortion_parameters:
27+
k1: -5.9837810961927029e-01
28+
k2: 3.8342172770055183e-01
29+
p1: 9.6107542719565779e-03
30+
p2: 1.2766832075472282e-02
31+
projection_parameters:
32+
fx: 1998.7356
33+
fy: 1991.8909
34+
cx: 851.8287
35+
cy: 424.0041
36+
37+
camera_matrix: !!opencv-matrix
38+
rows: 3
39+
cols: 3
40+
dt: d
41+
data: [1998.7356,0,851.8287,
42+
0,1991.8909,424.0041,
43+
0,0,1]
44+
45+
distortion: !!opencv-matrix
46+
rows: 5
47+
cols: 1
48+
dt: d
49+
data: [-5.9837810961927029e-01,3.8342172770055183e-01,9.6107542719565779e-03,1.2766832075472282e-02,-1.8572618846e-01]
50+
51+
52+
#imu parameters The more accurate parameters you provide, the worse performance
53+
acc_n: 0.02 # accelerometer measurement noise standard deviation.
54+
gyr_n: 0.01 # gyroscope measurement noise standard deviation.
55+
acc_w: 0.002 # accelerometer bias random work noise standard deviation.
56+
gyr_w: 4.0e-5 # gyroscope bias random work noise standard deviation.
57+
g_norm: 9.80511 #
58+
59+
#camera to base
60+
estimate_extrinsic: 0 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it.
61+
# 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess.
62+
# 2 Don't know anything about extrinsic parameters. You don't need to give R,T. We will try to calibrate it. Do some rotation movement at beginning.
63+
#If you choose 0 or 1, you should write down the following matrix.
64+
65+
extrinsicRotation: !!opencv-matrix
66+
rows: 3
67+
cols: 3
68+
dt: d
69+
data: [0.99780000,-0.01749999,-0.06450001,
70+
0.06540000,0.05530000,0.99630000,
71+
-0.01390000,-0.99830000,0.05630000]
72+
extrinsicTranslation: !!opencv-matrix
73+
rows: 3
74+
cols: 1
75+
dt: d
76+
data: [0.10297998,1.56720400,1.50535599]
77+
78+
79+
# extrinsicRotation: !!opencv-matrix
80+
# rows: 3
81+
# cols: 3
82+
# dt: d
83+
# data: [0.9978,-0.0175,-0.0645,
84+
# 0.0654,0.0553,0.9963,
85+
# -0.0139,-0.9983,0.0563]
86+
# extrinsicTranslation: !!opencv-matrix
87+
# rows: 3
88+
# cols: 1
89+
# dt: d
90+
# data: [0.5,1.6,1.38]
91+
# data: [-0.5442,1.6945,0.9509]
92+
93+
#feature traker paprameters
94+
max_cnt: 150 # max feature number in feature tracking
95+
min_dist: 20 # min distance between two features
96+
freq: 20 # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image
97+
F_threshold: 1.0 # ransac threshold (pixel)
98+
show_track: 1 # publish tracking image as topic
99+
equalize: 1 # if image is too dark or light, trun on equalize to find enough features
100+
fisheye: 0
101+
102+
103+
#optimization parameters
104+
max_solver_time: 0.035 # max solver itration time (ms), to guarantee real time
105+
max_num_iterations: 10 # max solver itrations, to guarantee real time
106+
keyframe_parallax: 10.0 # keyframe selection threshold (pixel)
107+
108+
#unsynchronization parameters
109+
estimate_td: 0 # online estimate time offset between camera and imu
110+
td: 0 # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock)
111+
112+
#rolling shutter parameters
113+
rolling_shutter: 0 # 0: global shutter camera, 1: rolling shutter camera
114+
rolling_shutter_tr: 0 # unit: s. rolling shutter read out time per frame (from data sheet).
115+
116+
#loop closure parameters
117+
loop_closure: 1 # start loop closure
118+
skip_time: 0.0
119+
skip_dist: 0.0
120+
debug_image: 0 # save raw image in loop detector for visualization prupose; you can close this function by setting 0
121+
match_image_scale: 0.5
122+
vocabulary_file: "/config/brief_k10L6.bin"
123+
brief_pattern_file: "/config/brief_pattern.yml"

‎src/lidar_imu_fusion/config/params_lidar_hs5.yaml

+103
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,103 @@
1+
# project name
2+
PROJECT_NAME: "lvi_sam"
3+
4+
lvi_sam:
5+
6+
pointCloudTopic: "/velodyne_points"
7+
imuTopic: "/cgi610/imu"
8+
gpsTopic: "/gps/odom"
9+
odomTopic: "odometry/imu"
10+
gpsFixTopic: "/cgi610/nav_fix"
11+
gpsRawTopic: "/localization/odom"
12+
13+
# Frames
14+
lidarFrame: "base_link"
15+
baselinkFrame: "base_link"
16+
odometryFrame: "odom"
17+
mapFrame: "map"
18+
19+
saveTrajectory: false
20+
21+
# #HS5 GPS Settings
22+
useGpsData: true
23+
useImuHeadingInitialization: true # if using GPS data, set to "true"
24+
useGpsElevation: true # if GPS elevation is bad, set to "false"
25+
gpsCovThreshold: 2.0 # m^2, threshold for using GPS data
26+
poseCovThreshold: 25.0 # m^2, threshold for using GPS data
27+
useGpsPoseinterpolation: false # true:pose,false:twist
28+
29+
# #HS5 lidar
30+
sensor: pandar40p # lidar sensor type, either 'velodyne' or 'ouster'
31+
N_SCAN: 40 # number of lidar channel (i.e., 16, 32, 64, 128,kitti 64)
32+
Horizon_SCAN: 1800 # lidar horizontal resolution (Velodyne:1800, Ouster:512,1024,2048)
33+
downsampleRate: 1 # default: 1. Downsample your data if too many points. i.e., 16 = 64 / 4, 16 = 16 / 1 , kitti2,4
34+
lidarMinRange: 1.0 # default: 1.0, minimum lidar range to be used
35+
lidarMaxRange: 1000.0 # default: 1000.0, maximum lidar range to be used
36+
37+
38+
imuRotEqualBase: true
39+
# #HS5 extrinsic settings
40+
extrinsicTrans: [0, 0, 0] #Imu to base_link
41+
extrinsicRPY: [0.0157072, 0.9997767, 0.0141383, -0.9998717, 0.0156614, 0.0033463, 0.0031242, -0.0141890, 0.9998944] #Base_link to lidar
42+
extrinsicRot: [0.0366,0.9993,0.0124,-0.9990,0.0369,-0.0234,-0.0239,-0.0115,0.9996] #Imu to lidar
43+
extrinsicTransLidar2Base : [-0.05,0.95,1.74] #Lidar to base_link
44+
extrinsicRotImu2Base : [0.9998527, -0.0171621, 0.0000962, 0.0171615, 0.9998428, 0.0044674, -0.0001729, -0.0044651, 0.9999900] #Imu to base_link
45+
46+
# #JA602
47+
# extrinsicTrans: [0, 0, 0] #Imu to base_link
48+
# extrinsicRPY: [0.0289305, 0.9995105, 0.0119117, -0.9995719, 0.0288762, 0.0047047, 0.0043584, -0.0120428, 0.9999180] #Base_link to lidar
49+
# extrinsicRot: [0.04607728,0.99880368,0.01637958, -0.99892992,0.04600541,0.00473750,0.00397820,-0.01658044,0.99985462] #Imu to lidar
50+
# extrinsicTransLidar2Base : [0.045894679025020091,3.9195938937608172,0.84463641163726733] #Lidar to base_link
51+
# extrinsicRotImu2Base : [0.9998527, -0.0171621, 0.0000962, 0.0171615, 0.9998428, 0.0044674, -0.0001729, -0.0044651, 0.9999900] #Imu to base_link
52+
53+
# Export settings
54+
savePCD: false
55+
savePCDDirectory: "/Downloads/LOAM/" # in your home folder, starts and ends with "/". Warning: the code deletes "LOAM" folder then recreates it. See "mapOptimization" for implementation
56+
57+
# IMU Settings
58+
imuAccNoise: 3.9939570888238808e-03
59+
imuGyrNoise: 1.5636343949698187e-03
60+
imuAccBiasN: 6.4356659353532566e-05
61+
imuGyrBiasN: 3.5640318696367613e-05
62+
imuGravity: 9.80511
63+
imuRate: 100
64+
65+
errorThreshold: 1.0
66+
67+
# LOAM feature threshold
68+
edgeThreshold: 1.0
69+
surfThreshold: 0.1
70+
edgeFeatureMinValidNum: 10
71+
surfFeatureMinValidNum: 100
72+
73+
# voxel filter paprams
74+
odometrySurfLeafSize: 0.5
75+
mappingCornerLeafSize: 0.5
76+
mappingSurfLeafSize: 0.5
77+
78+
# robot motion constraint (in case you are using a 2D robot)
79+
z_tollerance: 1000 # meters
80+
rotation_tollerance: 1000 # radians
81+
82+
# CPU Params
83+
numberOfCores: 4 # number of cores for mapping optimization
84+
mappingProcessInterval: 0.15 # seconds, regulate mapping frequency
85+
86+
# Surrounding map
87+
surroundingkeyframeAddingDistThreshold: 1.0 # meters, regulate keyframe adding threshold
88+
surroundingkeyframeAddingAngleThreshold: 0.2 # radians, regulate keyframe adding threshold
89+
surroundingKeyframeDensity: 2.0 # meters, downsample surrounding keyframe poses
90+
surroundingKeyframeSearchRadius: 50.0 # meters, within n meters scan-to-map optimization (when loop closure disabled)
91+
92+
# Loop closure
93+
loopClosureEnableFlag: false
94+
surroundingKeyframeSize: 25 # submap size (when loop closure enabled)
95+
historyKeyframeSearchRadius: 20.0 # meters, key frame that is within n meters from current pose will be considerd for loop closure
96+
historyKeyframeSearchTimeDiff: 30.0 # seconds, key frame that is n seconds older will be considered for loop closure
97+
historyKeyframeSearchNum: 25 # number of hostory key frames will be fused into a submap for loop closure
98+
historyKeyframeFitnessScore: 0.3 # icp threshold, the smaller the better alignment
99+
100+
# Visualization
101+
globalMapVisualizationSearchRadius: 1000.0 # meters, global map visualization radius
102+
globalMapVisualizationPoseDensity: 10.0 # meters, global map visualization keyframe density
103+
globalMapVisualizationLeafSize: 1.0 # meters, global map visualization cloud density

‎src/lidar_imu_fusion/config/robot.urdf.xacro

+57
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,57 @@
1+
<?xml version="1.0"?>
2+
<robot name="roboat" xmlns:xacro="http://roboat.org">
3+
<xacro:property name="PI" value="3.1415926535897931" />
4+
5+
<link name="base_link"></link>
6+
7+
<joint name="base_link_joint" type="fixed">
8+
<parent link="base_link"/>
9+
<child link="chassis_link" />
10+
<origin xyz="0 0 0" rpy="0 0 0" />
11+
</joint>
12+
13+
<link name="chassis_link"></link>
14+
15+
<link name="imu_link"> </link>
16+
<joint name="imu_joint" type="fixed">
17+
<parent link="chassis_link" />
18+
<child link="imu_link" />
19+
<origin xyz="0 -0.1 0" rpy="${PI} 0 ${PI}" />
20+
</joint>
21+
22+
<link name="imu_enu_link"> </link>
23+
<joint name="imu_enu_joint" type="fixed">
24+
<parent link="imu_link" />
25+
<child link="imu_enu_link" />
26+
<origin xyz="0 0 0" rpy="${PI} 0 ${PI}" />
27+
</joint>
28+
29+
<link name="velodyne"> </link>
30+
<joint name="velodyne_joint" type="fixed">
31+
<parent link="chassis_link" />
32+
<child link="velodyne" />
33+
<origin xyz="0 0 0" rpy="0 0 0" />
34+
</joint>
35+
36+
<link name="realsense_link"> </link>
37+
<joint name="realsense_joint" type="fixed">
38+
<parent link="chassis_link" />
39+
<child link="realsense_link" />
40+
<origin xyz="0.1 0 -0.1" rpy="0 0 0" />
41+
</joint>
42+
43+
<link name="navsat_link"> </link>
44+
<joint name="navsat_joint" type="fixed">
45+
<parent link="chassis_link" />
46+
<child link="navsat_link" />
47+
<origin xyz="-0.2 0 0.2" rpy="0 0 0" />
48+
</joint>
49+
50+
<link name="loam_camera"> </link>
51+
<joint name="_loam_camera_joint" type="fixed">
52+
<parent link="chassis_link" />
53+
<child link="loam_camera" />
54+
<origin xyz="0 0 0" rpy="1.570796 0 1.570796" />
55+
</joint>
56+
57+
</robot>

‎src/lidar_imu_fusion/config/rviz.rviz

+544
Large diffs are not rendered by default.

‎src/lidar_imu_fusion/include/utility.h

+382
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,382 @@
1+
#pragma once
2+
#ifndef _UTILITY_LIDAR_ODOMETRY_H_
3+
#define _UTILITY_LIDAR_ODOMETRY_H_
4+
5+
#include <ros/ros.h>
6+
7+
#include <std_msgs/Header.h>
8+
#include <std_msgs/Bool.h>
9+
#include <std_msgs/Float64MultiArray.h>
10+
#include <std_msgs/Float64.h>
11+
#include <sensor_msgs/Imu.h>
12+
#include <sensor_msgs/PointCloud2.h>
13+
#include <sensor_msgs/NavSatFix.h>
14+
#include <nav_msgs/Odometry.h>
15+
#include <nav_msgs/Path.h>
16+
#include <visualization_msgs/Marker.h>
17+
#include <visualization_msgs/MarkerArray.h>
18+
19+
#include <opencv/cv.h>
20+
21+
#include <pcl/point_cloud.h>
22+
#include <pcl/point_types.h>
23+
#include <pcl/search/impl/search.hpp>
24+
#include <pcl/range_image/range_image.h>
25+
#include <pcl/kdtree/kdtree_flann.h>
26+
#include <pcl/common/common.h>
27+
#include <pcl/common/transforms.h>
28+
#include <pcl/registration/icp.h>
29+
#include <pcl/io/pcd_io.h>
30+
#include <pcl/filters/filter.h>
31+
#include <pcl/filters/voxel_grid.h>
32+
#include <pcl/filters/crop_box.h>
33+
#include <pcl_conversions/pcl_conversions.h>
34+
35+
#include <tf/LinearMath/Quaternion.h>
36+
#include <tf/transform_listener.h>
37+
#include <tf/transform_datatypes.h>
38+
#include <tf/transform_broadcaster.h>
39+
40+
#include <vector>
41+
#include <cmath>
42+
#include <algorithm>
43+
#include <queue>
44+
#include <deque>
45+
#include <iostream>
46+
#include <fstream>
47+
#include <ctime>
48+
#include <cfloat>
49+
#include <iterator>
50+
#include <sstream>
51+
#include <string>
52+
#include <limits>
53+
#include <iomanip>
54+
#include <array>
55+
#include <thread>
56+
#include <mutex>
57+
using namespace std;
58+
59+
typedef pcl::PointXYZI PointType;
60+
61+
enum class SensorType { VELODYNE, OUSTER, Pandar40P };
62+
63+
class ParamServer
64+
{
65+
public:
66+
67+
ros::NodeHandle nh;
68+
std::string PROJECT_NAME;
69+
std::string robot_id;
70+
71+
//Topics
72+
string pointCloudTopic;
73+
string imuTopic;
74+
string odomTopic;
75+
string gpsTopic;
76+
string gpsRawTopic;
77+
string odomResetTopic;
78+
//Frames
79+
string lidarFrame;
80+
string baselinkFrame;
81+
string odometryFrame;
82+
string mapFrame;
83+
84+
// GPS Settings
85+
std::string gpsFixTopic;
86+
bool useGpsData;
87+
bool useGpsPoseinterpolation;
88+
bool useImuHeadingInitialization;
89+
bool useGpsElevation;
90+
float gpsCovThreshold;
91+
float poseCovThreshold;
92+
bool saveTrajectory;
93+
// Save pcd
94+
bool savePCD;
95+
string savePCDDirectory;
96+
97+
// Lidar Sensor Configuration
98+
SensorType sensor;
99+
int N_SCAN;
100+
int Horizon_SCAN;
101+
int downsampleRate;
102+
float lidarMinRange;
103+
float lidarMaxRange;
104+
105+
// IMU
106+
bool imuRotEqualBase;
107+
float imuAccNoise;
108+
float imuGyrNoise;
109+
float imuAccBiasN;
110+
float imuGyrBiasN;
111+
float imuGravity;
112+
float imuRPYWeight;
113+
float imuRate;
114+
vector<double> extRotV;
115+
vector<double> extRPYV;
116+
vector<double> extTransV;
117+
vector<double> extTransVL;
118+
vector<double> extROTVI;
119+
120+
Eigen::Matrix3d extRot;
121+
Eigen::Matrix3d extRotImu2Base;
122+
Eigen::Matrix3d extRPY;
123+
Eigen::Matrix3d extRPYInv;
124+
Eigen::Vector3d extTransLidar2Base;
125+
Eigen::Vector3d extTrans;
126+
Eigen::Quaterniond extQRPY;
127+
Eigen::Quaterniond extQImu2Base;
128+
129+
float errorThreshold;
130+
131+
// LOAM
132+
float edgeThreshold;
133+
float surfThreshold;
134+
int edgeFeatureMinValidNum;
135+
int surfFeatureMinValidNum;
136+
137+
// voxel filter paprams
138+
float odometrySurfLeafSize;
139+
float mappingCornerLeafSize;
140+
float mappingSurfLeafSize ;
141+
float mappingLeafSize;
142+
143+
float z_tollerance;
144+
float rotation_tollerance;
145+
146+
// CPU Params
147+
int numberOfCores;
148+
double mappingProcessInterval;
149+
150+
// Surrounding map
151+
float surroundingkeyframeAddingDistThreshold;
152+
float surroundingkeyframeAddingAngleThreshold;
153+
float surroundingKeyframeDensity;
154+
float surroundingKeyframeSearchRadius;
155+
156+
// Loop closure
157+
bool loopClosureEnableFlag;
158+
float loopClosureFrequency;
159+
int surroundingKeyframeSize;
160+
float historyKeyframeSearchRadius;
161+
float historyKeyframeSearchTimeDiff;
162+
int historyKeyframeSearchNum;
163+
float historyKeyframeFitnessScore;
164+
165+
// global map visualization radius
166+
float globalMapVisualizationSearchRadius;
167+
float globalMapVisualizationPoseDensity;
168+
float globalMapVisualizationLeafSize;
169+
170+
ParamServer()
171+
{
172+
nh.param<std::string>("/PROJECT_NAME", PROJECT_NAME, "sam");
173+
nh.param<std::string>("/robot_id", robot_id, "roboat");
174+
175+
nh.param<std::string>(PROJECT_NAME + "/pointCloudTopic", pointCloudTopic, "points_raw");
176+
nh.param<std::string>(PROJECT_NAME + "/imuTopic", imuTopic, "imu_raw");
177+
nh.param<std::string>(PROJECT_NAME + "/odomTopic", odomTopic, "odometry/imu");
178+
nh.param<std::string>(PROJECT_NAME + "/gpsTopic", gpsTopic, "odometry/gps");
179+
nh.param<std::string>(PROJECT_NAME + "/gpsRawTopic", gpsRawTopic, "/localization/odom");
180+
nh.param<std::string>(PROJECT_NAME + "/odomResetTopic", odomResetTopic, "/reset_lidar_osometry");
181+
nh.param<std::string>(PROJECT_NAME + "/lidarFrame", lidarFrame, "base_link");
182+
nh.param<std::string>(PROJECT_NAME + "/baselinkFrame", baselinkFrame, "base_link");
183+
nh.param<std::string>(PROJECT_NAME + "/odometryFrame", odometryFrame, "odom");
184+
nh.param<std::string>(PROJECT_NAME + "/mapFrame", mapFrame, "map");
185+
186+
nh.param<std::string>(PROJECT_NAME + "/gpsFixTopic", gpsFixTopic, "/cgi610/nav_fix");
187+
188+
nh.param<bool>(PROJECT_NAME + "/saveTrajectory", saveTrajectory, false);
189+
nh.param<bool>(PROJECT_NAME + "/useGpsData", useGpsData, false);
190+
nh.param<bool>(PROJECT_NAME + "/useGpsPoseinterpolation", useGpsPoseinterpolation, false);
191+
nh.param<bool>(PROJECT_NAME + "/useImuHeadingInitialization", useImuHeadingInitialization, false);
192+
nh.param<bool>(PROJECT_NAME + "/useGpsElevation", useGpsElevation, false);
193+
nh.param<float>(PROJECT_NAME + "/gpsCovThreshold", gpsCovThreshold, 2.0);
194+
nh.param<float>(PROJECT_NAME + "/poseCovThreshold", poseCovThreshold, 25.0);
195+
196+
nh.param<bool>(PROJECT_NAME + "/savePCD", savePCD, false);
197+
nh.param<std::string>(PROJECT_NAME + "/savePCDDirectory", savePCDDirectory, "/Downloads/LOAM/");
198+
199+
std::string sensorStr;
200+
nh.param<std::string>(PROJECT_NAME + "/sensor", sensorStr, "");
201+
if (sensorStr == "velodyne")
202+
{
203+
sensor = SensorType::VELODYNE;
204+
}
205+
else if (sensorStr == "ouster")
206+
{
207+
sensor = SensorType::OUSTER;
208+
}
209+
else if(sensorStr == "pandar40p")
210+
{
211+
sensor = SensorType::Pandar40P;
212+
}
213+
else
214+
{
215+
ROS_ERROR_STREAM(
216+
"Invalid sensor type (must be either 'velodyne' or 'ouster'): " << sensorStr);
217+
ros::shutdown();
218+
}
219+
220+
nh.param<int>(PROJECT_NAME + "/N_SCAN", N_SCAN, 16);
221+
nh.param<int>(PROJECT_NAME + "/Horizon_SCAN", Horizon_SCAN, 1800);
222+
nh.param<int>(PROJECT_NAME + "/downsampleRate", downsampleRate, 1);
223+
nh.param<float>(PROJECT_NAME + "/lidarMinRange", lidarMinRange, 1.0);
224+
nh.param<float>(PROJECT_NAME + "/lidarMaxRange", lidarMaxRange, 1000.0);
225+
226+
nh.param<bool>(PROJECT_NAME + "/imuRotEqualBase", imuRotEqualBase, false);
227+
nh.param<float>(PROJECT_NAME + "/imuAccNoise", imuAccNoise, 0.01);
228+
nh.param<float>(PROJECT_NAME + "/imuGyrNoise", imuGyrNoise, 0.001);
229+
nh.param<float>(PROJECT_NAME + "/imuAccBiasN", imuAccBiasN, 0.0002);
230+
nh.param<float>(PROJECT_NAME + "/imuGyrBiasN", imuGyrBiasN, 0.00003);
231+
nh.param<float>(PROJECT_NAME + "/imuGravity", imuGravity, 9.80511);
232+
nh.param<float>(PROJECT_NAME + "/imuRPYWeight", imuRPYWeight, 0.01);
233+
nh.param<float>(PROJECT_NAME + "/imuRate", imuRate, 100);
234+
235+
nh.param<vector<double>>(PROJECT_NAME + "/extrinsicRot", extRotV, vector<double>());
236+
nh.param<vector<double>>(PROJECT_NAME + "/extrinsicRPY", extRPYV, vector<double>());
237+
nh.param<vector<double>>(PROJECT_NAME + "/extrinsicTrans", extTransV, vector<double>());
238+
nh.param<vector<double>>(PROJECT_NAME + "/extrinsicTransLidar2Base", extTransVL, vector<double>());
239+
nh.param<vector<double>>(PROJECT_NAME + "/extrinsicRotImu2Base", extROTVI, vector<double>());
240+
extTransLidar2Base = Eigen::Map<const Eigen::Matrix<double, -1, -1, Eigen::RowMajor>>(extTransVL.data(), 3, 1);
241+
extRotImu2Base = Eigen::Map<const Eigen::Matrix<double, -1, -1, Eigen::RowMajor>>(extROTVI.data(), 3, 3);
242+
extRot = Eigen::Map<const Eigen::Matrix<double, -1, -1, Eigen::RowMajor>>(extRotV.data(), 3, 3);
243+
extRPY = Eigen::Map<const Eigen::Matrix<double, -1, -1, Eigen::RowMajor>>(extRPYV.data(), 3, 3);
244+
extTrans = Eigen::Map<const Eigen::Matrix<double, -1, -1, Eigen::RowMajor>>(extTransV.data(), 3, 1);
245+
extQRPY = Eigen::Quaterniond(extRPY);
246+
extRPYInv = extRPY.inverse();
247+
extQImu2Base = Eigen::Quaterniond(extRotImu2Base);
248+
249+
nh.param<float>(PROJECT_NAME + "/errorThreshold", errorThreshold, 2.0);
250+
nh.param<float>(PROJECT_NAME + "/edgeThreshold", edgeThreshold, 0.1);
251+
nh.param<float>(PROJECT_NAME + "/surfThreshold", surfThreshold, 0.1);
252+
nh.param<int>(PROJECT_NAME + "/edgeFeatureMinValidNum", edgeFeatureMinValidNum, 10);
253+
nh.param<int>(PROJECT_NAME + "/surfFeatureMinValidNum", surfFeatureMinValidNum, 100);
254+
255+
nh.param<float>(PROJECT_NAME + "/odometrySurfLeafSize", odometrySurfLeafSize, 0.5);
256+
nh.param<float>(PROJECT_NAME + "/mappingCornerLeafSize", mappingCornerLeafSize, 0.5);
257+
nh.param<float>(PROJECT_NAME + "/mappingSurfLeafSize", mappingSurfLeafSize, 0.5);
258+
nh.param<float>(PROJECT_NAME + "/mappingLeafSize", mappingLeafSize, 0.5);
259+
260+
nh.param<float>(PROJECT_NAME + "/z_tollerance", z_tollerance, FLT_MAX);
261+
nh.param<float>(PROJECT_NAME + "/rotation_tollerance", rotation_tollerance, FLT_MAX);
262+
263+
nh.param<int>(PROJECT_NAME + "/numberOfCores", numberOfCores, 2);
264+
nh.param<double>(PROJECT_NAME + "/mappingProcessInterval", mappingProcessInterval, 0.15);
265+
266+
nh.param<float>(PROJECT_NAME + "/surroundingkeyframeAddingDistThreshold", surroundingkeyframeAddingDistThreshold, 1.0);
267+
nh.param<float>(PROJECT_NAME + "/surroundingkeyframeAddingAngleThreshold", surroundingkeyframeAddingAngleThreshold, 0.2);
268+
nh.param<float>(PROJECT_NAME + "/surroundingKeyframeDensity", surroundingKeyframeDensity, 1.0);
269+
nh.param<float>(PROJECT_NAME + "/surroundingKeyframeSearchRadius", surroundingKeyframeSearchRadius, 50.0);
270+
271+
nh.param<bool>(PROJECT_NAME + "/loopClosureEnableFlag", loopClosureEnableFlag, false);
272+
nh.param<float>(PROJECT_NAME + "/loopClosureFrequency", loopClosureFrequency, 1.0);
273+
nh.param<int>(PROJECT_NAME + "/surroundingKeyframeSize", surroundingKeyframeSize, 50);
274+
nh.param<float>(PROJECT_NAME + "/historyKeyframeSearchRadius", historyKeyframeSearchRadius, 10.0);
275+
nh.param<float>(PROJECT_NAME + "/historyKeyframeSearchTimeDiff", historyKeyframeSearchTimeDiff, 30.0);
276+
nh.param<int>(PROJECT_NAME + "/historyKeyframeSearchNum", historyKeyframeSearchNum, 25);
277+
nh.param<float>(PROJECT_NAME + "/historyKeyframeFitnessScore", historyKeyframeFitnessScore, 0.3);
278+
279+
nh.param<float>(PROJECT_NAME + "/globalMapVisualizationSearchRadius", globalMapVisualizationSearchRadius, 1e3);
280+
nh.param<float>(PROJECT_NAME + "/globalMapVisualizationPoseDensity", globalMapVisualizationPoseDensity, 10.0);
281+
nh.param<float>(PROJECT_NAME + "/globalMapVisualizationLeafSize", globalMapVisualizationLeafSize, 1.0);
282+
283+
usleep(100);
284+
}
285+
286+
sensor_msgs::Imu imuConverter(const sensor_msgs::Imu& imu_in)
287+
{
288+
sensor_msgs::Imu imu_out = imu_in;
289+
290+
// rotate acceleration
291+
Eigen::Vector3d acc(imu_in.linear_acceleration.x, imu_in.linear_acceleration.y, imu_in.linear_acceleration.z);
292+
acc = extRotImu2Base * acc;
293+
imu_out.linear_acceleration.x = acc.x();
294+
imu_out.linear_acceleration.y = acc.y();
295+
imu_out.linear_acceleration.z = acc.z();
296+
Eigen::Vector3d gyr(imu_in.angular_velocity.x, imu_in.angular_velocity.y, imu_in.angular_velocity.z);
297+
gyr = extRotImu2Base * gyr;
298+
imu_out.angular_velocity.x = gyr.x();
299+
imu_out.angular_velocity.y = gyr.y();
300+
imu_out.angular_velocity.z = gyr.z();
301+
if(!imuRotEqualBase){
302+
// rotate roll pitch yaw
303+
Eigen::Quaterniond q_from(imu_in.orientation.w, imu_in.orientation.x, imu_in.orientation.y, imu_in.orientation.z);
304+
Eigen::Quaterniond q_final = q_from * extQImu2Base;
305+
imu_out.orientation.x = q_final.x();
306+
imu_out.orientation.y = q_final.y();
307+
imu_out.orientation.z = q_final.z();
308+
imu_out.orientation.w = q_final.w();
309+
310+
if (sqrt(q_final.x()*q_final.x() + q_final.y()*q_final.y() + q_final.z()*q_final.z() + q_final.w()*q_final.w()) < 0.1)
311+
{
312+
ROS_ERROR("Invalid quaternion, please use a 9-axis IMU!");
313+
ros::shutdown();
314+
}
315+
}
316+
return imu_out;
317+
}
318+
};
319+
320+
321+
sensor_msgs::PointCloud2 publishCloud(ros::Publisher *thisPub, pcl::PointCloud<PointType>::Ptr thisCloud, ros::Time thisStamp, std::string thisFrame)
322+
{
323+
sensor_msgs::PointCloud2 tempCloud;
324+
pcl::toROSMsg(*thisCloud, tempCloud);
325+
tempCloud.header.stamp = thisStamp;
326+
tempCloud.header.frame_id = thisFrame;
327+
if (thisPub->getNumSubscribers() != 0)
328+
thisPub->publish(tempCloud);
329+
return tempCloud;
330+
}
331+
332+
template<typename T>
333+
double ROS_TIME(T msg)
334+
{
335+
return msg->header.stamp.toSec();
336+
}
337+
338+
339+
template<typename T>
340+
void imuAngular2rosAngular(sensor_msgs::Imu *thisImuMsg, T *angular_x, T *angular_y, T *angular_z)
341+
{
342+
*angular_x = thisImuMsg->angular_velocity.x;
343+
*angular_y = thisImuMsg->angular_velocity.y;
344+
*angular_z = thisImuMsg->angular_velocity.z;
345+
}
346+
347+
348+
template<typename T>
349+
void imuAccel2rosAccel(sensor_msgs::Imu *thisImuMsg, T *acc_x, T *acc_y, T *acc_z)
350+
{
351+
*acc_x = thisImuMsg->linear_acceleration.x;
352+
*acc_y = thisImuMsg->linear_acceleration.y;
353+
*acc_z = thisImuMsg->linear_acceleration.z;
354+
}
355+
356+
357+
template<typename T>
358+
void imuRPY2rosRPY(sensor_msgs::Imu *thisImuMsg, T *rosRoll, T *rosPitch, T *rosYaw)
359+
{
360+
double imuRoll, imuPitch, imuYaw;
361+
tf::Quaternion orientation;
362+
tf::quaternionMsgToTF(thisImuMsg->orientation, orientation);
363+
tf::Matrix3x3(orientation).getRPY(imuRoll, imuPitch, imuYaw);
364+
365+
*rosRoll = imuRoll;
366+
*rosPitch = imuPitch;
367+
*rosYaw = imuYaw;
368+
}
369+
370+
371+
float pointDistance(PointType p)
372+
{
373+
return sqrt(p.x*p.x + p.y*p.y + p.z*p.z);
374+
}
375+
376+
377+
float pointDistance(PointType p1, PointType p2)
378+
{
379+
return sqrt((p1.x-p2.x)*(p1.x-p2.x) + (p1.y-p2.y)*(p1.y-p2.y) + (p1.z-p2.z)*(p1.z-p2.z));
380+
}
381+
382+
#endif

‎src/lidar_imu_fusion/msg/cloud_img.msg

+3
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,3 @@
1+
Header header
2+
sensor_msgs/PointCloud2 local_map
3+
sensor_msgs/Image img

‎src/lidar_imu_fusion/msg/cloud_info.msg

+29
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,29 @@
1+
# Cloud Info
2+
Header header
3+
4+
int32[] startRingIndex
5+
int32[] endRingIndex
6+
7+
int32[] pointColInd # point column index in range image
8+
float32[] pointRange # point range
9+
10+
int64 imuAvailable
11+
int64 odomAvailable
12+
13+
# Attitude for LOAM initialization
14+
float32 imuRollInit
15+
float32 imuPitchInit
16+
float32 imuYawInit
17+
18+
# Initial guess from imu pre-integration
19+
float32 initialGuessX
20+
float32 initialGuessY
21+
float32 initialGuessZ
22+
float32 initialGuessRoll
23+
float32 initialGuessPitch
24+
float32 initialGuessYaw
25+
26+
# Point cloud messages
27+
sensor_msgs/PointCloud2 cloud_deskewed # original cloud deskewed
28+
sensor_msgs/PointCloud2 cloud_corner # extracted corner feature
29+
sensor_msgs/PointCloud2 cloud_surface # extracted surface feature

‎src/lidar_imu_fusion/package.xml

+96
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,96 @@
1+
<?xml version="1.0"?>
2+
<package format="2">
3+
<name>lidar_imu_fusion</name>
4+
<version>0.0.0</version>
5+
<description>The lidar_imu_fusion package</description>
6+
7+
<!-- One maintainer tag required, multiple allowed, one person per tag -->
8+
<!-- Example: -->
9+
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
10+
<maintainer email="weisong@todo.todo">weisong</maintainer>
11+
12+
13+
<!-- One license tag required, multiple allowed, one license per tag -->
14+
<!-- Commonly used license strings: -->
15+
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
16+
<license>TODO</license>
17+
18+
19+
<!-- Url tags are optional, but multiple are allowed, one per tag -->
20+
<!-- Optional attribute type can be: website, bugtracker, or repository -->
21+
<!-- Example: -->
22+
<!-- <url type="website">http://wiki.ros.org/lidar_imu_fusion</url> -->
23+
24+
25+
<!-- Author tags are optional, multiple are allowed, one per tag -->
26+
<!-- Authors do not have to be maintainers, but could be -->
27+
<!-- Example: -->
28+
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
29+
30+
31+
<!-- The *depend tags are used to specify dependencies -->
32+
<!-- Dependencies can be catkin packages or system dependencies -->
33+
<!-- Examples: -->
34+
<!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
35+
<!-- <depend>roscpp</depend> -->
36+
<!-- Note that this is equivalent to the following: -->
37+
<!-- <build_depend>roscpp</build_depend> -->
38+
<!-- <exec_depend>roscpp</exec_depend> -->
39+
<!-- Use build_depend for packages you need at compile time: -->
40+
<!-- <build_depend>message_generation</build_depend> -->
41+
<!-- Use build_export_depend for packages you need in order to build against this package: -->
42+
<!-- <build_export_depend>message_generation</build_export_depend> -->
43+
<!-- Use buildtool_depend for build tool packages: -->
44+
<!-- <buildtool_depend>catkin</buildtool_depend> -->
45+
<!-- Use exec_depend for packages you need at runtime: -->
46+
<!-- <exec_depend>message_runtime</exec_depend> -->
47+
<!-- Use test_depend for packages you need only for testing: -->
48+
<!-- <test_depend>gtest</test_depend> -->
49+
<!-- Use doc_depend for packages you need only for building documentation: -->
50+
<!-- <doc_depend>doxygen</doc_depend> -->
51+
<buildtool_depend>catkin</buildtool_depend>
52+
<build_depend>cv_bridge</build_depend>
53+
<build_depend>geometry_msgs</build_depend>
54+
<build_depend>image_transport</build_depend>
55+
<build_depend>message_generation</build_depend>
56+
<build_depend>nav_msgs</build_depend>
57+
<build_depend>pcl_conversions</build_depend>
58+
<build_depend>roscpp</build_depend>
59+
<build_depend>roslib</build_depend>
60+
<build_depend>rospy</build_depend>
61+
<build_depend>sensor_msgs</build_depend>
62+
<build_depend>std_msgs</build_depend>
63+
<build_depend>tf</build_depend>
64+
<build_export_depend>cv_bridge</build_export_depend>
65+
<build_export_depend>geometry_msgs</build_export_depend>
66+
<build_export_depend>image_transport</build_export_depend>
67+
<build_export_depend>nav_msgs</build_export_depend>
68+
<build_export_depend>pcl_conversions</build_export_depend>
69+
<build_export_depend>roscpp</build_export_depend>
70+
<build_export_depend>roslib</build_export_depend>
71+
<build_export_depend>rospy</build_export_depend>
72+
<build_export_depend>sensor_msgs</build_export_depend>
73+
<build_export_depend>std_msgs</build_export_depend>
74+
<build_export_depend>tf</build_export_depend>
75+
<build_export_depend>message_generation</build_export_depend>
76+
<exec_depend>cv_bridge</exec_depend>
77+
<exec_depend>geometry_msgs</exec_depend>
78+
<exec_depend>image_transport</exec_depend>
79+
<exec_depend>nav_msgs</exec_depend>
80+
<exec_depend>pcl_conversions</exec_depend>
81+
<exec_depend>roscpp</exec_depend>
82+
<exec_depend>roslib</exec_depend>
83+
<exec_depend>rospy</exec_depend>
84+
<exec_depend>sensor_msgs</exec_depend>
85+
<exec_depend>std_msgs</exec_depend>
86+
<exec_depend>tf</exec_depend>
87+
<exec_depend>message_runtime</exec_depend>
88+
89+
90+
91+
<!-- The export tag contains other, unspecified, tags -->
92+
<export>
93+
<!-- Other tools can request additional information be placed here -->
94+
95+
</export>
96+
</package>

‎src/lidar_imu_fusion/src/featureExtraction.cpp

+368
Large diffs are not rendered by default.

‎src/lidar_imu_fusion/src/gpsConverter.cpp

+119
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,119 @@
1+
#include <sensor_msgs/NavSatFix.h>
2+
#include "utility.h"
3+
class GpsConvert : public ParamServer{
4+
private:
5+
ros::Subscriber subImu;
6+
ros::Subscriber subOdom;
7+
ros::Subscriber subGps;
8+
ros::Publisher pubOdom;
9+
ros::Publisher pubOdomPath;
10+
nav_msgs::Path odomPath;
11+
std::vector<double> vecTimeGps;
12+
std::vector<bool> vecGpsFlag;
13+
bool publishGpsData;
14+
ofstream outFileRtkPose;
15+
public:
16+
GpsConvert():
17+
18+
//订阅发布数据
19+
publishGpsData(false){
20+
subOdom = nh.subscribe<nav_msgs::Odometry> (gpsRawTopic, 2000, &GpsConvert::odometryHandler, this, ros::TransportHints().tcpNoDelay());
21+
if(gpsRawTopic != gpsTopic){
22+
subGps = nh.subscribe<sensor_msgs::NavSatFix>(gpsFixTopic, 2000, &GpsConvert::gpsHandler, this, ros::TransportHints().tcpNoDelay());
23+
pubOdom = nh.advertise<nav_msgs::Odometry> (gpsTopic,2000);
24+
publishGpsData = true;
25+
}
26+
pubOdomPath = nh.advertise<nav_msgs::Path> ("/odom/path", 1);
27+
odomPath.header.frame_id = "/map";
28+
//是否保留轨迹
29+
if(saveTrajectory){
30+
outFileRtkPose.open("/data_work/bag/rtk_pose.txt",ios::binary | ios::trunc| ios::in | ios::out);
31+
}
32+
}
33+
~GpsConvert(){
34+
if(saveTrajectory){
35+
outFileRtkPose.close();
36+
}
37+
}
38+
39+
40+
void odometryHandler(const nav_msgs::Odometry::ConstPtr& odomMsg){
41+
int index = -1;
42+
double time = 0.1;
43+
double timeStamp = odomMsg->header.stamp.toSec();
44+
if(publishGpsData){
45+
for (int i = 0; i < vecTimeGps.size(); ++i)
46+
{
47+
double time_diff = timeStamp - vecTimeGps[i];
48+
if(time_diff < 0 && abs(time_diff) < time){
49+
index = i;
50+
break;
51+
}
52+
else if(time_diff < 0){
53+
break;
54+
}
55+
else if(time_diff < time){
56+
time = time_diff;
57+
index = i;
58+
}
59+
}
60+
if (-1 != index)
61+
{
62+
nav_msgs::Odometry odom = *odomMsg;
63+
if (vecGpsFlag[index])
64+
{
65+
odom.pose.covariance[0] = 0.00001;
66+
odom.pose.covariance[7] = 0.00001;
67+
odom.pose.covariance[14] = 0.00001;
68+
69+
}
70+
else{
71+
odom.pose.covariance[0] = 999;
72+
odom.pose.covariance[7] = 999;
73+
odom.pose.covariance[14] = 999;
74+
}
75+
pubOdom.publish(odom);
76+
vecGpsFlag.erase(vecGpsFlag.begin(), vecGpsFlag.begin() + index + 1);
77+
vecTimeGps.erase(vecTimeGps.begin(),vecTimeGps.begin()+index+1);
78+
}
79+
}
80+
81+
static double last_path_time = -1;
82+
if (timeStamp - last_path_time > 0.1)
83+
{
84+
last_path_time = timeStamp;
85+
geometry_msgs::PoseStamped pose_odm;
86+
pose_odm.header.stamp = odomMsg->header.stamp;
87+
pose_odm.header.frame_id = "/map";
88+
pose_odm.pose.position = odomMsg->pose.pose.position;
89+
pose_odm.pose.orientation = odomMsg->pose.pose.orientation;
90+
odomPath.poses.push_back(pose_odm);
91+
92+
if(pubOdomPath.getNumSubscribers() != 0){
93+
odomPath.header.stamp = odomMsg->header.stamp;
94+
odomPath.header.frame_id = "/map";
95+
pubOdomPath.publish(odomPath);
96+
}
97+
}
98+
if(saveTrajectory){
99+
outFileRtkPose << std::setprecision(19) << odomMsg->header.stamp.toSec() << " " << odomMsg->pose.pose.position.x << " "
100+
<< odomMsg->pose.pose.position.y << " " << odomMsg->pose.pose.position.z << " " << odomMsg->pose.pose.orientation.x << " "
101+
<< odomMsg->pose.pose.orientation.y << " " << odomMsg->pose.pose.orientation.z << " " << odomMsg->pose.pose.orientation.w << "\n";
102+
}
103+
}
104+
105+
void gpsHandler(const sensor_msgs::NavSatFix::ConstPtr& gpsMsg){
106+
sensor_msgs::NavSatFix gpsData = *gpsMsg;
107+
vecTimeGps.push_back(gpsMsg->header.stamp.toSec());
108+
vecGpsFlag.push_back((sensor_msgs::NavSatStatus::STATUS_FIX == gpsMsg->status.status));
109+
}
110+
};
111+
112+
113+
int main(int argc, char** argv){
114+
ros::init(argc, argv, "lidar");
115+
GpsConvert gps_tool;
116+
ROS_INFO("\033[1;32m----> GPS Converter Started.\033[0m");
117+
ros::spin();
118+
return 0;
119+
}

‎src/lidar_imu_fusion/src/imageProjection.cpp

+875
Large diffs are not rendered by default.

‎src/lidar_imu_fusion/src/imuPreintegration.cpp

+653
Large diffs are not rendered by default.

‎src/lidar_imu_fusion/src/mapOptmization.cpp

+2,302
Large diffs are not rendered by default.

‎src/lidar_imu_fusion/srv/reset_lidar_odometry.srv

+4
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,4 @@
1+
bool resetCloud
2+
---
3+
float64 resetCloudTime
4+
bool success

0 commit comments

Comments
 (0)
Please sign in to comment.