目錄
傳感器數據的走向
體素濾波與之后的處理
3D情況下的激光雷達數據的預處理
初始位姿估計
位姿推測器的優缺點分析與總結
可能有問題的點
可能的改進建議
傳感器數據的走向
傳感器數據從 CollatedTrajectoryBuilder類的HandleCollatedSensorData函數 傳遞GlobalTrajectoryBuilder類 的相應函數
從這里開始, 傳感器數據才真正進入到SLAM的前端與后端部分
運動畸變去除后的點云, 點的坐標相對于local_frame了, 點云依然圍繞著tracking_frame
sensor::RangefinderPoint hit_in_local = range_data_poses[i] * sensor::ToRangefinderPoint(hit);
以點云的時間(也就是最后一個點的時間)預測出來的坐標做為點云的origin
accumulated_range_data_.origin = range_data_poses.back().translation()
計算從tracking_frame變換到local_frame原點并且變換后姿態為0的坐標變換
transform_to_gravity_aligned_frame = gravity_alignment.cast<float>() *
range_data_poses.back().inverse()
將點云進行平移與旋轉, 點的坐標相對于local_frame, 點云圍繞這local_frame坐標系原點
sensor::TransformRangeData(range_data, transform_to_gravity_aligned_frame)
進行z軸的過濾
sensor::CropRangeData(sensor::TransformRangeData(range_data,
transform_to_gravity_aligned_frame), options_.min_z(), options_.max_z())
單線雷達不能設置 大于0的min_z, 因為單線雷達的z為0
體素濾波與之后的處理
分別對 returns點云 與 misses點云 進行體素濾波
sensor::RangeData{ cropped.origin,
sensor::VoxelFilter(cropped.returns, options_.voxel_filter_size()),
sensor::VoxelFilter(cropped.misses, options_.voxel_filter_size())};對 returns點云 進行自適應體素濾波
sensor::AdaptiveVoxelFilter(gravity_aligned_range_data.returns,
options_.adaptive_voxel_filter_options())
將 原點位于local坐標系原點處的點云 變換成 原點位于匹配后的位姿處的點云
TransformRangeData(gravity_aligned_range_data, transform::Embed3D(pose_estimate_2d-
>cast<float>()) )
將 原點位于匹配后的位姿處的點云 返回到node.cc 中, node.cc將這個點云發布出去, 在rviz中可視化
3D情況下的激光雷達數據的預處理
進行多個雷達點云數據的時間同步
對點云進行第一次體素濾波
激光雷達數據運動畸變的校正, 同時將點云的相對于tracking_frame的點坐標 轉成 相對于local slam坐標系的 點坐標
分別對 returns 與 misses 進行第二次體素濾波
將原點位于機器人當前位姿處的點云 轉成 原點位于local坐標系原點處的點云
使用高分辨率進行自適應體素濾波 生成高分辨率點云
使用低分辨率進行自適應體素濾波 生成低分辨率點云
將 原點位于local坐標系原點處的點云 變換成 原點位于匹配后的位姿處的點云
將 原點位于匹配后的位姿處的點云 返回到node.cc 中, node.cc將這個點云發布出去, 在rviz中可視化
初始位姿估計
基于IMU與里程計的位姿推測器
重要成員變量說明
姿態預測相關
imu_tracker_ 只在添加位姿時更新, 用于保存添加校位姿準時的姿態
odometry_imu_tracker_ 只在添加位姿時更新, 用于根據里程計數據計算線速度時姿態的預測
extrapolation_imu_tracker_ 只在添加位姿時更新, 用于位姿預測時的姿態預測
通過里程計計算的線速度與角速度
linear_velocity_from_odometry_ 只在添加里程計數據時更新, 用于位姿預測時的平移量預測
angular_velocity_from_odometry_ 只在添加里程計數據時更新, 用于不使用imu數據時的imu_tracker_的角 速度的更新
通過pose計算的線速度與角速度
linear_velocity_from_poses_ 只在添加位姿時更新, 用于位姿預測時 不使用里程計數據時 平移量的預測
angular_velocity_from_poses_ 只在添加位姿時更新, 用于 不使用里程計數據時 的imu_tracker_的角速度的更新
傳感器數據隊列的個數
imu_date_ 隊列數據的個數最少是1個
odometry_data_ 隊列數據的個數最少是2個, 所以, odometry_data_.size() < 2 就意味著不使用里程計
timed_pose_queue_ 隊列數據的個數最少是2個
位姿推測器的優缺點分析與總結
預測位姿時的4種情況 都是勻速模型
使用imu, 使用里程計
平移的預測: 通過里程計數據隊列開始和末尾的2個數據計算出的線速度乘以時間
姿態的預測: 通過imu的角速度乘以時間
使用imu, 不使用里程計
平移的預測: 通過pose數據隊列開始和末尾的2個數據計算出的線速度乘以時間
姿態的預測: 通過imu的角速度乘以時間
不使用imu, 使用里程計
平移的預測: 通過里程計數據隊列開始和末尾的2個數據計算出的線速度乘以時間
姿態的預測: 通過里程計數據隊列開始和末尾的2個數據計算出的角速度乘以時間
不使用imu, 不是用里程計
平移的預測: 通過pose數據隊列開始和末尾的2個數據計算出的線速度乘以時間
姿態的預測: 通過pose數據隊列開始和末尾的2個數據計算出的角速度乘以時間
總結:
預測平移時: 有里程計就用里程計的線速度, 沒有里程計就用pose計算的線速度進行預測
預測姿態時: 有IMU就用IMU的角速度, 沒有IMU時, 如果有里程計就用里程計計算出的角速度, 沒有里程計就用
pose計算的角速度進行預測
預測的都是相對值, 要加上最后一個pose的位姿
可能有問題的點
計算pose的線速度與角速度時, 是采用的數據隊列開始和末尾的2個數據計算的
計算里程計的線速度與角速度時, 是采用的數據隊列開始和末尾的2個數據計算的
使用里程計, 不使用imu時, 計算里程計的線速度方向和姿態的預測時, 用的是里程計數據隊列開始和末尾的2個
數據的平均角速度計算的, 時間長了就不準
不使用里程計, 不使用imu時, 用的是pose數據隊列開始和末尾的2個數據的平均角速度計算的, 時間長了就不 準
添加位姿時, 沒有用pose的姿態對imu_tracker_進行校準, 也沒有對整體位姿預測器進行校準, 只計算了pose 的線速度與角速度
從代碼上看, cartographer認為位姿推測器推測出來的位姿與姿態是準確的
可能的改進建議
pose的距離越小, 勻速模型越能代替機器人的線速度與角速度, 計算pose的線速度與角速度時, 可以考慮使用 最近的2個數據進行計算
里程計距離越短數據越準, 計算里程計的線速度與角速度時, 可以考慮使用最近的2個數據進行計算
使用里程計, 不使用imu時, 計算里程計的線速度方向時, 可以考慮使用里程計的角度進行計算
使用里程計, 不使用imu時, 進行姿態的預測時, 可以考慮使用里程計的角度進行預測
不使用里程計, 不使用imu時, 可以考慮用最近的2個pose計算線速度與角速度
使用pose對imu_tracker_的航向角進行校準
