admin管理员组文章数量:1660165
从网上搜索大量的文献来解决这一部分难题,进度上更慢一些,理解可能会有偏差,有错误之处请多多指出
数据处理+Local SLAM
输入:
1.激光雷达的数据经过Voxel Filter、Adaptive Voxel Filter(调用体素滤波,如果体素滤波后点数大于阈值,则返回,如果小于阈值,则接着使用二分法进行体素滤波)后的点云数据
2.经过里程计、惯导及点云匹配后的位姿态预估下一时刻的位姿
输出:
1.Submaps
Local SLAM主要是运用了ScanMatching的方法,当获得一次扫描数据后,扫描数据与当前最近的submap匹配,获得最优的位置姿态(????)后,将这一帧点云加入到submap中,当submap里的帧数达到一定数目(关键帧插入方法???)后就不在更新这个子地图,接下来会创建下一个submap继续以上工作。
流程:
- 多激光传感器数据基于时间戳同步融合
- 开启位置估计器
- 根据位置估计器,针对每个点云point的时间戳进行预测位置进行畸变校准
- 校准后的点云转换成scanmath和map 更新使用的range格式,并将miss和hit分类
- 获取校准后的origin pos
- 获取预测的重力加速度方向
- 根据重力加速度方向投影点云
- 降采样滤波,并抛弃设置高度范围外的所有点云
- 然后调用匹配方法
- 插入并更新submap
- 获取匹配后的结果内容,包括时间,轨迹节点位置,节点位置对应点云,submap
Local SLAM之相关匹配(correlative scan matcher)
原理
目前寻找的最好的资料:Real-Time Correlative Scan Matching完全解析(CSM帧匹配算法)
通过Real-time correlative scan matching 论文算法分析、[翻译]Real-Time Correlative Scan Matching对算法有了大致的了解,Cartographer是采取的双搜索的方式进行的, 先用一次real-time correlative scan matcher(三维窗口遍历寻优),再构建优化等式,利用ceres优化求解。(栅格概率, T的偏差,R的偏差)
数学模型
模型为利用最大似然估计计算在t时刻最有可能的位姿,可以理解为匹配的越好时候的位姿,上面公式的概率越大,我们的目的就是求解这个,其中
、表示t时刻的位姿数据和激光雷达的观测数据;
在此处理解为距离最近的submap;
,表示为时刻的位姿和控制量信息,控制信息可以理解为从里程计和IMU推导出来的数据,有地方公式将也表达为;
为观测模型,理解为在位姿,地图下,观测数据为的概率,详细可以参照卡尔曼滤波相关知识;
为运动模型,理解为在,下时刻最可能的位姿。
公式细化
虽然有公式了,但是还不够详细,激光雷达来一帧数据,是实实在在的数据,得踏踏实实的处理这些数据
1.运动模型
cartographer部分利用imu构建预测模型,采取UKF进行运动预测。
具体运动模型推测见:cartographer 处理IMU(激光,里程计等)流程
2.观测模型
单线激光雷达的数据一般是一个距离和一个角度的极坐标信息,通过求解可以得出x,y;多线激光雷达直接输出x,y,z
一帧数据写到每个点上:
其中为激光雷达每一个点的概率值,将数据进行栅格化后再计算
栅格地图的知识可以参考占据栅格地图(Occupancy Grid Map)
每一帧数据都可进行栅格化,如下图所示,用来表示submap,为当前帧的数据所形成的栅格地图
cartographer 是一种暴力搜索的模式,主要的算法流程:
1.在时刻设定一个search window(这个search window用来和当前的激光雷达的帧的数据进行匹配)
2.在search window区域生成当前的位姿的可能值,作为一个子集
3.遍历所有的子集,计算每一个子集上所有的概率和,概率最高的那个位姿就是我们要找的当前时刻的位姿(概率和?当前帧数据的栅格和submap里的概率和)
算法流程(未完成)
1.构造粗分辨率和细分辨率两个似然场
2.在粗分辨率似然场上进行搜索,获取最优位姿
3.粗分辨率最优位姿对应的栅格进行细分辨率划分
4.进行细分辨率搜索,获取最优位姿
5.计算位姿匹配方差
代码解析
double RealTimeCorrelativeScanMatcher2D::Match(
const transform::Rigid2d& initial_pose_estimate,
const sensor::PointCloud& point_cloud, const Grid2D& grid,
transform::Rigid2d* pose_estimate) const {
CHECK(pose_estimate != nullptr);
//获取初始位置的角度
const Eigen::Rotation2Dd initial_rotation = initial_pose_estimate.rotation();
//将当前点云转换到初始角度坐标系下
const sensor::PointCloud rotated_point_cloud = sensor::TransformPointCloud(
point_cloud,
transform::Rigid3f::Rotation(Eigen::AngleAxisf(
initial_rotation.cast<float>().angle(), Eigen::Vector3f::UnitZ())));
//定义搜索空间,包括平移的窗口和角度窗口
//option_参数由上层调用传入??????
const SearchParameters search_parameters(
options_.linear_search_window(), options_.angular_search_window(),
rotated_point_cloud, grid.limits().resolution());
//在一定的角度搜索空间中(一定的窗口内,一定分辨率,对当前点云放入vector中)
const std::vector<sensor::PointCloud> rotated_scans =
GenerateRotatedScans(rotated_point_cloud, search_parameters);
//将点云进行坐标变换转换到世界坐标系下,根据当前的位置(里程计的位置)进行转换
const std::vector<DiscreteScan2D> discrete_scans = DiscretizeScans(
grid.limits(), rotated_scans,
Eigen::Translation2f(initial_pose_estimate.translation().x(),
initial_pose_estimate.translation().y()));
//初始化xy平移空间内偏移量,即窗口大小和偏移量
std::vector<Candidate2D> candidates =
GenerateExhaustiveSearchCandidates(search_parameters);
//计算当前帧在角度、x、y三层窗口中每个状态在grid中的匹配评分
ScoreCandidates(grid, discrete_scans, search_parameters, &candidates);
//采用标准库,迭代获取最大置信度元素
const Candidate2D& best_candidate =
*std::max_element(candidates.begin(), candidates.end());
//坐标转换为全局坐标
*pose_estimate = transform::Rigid2d(
{initial_pose_estimate.translation().x() + best_candidate.x,
initial_pose_estimate.translation().y() + best_candidate.y},
initial_rotation * Eigen::Rotation2Dd(best_candidate.orientation));
return best_candidate.score;
}
基础太差了,全都细细的分析,步履维艰,脑补c++
1.double RealTimeCorrelativeScanMatcher2D::Match(const transform::Rigid2d& initial_pose_estimate,const sensor::PointCloud& point_cloud, const Grid2D& grid,transform::Rigid2d* pose_estimate)
1.1 transform::Rigid2d& initial_pose_estimate(定义在catkin_ws/src_carto/cartographer/cartographer/transform/transform.h)//初始的位姿信息
template <typename FloatType> class Rigid2{};
using Rigid2d = Rigid2<double>;
1.2 const sensor::PointCloud& point_cloud;点云的类型
1.3 transform::Rigid2d* pose_estimate //估计的位姿
2.const Eigen::Rotation2Dd initial_rotation = initial_pose_estimate.rotation();
3.const sensor::PointCloud rotated_point_cloud = sensor::TransformPointCloud(point_cloud,transform::Rigid3f::Rotation(Eigen::AngleAxisf(initial_rotation.cast<float>().angle(), Eigen::Vector3f::UnitZ())));
3.1const sensor::PointCloud
定义在(/catkin_ws/src_carto/cartographer/cartographer/sensor/point_cloud.h)
using PointCloud = std::vector<RangefinderPoint>;
struct RangefinderPoint {
Eigen::Vector3f position;
};
3.2 sensor::TransformPointCloud函数( vector::reserve 、c++11 之emplace_back 与 push_back的区别)
PointCloud TransformPointCloud(const PointCloud& point_cloud,
const transform::Rigid3f& transform) {
PointCloud result;
result.reserve(point_cloud.size());
for (const RangefinderPoint& point : point_cloud) {
result.emplace_back(transform * point);
//c++开发中我们会经常用到插入操作对stl的各种容器进行操作,常使用push_back()向容器中加入一个右值元
//素(临时对象)时,首先会调用构造函数构造这个临时对象,然后需要调用拷贝构造函数将这个临时对象放入容
//器中。原来的临时变量释放。这样造成的问题就是临时变量申请资源的浪费。
//引入了右值引用,转移构造函数后,push_back()右值时就会调用构造函数和转移构造函数,如果可以在插入
//的时候直接构造,就只需要构造一次即可。这就是c++11 新加的emplace_back。
}
return result;
}
for实现对vector的遍历
假设实现一个vector遍历可以用以下方法
vector<
int
> line={1,2,3,4,5,6,7,8,9};
(1)
for (vector<int>::const_iterator iter = line.cbegin();iter != line.cend(); iter++) {
cout << (*iter) << endl;
}
(2)
for (auto iter = line.cbegin(); iter != line.cend(); iter++) {
cout << (*iter) << endl;
}
(3)
for (auto lin : line) {
cout << lin;
}
4.const SearchParameters search_parameters(options_.linear_search_window(), options_.angular_search_window(),rotated_point_cloud, grid.limits().resolution());
4.1 SearchParameters
定义在/catkin_ws/src_carto/cartographer/cartographer/mapping/internal/2d/scan_matching/correlative_scan_matcher_2d.h
struct SearchParameters {
// Linear search window in pixel offsets; bounds are inclusive.
struct LinearBounds {
int min_x;
int max_x;
int min_y;
int max_y;
};
SearchParameters(double linear_search_window, double angular_search_window,
const sensor::PointCloud& point_cloud, double resolution);
SearchParameters::SearchParameters(const double linear_search_window,
const double angular_search_window,
const sensor::PointCloud& point_cloud,
const double resolution)
: resolution(resolution) {
// We set this value to something on the order of resolution to make sure that
// the std::acos() below is defined.
float max_scan_range = 3.f * resolution;
for (const sensor::RangefinderPoint& point : point_cloud) {
const float range = point.position.head<2>().norm();
max_scan_range = std::max(range, max_scan_range);
}
const double kSafetyMargin = 1. - 1e-3;
angular_perturbation_step_size =
kSafetyMargin * std::acos(1. - common::Pow2(resolution) /
(2. * common::Pow2(max_scan_range)));
num_angular_perturbations =
std::ceil(angular_search_window / angular_perturbation_step_size);
num_scans = 2 * num_angular_perturbations + 1;
const int num_linear_perturbations =
std::ceil(linear_search_window / resolution);
linear_bounds.reserve(num_scans);
for (int i = 0; i != num_scans; ++i) {
linear_bounds.push_back(
LinearBounds{-num_linear_perturbations, num_linear_perturbations,
-num_linear_perturbations, num_linear_perturbations});
}
}
5. const std::vector<sensor::PointCloud> rotated_scans = GenerateRotatedScans(rotated_point_cloud, search_parameters);
std::vector<sensor::PointCloud> GenerateRotatedScans(
const sensor::PointCloud& point_cloud,
const SearchParameters& search_parameters) {
std::vector<sensor::PointCloud> rotated_scans;//定义一个参数存储旋转后的数据
rotated_scans.reserve(search_parameters.num_scans);//预设大小
double delta_theta = -search_parameters.num_angular_perturbations *
search_parameters.angular_perturbation_step_size;//计算一个最大的角度变换量
for (int scan_index = 0; scan_index < search_parameters.num_scans;
++scan_index,delta_theta += search_parameters.angular_perturbation_step_size){
rotated_scans.push_back(sensor::TransformPointCloud(
point_cloud, transform::Rigid3f::Rotation(Eigen::AngleAxisf(
delta_theta, Eigen::Vector3f::UnitZ()))));//每一个点计算旋转之后的坐标
}
return rotated_scans;
}
6. const std::vector<DiscreteScan2D> discrete_scans = DiscretizeScans(grid.limits(), rotated_scans,Eigen::Translation2f(initial_pose_estimate.translation().x(),initial_pose_estimate.translation().y()));
std::vector<DiscreteScan2D> DiscretizeScans(
const MapLimits& map_limits, const std::vector<sensor::PointCloud>& scans,
const Eigen::Translation2f& initial_translation) {
std::vector<DiscreteScan2D> discrete_scans;
discrete_scans.reserve(scans.size());
for (const sensor::PointCloud& scan : scans) {
discrete_scans.emplace_back();
discrete_scans.back().reserve(scan.size());
for (const sensor::RangefinderPoint& point : scan) {
const Eigen::Vector2f translated_point =
Eigen::Affine2f(initial_translation) * point.position.head<2>();
discrete_scans.back().push_back(
map_limits.GetCellIndex(translated_point));
}
}
return discrete_scans;
}
7.std::vector<Candidate2D> candidates = GenerateExhaustiveSearchCandidates(search_parameters);///初始化x,y平移空间内偏移量
8.ScoreCandidates(grid, discrete_scans, search_parameters, &candidates);//计算得分
9.const Candidate2D& best_candidate = *std::max_element(candidates.begin(), candidates.end());//获取最大的置信度
10.*pose_estimate = transform::Rigid2d({initial_pose_estimate.translation().x() + best_candidate.x,//转换到全局坐标系
initial_pose_estimate.translation().y() + best_candidate.y},
initial_rotation * Eigen::Rotation2Dd(best_candidate.orientation));
11.return best_candidate.score;//返回得分
--------------------------------------
参考文献
1.cartographer 代码思想解读(1)- 相关匹配
2.Cartographe纯雷达计算位姿的前后端总结
3.Real-time correlative scan matching 论文算法分析
4.Cartographer中对激光雷达运动畸变的处理方法分析
本文标签: 数据处理算法入门核心Cartographer
版权声明:本文标题:cartographer从入门到放弃4---核心算法分析---数据处理与Local SLAM 内容由热心网友自发贡献,该文观点仅代表作者本人, 转载请联系作者并注明出处:https://www.elefans.com/dianzi/1729850530a1215348.html, 本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如发现本站有涉嫌抄袭侵权/违法违规的内容,一经查实,本站将立刻删除。
发表评论