i2nav-whu / ff-lins Goto Github PK

在铁轨火车运行，柴油机震动较大，收集的livox horizon数据集一运行程序就died，也按照要求imu修改成front 、right、down，在fusion.cc如下
// IMU measurements, Front-Right-Down
imu_.dtheta[0] = imumsg->angular_velocity.x * imu_.dt;
imu_.dtheta[1] = -imumsg->angular_velocity.y * imu_.dt;
imu_.dtheta[2] = -imumsg->angular_velocity.z * imu_.dt;
imu_.dvel[0] = imumsg->linear_acceleration.x * imu_.dt;
imu_.dvel[1] = -imumsg->linear_acceleration.y * imu_.dt;
imu_.dvel[2] = -imumsg->linear_acceleration.z * imu_.dt;
运行结果如下
PARAMETERS

• /ff_lins_node/configfile: /home/yijing/pack...
• /ff_lins_node/imu_topic: /livox/imu_3WEDJB...
• /ff_lins_node/lidar_topic: /livox/lidars_3WE...
• /imu/ab_prior_std: 30000
• /imu/abstd: 500
• /imu/arw: 2.0
• /imu/corrtime: 1.0
• /imu/gb_prior_std: 1500
• /imu/gbstd: 200
• /imu/imudatarate: 200
• /imu/vrw: 5.0
• /is_make_outputdir: True
• /is_use_visualization: True
• /lidar/downsample_size: 0.5
• /lidar/farthest_distance: 250.0
• /lidar/frame_rate: 10
• /lidar/is_save_pointcloud: False
• /lidar/is_use_lidar: False
• /lidar/minimum_keyframe_translation: 0.4
• /lidar/nearest_distance: 1
• /lidar/plane_estimation_threshold: 0.1
• /lidar/point_filter_num: 10
• /lidar/q_b_l: [1, 0, 0, 0]
• /lidar/scan_line: 6
• /lidar/t_b_l: [0.05512, 0.02226...
• /lidar/td_b_l: 0.0
• /optimizer/optimize_estimate_lidar_extrinsic: True
• /optimizer/optimize_estimate_lidar_td: True
• /optimizer/optimize_lidar_extrinsic_accurate: False
• /optimizer/optimize_point_to_plane_std: 0.1
• /optimizer/optimize_window_size: 10
• /outputpath: /home/yijing/pack...
• /rosdistro: noetic
• /rosversion: 1.16.0

NODES
/
ff_lins_node (ff_lins/ff_lins_ros)
visualisation (rviz/rviz)
world_to_map_broadcaster (tf2_ros/static_transform_publisher)

auto-starting new master
process[master]: started with pid [252783]
ROS_MASTER_URI=http://localhost:11311

setting /run_id to f4d060a2-de13-11ee-a3fc-73eac6e823a8
process[rosout-1]: started with pid [252808]
started core service [/rosout]
process[ff_lins_node-2]: started with pid [252815]
process[world_to_map_broadcaster-3]: started with pid [252816]
process[visualisation-4]: started with pid [252817]
Fusion process is started...
Check thread is started...
I0309 20:52:59.303265 252815 lidar_map.cc:65] Lidar mapping is constructed
I0309 20:52:59.303401 252832 lidar_viewer_rviz.cc:50] Lidar viewer thread is started
I0309 20:52:59.303548 252815 optimizer.cc:83] Optimizer is constructed
I0309 20:52:59.303622 252833 ff_lins.cc:176] Fusion thread is started
I0309 20:52:59.305052 252815 fusion.cc:138] Waiting ROS message...
I0309 20:53:13.214826 252833 misc.cc:198] Redo INS mechanization at 353296.000000 to 353297.000355 is 1.00036 s
W0309 20:53:13.214896 252833 ff_lins.cc:629] Get roll 0.107047, pitch -0.0958466 without zero velocity
I0309 20:53:13.215011 252833 misc.cc:198] Redo INS mechanization at 353296.000000 to 353297.000355 is 1.00036 s
I0309 20:53:13.215032 252833 ff_lins.cc:674] Initialization at 353296.000000
I0309 20:53:13.216490 252833 ff_lins.cc:506] Add new IMU time node at 353297.000000 with dt 1.000
I0309 20:53:13.236536 252833 ff_lins.cc:450] Append lidar frame to node 353297.000000
I0309 20:53:13.252902 252833 ff_lins.cc:794] Accumulate 10 frames at 353297.000000 with 13405 points
I0309 20:53:13.392108 252833 lidar_map.cc:83] New lidar keyframe at 353297.117165 with interval 0.117165 s, translation 1.09229, rotation 0.000750995
I0309 20:53:13.393277 252833 ff_lins.cc:237] Accumulate 2 frames at 353297.117165 with 4093 points
I0309 20:53:13.393476 252833 ff_lins.cc:506] Add new lidar time node at 353297.117165 with dt 0.117
I0309 20:53:13.407680 252833 lidar_map.cc:406] Find the newest 42 featureas and average 42 features at 353297.117165, update map cost 0.443458 ms
I0309 20:53:13.407718 252833 optimizer.cc:408] Total 3 pose states from 353296.000000 to 353297.117165
I0309 20:53:13.407793 252833 optimizer.cc:128] Add 2 preintegration, 42 lidar factors
I0309 20:53:13.408572 252833 optimizer.cc:157] Remove 12 lidar outliers factors
I0309 20:53:13.408805 252833 optimizer.cc:183] Optimization costs 0.742338 ms and 0.148943, with iteration 1 and 1
I0309 20:53:13.409003 252833 misc.cc:198] Redo INS mechanization at 353297.117165 to 353297.117335 is 0.000169754 s
I0309 20:53:13.492069 252833 lidar_map.cc:83] New lidar keyframe at 353297.217165 with interval 0.0999994 s, translation 1.0223, rotation 0.000909034
I0309 20:53:13.492628 252833 ff_lins.cc:237] Accumulate 1 frames at 353297.217165 with 2411 points
[ff_lins_node-2] process has died [pid 252815, exit code -11, cmd /home/yijing/packagegit/ff_lins/devel/lib/ff_lins/ff_lins_ros __name:=ff_lins_node __log:=/home/yijing/.ros/log/f4d060a2-de13-11ee-a3fc-73eac6e823a8/ff_lins_node-2.log].
log file: /home/yijing/.ros/log/f4d060a2-de13-11ee-a3fc-73eac6e823a8/ff_lins_node-2*.log

