Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

关于使用其他数据集的配置问题 #158

Open
DioBakery opened this issue Dec 1, 2024 · 2 comments
Open

关于使用其他数据集的配置问题 #158

DioBakery opened this issue Dec 1, 2024 · 2 comments

Comments

@DioBakery
Copy link

DioBakery commented Dec 1, 2024

使用私有数据集的时候能够很好地运行,但是使用其他数据集时候不确定要设定哪些参数,导致可能在开启img_enable时效果不如关闭时的效果,甚至产生较大漂移
例如:使用m2dgr的gate_01.bag时,使用如下参数:
config.yaml

feature_extract_enable : 0
point_filter_num : 1
max_iteration : 10
dense_map_enable : 1
filter_size_surf : 0.15
filter_size_map : 0.15
cube_side_length : 20
grid_size : 40
patch_size : 8
img_enable : 1
lidar_enable : 1
outlier_threshold : 300 # 78 100 156
ncc_en: false
ncc_thre: 0
img_point_cov : 1000 # 1000
laser_point_cov : 0.001 # 0.001
pose_output_en: false
delta_time: 0.0

common:
lid_topic: "/velodyne_points"
imu_topic: "/handsfree/imu"

preprocess:
lidar_type: 2 # 1:Livox LiDAR 2:VELO16 3:OUST64 4:XT32 5:Mid360
scan_line: 32 # 16 64 32
blind: 0.5 # blind x m disable

mapping:
acc_cov_scale: 100
gyr_cov_scale: 10000
extrinsic_T: [0.27255, -0.00053,0.17954] # lidar to imu
extrinsic_R: [1, 0, 0,
0, 1, 0,
0, 0, 1]

pcd_save:
pcd_save_en: false

camera:
img_topic: /camera/color/image_raw
Rcl: [0, -1, 0,
0, 0, -1,
1, 0, 0]
Pcl: [-0.27255, 0.00053, -0.17954]

camera.yaml

cam_model: Pinhole
cam_width: 640
cam_height: 480
cam_fx: 617.971050917033
cam_fy: 616.445131524790
cam_cx: 327.710279392468
cam_cy: 253.976983707814
cam_d0: 0.148000794688248
cam_d1: -0.217835187249065
cam_d2: 0
cam_d3: 0
效果如下:
img_enable=1:
Screenshot from 2024-12-01 23-54-24
img_enable=0:
Screenshot from 2024-12-01 23-54-11
将point_filter_num调小效果会稍好一些,但仍然会有偏移
相机和雷达的内外参是根据数据集标定配置的,但其他参数不太确定作用,烦请指教。

@xuankuzcr
Copy link
Member

m2dgr相机和LiDAR的数据未同步,从你第一张图的残影就可以看出来。由于相机和LiDAR的时间戳不是他们真实采样时刻的时间戳,不同时空系的数据加入一定会产生负优化。可以通过调整delta_time参数手调出一个time offset。

@DioBakery
Copy link
Author

DioBakery commented Dec 2, 2024

m2dgr相机和LiDAR的数据未同步,从你第一张图的残影就可以看出来。由于相机和LiDAR的时间戳不是他们真实采样时刻的时间戳,不同时空系的数据加入一定会产生负优化。可以通过调整delta_time参数手调出一个time offset。

了解了,但是如果camera是15hz而lidar是10hz,所以先尝试S3E数据集,这时出现了另一个问题:
p_imu->Process2(LidarMeasures, state, feats_undistort);这一步始终feats_undistort的size为0,使得一直无法建立ikdforest树,因而无法进行LIO进程,请问这是怎么回事呢?

#ifdef USE_IKFOM
        p_imu->Process(LidarMeasures, kf, feats_undistort);
        state_point = kf.get_x();
        pos_lid = state_point.pos + state_point.rot * state_point.offset_T_L_I;
        #else
        p_imu->Process2(LidarMeasures, state, feats_undistort);
        state_propagat = state;
        #endif

        if (lidar_selector->debug)
        {
            LidarMeasures.debug_show();
        }
        std::cout << "LidarMeasures.lidar->points.size(): " << LidarMeasures.lidar->points.size() << std::endl;
        std::cout << "feats_undistort->points.size(): " << feats_undistort->points.size() << std::endl;

output:

LidarMeasures.lidar->points.size(): 26583
feats_undistort->points.size(): 0

并且若feats_undistort的size有输出后,经过下采样输出的size会是1:
code:

/*** Segment the map in lidar FOV ***/
        #ifndef USE_ikdforest            
            lasermap_fov_segment();
        #endif
        /*** downsample the feature points in a scan ***/
        downSizeFilterSurf.setInputCloud(feats_undistort);
        std::cout << "-----feats_undistort->points.size(): " << feats_undistort->points.size() << endl;
        downSizeFilterSurf.filter(*feats_down_body);
        std::cout << "-----feats_down_body->points.size(): " << feats_down_body->points.size() << endl;

output:

[ INFO ]: get standard point cloud at time: 1660985901.423132 and size: 24880, before: 633600
LidarMeasures.lidar->points.size(): 25127
feats_undistort->points.size(): 25127
-----feats_undistort->points.size(): 25127
-----feats_down_body->points.size(): 1

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

No branches or pull requests

2 participants