以读“pcd”:
读取4个激光雷达数据,input
estimator.inputCloud(cloud_time, laser_cloud_list);
inputCloud展开:
给每个激光雷达的三维点赋⼀个0-1值,为这个点的时间戳相对于这⼀帧点云的相对时
间,这个与A-LOAM⼀致
f_extract_.calTimestamp(v_laser_cloud_in[i], laser_cloud);
聚类分割
⽤类似Lego LOAM的⽅法,给点云⽣成深度图
深度优先搜索对上述深度图进⾏聚类 去除聚类中尺⼨过小或者过⼤的。
⽣成⼀个与上述图同样⼤的label_mat。
- -1表⽰对应的像素处⽆数据
- 999999表⽰⽆效的聚类
- 否则表⽰这个像素属于的聚类的标号
根据label_mat来去除点云中的⽆效点
img_segment_.segmentCloud(laser_cloud, laser_cloud_segment, laser_cloud_outlier, scan_info);
特征提取
f_extract_.extractCloud(laser_cloud_segment, scan_info, *feature_frame_ptr[i]);
求出的特征都保存到cloudFeature这样一个数据结构里
std::queue<std::pair<double, std::vector<cloudFeature> > > feature_buf_;
feature_buf_报错了当前4个激光雷达的特征和时间戳,以队列+pair形式
cloudFeature是字符串-点云对应格式,相当于给点云打一个label
typedef std::map<std::string, common::PointICloud> cloudFeature;
然后就是数据处理:
processMeasurements();
展开里面有一个 process();和 pubOdometry(*this, cur_time_);
process()展开:
如果没有先验外参,对于每个激光雷达,trackCloud以获得每帧的相对位移与绝对位姿,同每个激光雷达单独计算里程计
pose_rlt_[n] = lidar_tracker_.trackCloud(prev_cloud_feature, cur_cloud_feature, pose_rlt_[n], cur_feature_.first, n);
pose_rlt_ 保存存了第i时刻 全部雷达的运动增量数据
将上⾯得到的每个激光雷达的相对位姿加⼊到待优化队列中
if (initial_extrinsics_.addPose(pose_rlt_) && (cir_buf_cnt_ == WINDOW_SIZE))
对于每⼀个激光雷达,标定其到主激光雷达的外参的旋转
if (initial_extrinsics_.calibExRotation(IDX_REF, n, calib_result))
成功则标定其到主激光雷达的外参的平移
if (initial_extrinsics_.calibExTranslation(IDX_REF, n, calib_result))
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)