Thanks for sharing the great work!
But I have a question. In the paper, I saw that the standard deviation of some dimensions increases over time (such as yaw), but when optimizing with ceres, how to get the covariance of the estimated result (or the standard deviation of the corresponding dimension)?
中文：
但是有个疑问想请教一下，在论文中看到了一些维度的标准差在随时间增长(比如yaw的标准差)，但是用ceres进行优化时，是如何得到估计结果的协方差(或对应维度的标准差呢)

Hello, which LIVOX lidar is the author using, the IMU orientation of the three axes of the 3D map I built after running with livox-avia does not seem to be quite right. Where should I change the configuration file, and what configuration should I change to?

Hi, @thlsealight
Thanks for your nice work!
Is that the derivative of r with respect to R_b_r ?

使用avia和mid360时，在fusion.cc中变换IMU坐标系为前右下，同时外参配置里的q配置成（1，0，0，0）.但是运行后，雷达一动就会点云漂移。不知道是否哪里配置错误。

The reference 10 is VISUAL-INERTIAL NAVIGATION: A CONCISE REVIEW. Within it, I find no explaination. Maybe I somehow missed it. Could you help me by pointing it out.
According to my own understanding, if there is a big difference between the drifts of old and new keyframes, the built local map would be inconsistent, and it is likely the scan to map registration would be unstable or even fail. When we use keyframes that are close in terms of time, there is no such worry. However, we should use GPS and loop closure information to keep drift small anyway. When there is a big drift, inconsistency within local map is not my biggest concern, there is no point to continue the slam anyway. When we build local map using spatial information instead of a simple time window, it provides mid-term and long-term data association that is good to reduce drift. And slide-window approach introduces too many keyframes into the map its memory footprint grows quickly and unboundedly. So In my opinion, spatially local map is good for global consistency but bad for local consistency, slide window good for local consistency but bad for global consistency, and the former beats the latter hands-down in memory footprint.