从零开始搭二维激光SLAM --- 简单重写 Hector SLAM
上篇文章讲解了如何在固定位置使用Hector构建单帧的栅格地图,以及知道了SLAM的本质就是将不同时刻的scan在正确的位置上写成栅格地图.本篇文章将对 Hector 进行简单的重写,使得其代码更简单,更清晰.这也是本系列教程第一次成功建出一张比较好的地图.先放图,虽然有些瑕疵,但是整体还是不错的.话不多说,先将代码做了那些改变,然后在着重讲解一下 Hector是如何用 高斯牛顿 就进行scan
上篇文章讲解了如何在固定位置使用Hector构建单帧的栅格地图,以及知道了SLAM的本质就是将不同时刻的scan在正确的位置上写成栅格地图.
本篇文章将对 Hector 进行简单的重写,使得其代码更简单,更清晰.
这也是本系列教程第一次成功建出一张比较好的地图.
先放图,虽然有些瑕疵,但是整体还是不错的.
话不多说,先说明一下对代码做了那些改变,然后再着重讲解一下 Hector是如何做scan match的.
1 代码简单重构
相对于原版Hector做了哪些改变
- 重写了原版的HectorMappingRos.cpp,使得代码变得更加简单与清晰
- 发布了 map -> odom -> base_link 的 TF树
- 去掉了原版的 DebugInfo 以及 Drawing 这两个功能以及代码文件.个人感觉这两个功能用不上,导致代码复杂
- 更改了一些文件的位置,使得现在的.cc文件只有一个,其余全是头文件.原版的src文件夹下有很多文件
- 对很多文件进行了注释
2 代码讲解
2.1 获取代码
代码已经提交在github上了,如果不知道github的地址的朋友, 请在我的公众号: 从零开始搭激光SLAM 中回复 开源地址 获得。
推荐使用 git clone 的方式进行下载, 因为代码是正处于更新状态的, git clone 下载的代码可以使用 git pull 很方便地进行更新.
本篇文章对应的代码为 Creating-2D-laser-slam-from-scratch/lesson4/src/hector_mapping/ 与 Creating-2D-laser-slam-from-scratch/lesson4/include/hector_mapping/hector_slam.cc
2.2 构造函数
在构造函数中 首先调用 InitParams 进行了 ROS参数的初始化.
之后对 HectorSlamProcessor 对象进行初始化.
再之后对多层地图进行了初始化,并调用 setMapInfo() 进行 ROS地图的内存的分配.
最后,查找从 base_link 到 雷达坐标系 间的坐标变换.
// 构造函数
HectorMappingRos::HectorMappingRos()
: private_node_("~"), lastGetMapUpdateIndex(-100), tfB_(0), map_publish_thread_(0)
{
ROS_INFO_STREAM("\033[1;32m----> Hector SLAM started.\033[0m");
// 参数初始化
InitParams();
laser_scan_subscriber_ = node_handle_.subscribe(p_scan_topic_, p_scan_subscriber_queue_size_, &HectorMappingRos::scanCallback, this); // 雷达数据处理
if (p_pub_odometry_)
{
odometryPublisher_ = node_handle_.advertise<nav_msgs::Odometry>("odom_hector", 50);
}
tfB_ = new tf::TransformBroadcaster();
slamProcessor = new hectorslam::HectorSlamProcessor(static_cast<float>(p_map_resolution_),
p_map_size_, p_map_size_,
Eigen::Vector2f(p_map_start_x_, p_map_start_y_),
p_map_multi_res_levels_);
slamProcessor->setUpdateFactorFree(p_update_factor_free_); // 0.4
slamProcessor->setUpdateFactorOccupied(p_update_factor_occupied_); // 0.9
slamProcessor->setMapUpdateMinDistDiff(p_map_update_distance_threshold_); // 0.4
slamProcessor->setMapUpdateMinAngleDiff(p_map_update_angle_threshold_); // 0.9
// 多层地图的初始化
int mapLevels = slamProcessor->getMapLevels();
mapLevels = 1; // 这里设置成只发布最高精度的地图,如果有其他需求,如进行路径规划等等需要多层地图时,注释本行。
std::string mapTopic_ = "map";
for (int i = 0; i < mapLevels; ++i)
{
mapPubContainer.push_back(MapPublisherContainer());
slamProcessor->addMapMutex(i, new HectorMapMutex());
std::string mapTopicStr(mapTopic_);
if (i != 0)
{
mapTopicStr.append("_" + boost::lexical_cast<std::string>(i));
}
std::string mapMetaTopicStr(mapTopicStr);
mapMetaTopicStr.append("_metadata");
MapPublisherContainer &tmp = mapPubContainer[i];
tmp.mapPublisher_ = node_handle_.advertise<nav_msgs::OccupancyGrid>(mapTopicStr, 1, true);
tmp.mapMetadataPublisher_ = node_handle_.advertise<nav_msgs::MapMetaData>(mapMetaTopicStr, 1, true);
setMapInfo(tmp.map_, slamProcessor->getGridMap(i)); // 设置地图服务
if (i == 0)
{
mapPubContainer[i].mapMetadataPublisher_.publish(mapPubContainer[i].map_.map.info);
}
}
// 新建一个线程用来发布地图
map_publish_thread_ = new boost::thread(boost::bind(&HectorMappingRos::publishMapLoop, this, p_map_pub_period_));
map_to_odom_.setIdentity();
// 查找 base_link -> front_laser_link 的tf,循环5次以确保其能找到
int count = 5;
ros::Duration dur(0.5);
while (count-- != 0)
{
if (tf_.waitForTransform(p_base_frame_, p_scan_frame_, ros::Time(0), dur))
{
tf_.lookupTransform(p_base_frame_, p_scan_frame_, ros::Time(0), laserTransform_);
break;
}
else
{
ROS_WARN("lookupTransform laser frame into base_link timed out.");
}
}
}
2.3 回调函数
首先进行了雷达数据格式的转换. 先通过 laser_geometry 这个包将 LaserScan 转换成 PointCloud 格式,这个很简单,我之前的文章已经实现过了,这里懒的改了.
之后是将雷达数据从点云形式转换到Hector自己的雷达数据存储格式.
再之后就是进行扫描匹配与地图更新.
扫描匹配计算完成之后再发布里程计 topic 与 tf,tf将根据不同的配置决定是发布 map -> base_link 还是 map -> odom -> base_link.
想要看输出的里程计信息 topic 就必须要有odom坐标系.
这里将 map 与 odom 的坐标系重合了( 没有对 map_to_odom_ 再赋值 ) ,所以 base_link 在 map 与 odom 下的坐标是相同的.
在发布TF的时候,将TF的时间戳都设置成了雷达的时间戳,这块好像有问题,会导致在rviz里 fixed frame 设置成map时看不到hector发布的odom数据。
我也将时间戳改成了 ros::Time::now() ,改完了之后有时能看到有时看不到,还不清楚原因,所以先在rviz里将 fixed frame 设置成了odom.
/**
* 激光数据处理回调函数,将ros数据格式转换为算法中的格式,并转换成地图尺度,交由slamProcessor处理。
* 算法中所有的计算都是在地图尺度下进行。
*/
void HectorMappingRos::scanCallback(const sensor_msgs::LaserScan &scan)
{
start_time_ = std::chrono::steady_clock::now();
ros::WallTime startTime = ros::WallTime::now();
// 将 scan 转换成 点云格式
projector_.projectLaser(scan, laser_point_cloud_, 30.0);
Eigen::Vector3f startEstimate(Eigen::Vector3f::Zero());
// 将雷达数据的点云格式 更改成 hector 内部的数据格式
if (rosPointCloudToDataContainer(laser_point_cloud_, laserTransform_, laserScanContainer, slamProcessor->getScaleToMap()))
{
// 首先获取上一帧的位姿,作为初值
startEstimate = slamProcessor->getLastScanMatchPose();
// 进入扫描匹配与地图更新
slamProcessor->update(laserScanContainer, startEstimate);
}
end_time_ = std::chrono::steady_clock::now();
time_used_ = std::chrono::duration_cast<std::chrono::duration<double>>(end_time_ - start_time_);
std::cout << "数据转换与扫描匹配用时: " << time_used_.count() << " 秒。" << std::endl;
if (p_timing_output_)
{
ros::WallDuration duration = ros::WallTime::now() - startTime;
ROS_INFO("HectorSLAM Iter took: %f milliseconds", duration.toSec() * 1000.0f);
}
// 更新存储的位姿, 这里的位姿是 base_link 在 map 下的位姿
poseInfoContainer_.update(slamProcessor->getLastScanMatchPose(), slamProcessor->getLastScanMatchCovariance(), scan.header.stamp, p_map_frame_);
// 发布 odom topic
if (p_pub_odometry_)
{
nav_msgs::Odometry tmp;
tmp.pose = poseInfoContainer_.getPoseWithCovarianceStamped().pose;
tmp.header = poseInfoContainer_.getPoseWithCovarianceStamped().header;
tmp.child_frame_id = p_base_frame_;
odometryPublisher_.publish(tmp);
}
if (pub_map_to_baselink_tf_)
{
// pub map -> odom -> base_link tf
if (p_pub_map_odom_transform_)
{
tfB_->sendTransform(tf::StampedTransform(map_to_odom_, scan.header.stamp, p_map_frame_, p_odom_frame_));
tfB_->sendTransform(tf::StampedTransform(poseInfoContainer_.getTfTransform(), scan.header.stamp, p_odom_frame_, p_tf_map_scanmatch_transform_frame_name_));
}
// pub map -> base_link tf
else
{
tfB_->sendTransform(tf::StampedTransform(poseInfoContainer_.getTfTransform(), scan.header.stamp, p_map_frame_, p_tf_map_scanmatch_transform_frame_name_));
}
}
}
2.4 转换scan格式的函数
这里的 laserTransform 指的是从 base_link 到 雷达坐标系 间的坐标变换.
首先将base_link到雷达坐标系的坐标转换 乘以地图分辨率 设置为这帧数据的 origo.
之后对每个雷达数据进行距离判定,如果距离太近或者太远就跳过.
这里还进行了如果距离大于 雷达数据的最大使用距离 的判断,因为雷达数据太远时的数据跳动比较厉害,所以这里将 p_use_max_scan_range_ 之外的数据也过滤掉不进行使用.
之后就是将数据点进行坐标变换,通过左乘laserTransform得到在base_link下的正确的x y 值,因为只进行了一次左乘laserTransform,所以转换后的z坐标是不正确的,通过再次减去laserTransform才得到正确的z坐标.
最后将base_link下的xy乘以分辨率放入dataContainer中.
// 将点云数据转换成Hector中雷达数据的格式
bool HectorMappingRos::rosPointCloudToDataContainer(const sensor_msgs::PointCloud &pointCloud, const tf::StampedTransform &laserTransform, hectorslam::DataContainer &dataContainer, float scaleToMap)
{
size_t size = pointCloud.points.size();
dataContainer.clear();
tf::Vector3 laserPos(laserTransform.getOrigin());
// dataContainer.setOrigo(Eigen::Vector2f::Zero());
// 将base_link到雷达坐标系的坐标转换 乘以地图分辨率 当成这帧数据的 origo
dataContainer.setOrigo(Eigen::Vector2f(laserPos.x(), laserPos.y()) * scaleToMap);
for (size_t i = 0; i < size; ++i)
{
const geometry_msgs::Point32 &currPoint(pointCloud.points[i]);
float dist_sqr = currPoint.x * currPoint.x + currPoint.y * currPoint.y;
if ((dist_sqr > p_sqr_laser_min_dist_) && (dist_sqr < p_sqr_laser_max_dist_))
{
if ((currPoint.x < 0.0f) && (dist_sqr < 0.50f))
{
continue;
}
// 距离太远的点跳动太大,如果距离大于使用距离(20m),就跳过
if (dist_sqr > p_use_max_scan_range_ * p_use_max_scan_range_)
continue;
// 点的坐标左乘base_link->laser_link的tf 将得到该点在base_link下的 xy 坐标, 但是z坐标是不正确
tf::Vector3 pointPosBaseFrame(laserTransform * tf::Vector3(currPoint.x, currPoint.y, currPoint.z));
// 通过再减去 base_link->laser_link的tf的z的值,得到该点在base_link下正确的 z 坐标
float pointPosLaserFrameZ = pointPosBaseFrame.z() - laserPos.z();
if (pointPosLaserFrameZ > p_laser_z_min_value_ && pointPosLaserFrameZ < p_laser_z_max_value_)
{
// 将雷达数据的 x y 都乘地图的分辨率 0.05 再放入dataContainer中
dataContainer.add(Eigen::Vector2f(pointPosBaseFrame.x(), pointPosBaseFrame.y()) * scaleToMap);
}
}
}
return true;
}
2.5 update()
update() 函数位于 Creating-2D-laser-slam-from-scratch/lesson4/include/lesson4/hector_mapping/slam_main/HectorSlamProcessor.h中.
将scan_match与地图更新分成了2步进行.
scan_match是调用mapRep->matchData()函数,返回了当前scan在与地图进行扫描匹配后的地图坐标系下的位姿.
之后调用了updateByScan()进行了地图的更新,这块代码已经在上一篇文章中讲过了,所以这里就不再进行讲解了.
/**
* 对每一帧的激光数据进行处理
* @param dataContainer 激光数据存储容器,坐标已转换成地图尺度,为地图中激光系的坐标
* @param poseHintWorld 激光系在地图中的初始pose
* @param map_without_matching 是否进行匹配
*/
void update(const DataContainer &dataContainer, const Eigen::Vector3f &poseHintWorld, bool map_without_matching = false)
{
//std::cout << "\nph:\n" << poseHintWorld << "\n";
/** 1. 位姿匹配 **/
Eigen::Vector3f newPoseEstimateWorld;
if (!map_without_matching)
{
// 进行 scan to map 的地方
newPoseEstimateWorld = (mapRep->matchData(poseHintWorld, dataContainer, lastScanMatchCov));
}
else
{
newPoseEstimateWorld = poseHintWorld;
}
lastScanMatchPose = newPoseEstimateWorld;
/** 2.地图更新 **/
if (util::poseDifferenceLargerThan(newPoseEstimateWorld, lastMapUpdatePose, paramMinDistanceDiffForMapUpdate, paramMinAngleDiffForMapUpdate) || map_without_matching)
{ // 仅在位姿变化大于阈值 或者 map_without_matching为真 的时候进行地图更新
mapRep->updateByScan(dataContainer, newPoseEstimateWorld);
mapRep->onMapUpdated();
lastMapUpdatePose = newPoseEstimateWorld;
}
}
2.5.1 matchData()_1
第一个matchData() 位于 Creating-2D-laser-slam-from-scratch/lesson4/include/lesson4/hector_mapping/slam_main/MapRepMultiMap.h中.
假设一共3层多分辨率地图,这里首先使用 setFrom() 将雷达数据dataContainer进行长度的缩短,对每个原始数据都乘以 1/(2的n次方) , n是 2, 1, 0.
之后使用缩短后的雷达数据,与对应分辨率的地图进行扫描匹配.
也就是在粗分辨率地图上先进行扫描匹配,得出的结果 再传入 更精细分辨率地图上进行扫描匹配.
最终返回一个在map坐标系下的位姿.
/**
* 地图匹配,通过多分辨率地图求解当前激光帧的pose。
*/
virtual Eigen::Vector3f matchData(const Eigen::Vector3f &beginEstimateWorld, const DataContainer &dataContainer, Eigen::Matrix3f &covMatrix)
{
size_t size = mapContainer.size();
Eigen::Vector3f tmp(beginEstimateWorld);
/// coarse to fine 的pose求精过程,i层的求解结果作为i-1层的求解初始值。
for (int index = size - 1; index >= 0; --index)
{
//std::cout << " m " << i;
if (index == 0)
{
tmp = (mapContainer[index].matchData(tmp, dataContainer, covMatrix, 5)); /// 输入数据dataContainer 对应 mapContainer[0]
}
else
{
// 根据地图层数对原始激光数据坐标进行缩小,得到对应图层尺寸的激光数据
dataContainers[index - 1].setFrom(dataContainer, static_cast<float>(1.0 / pow(2.0, static_cast<double>(index))));
tmp = (mapContainer[index].matchData(tmp, dataContainers[index - 1], covMatrix, 3));
/// dataContainers[i]对应mapContainer[i+1]
}
}
return tmp;
}
2.5.2 matchData()_2
第二个machData() 位于 Creating-2D-laser-slam-from-scratch/lesson4/include/lesson4/hector_mapping/slam_main/MapProcContainer.h 中.
可以看到,这个函数中并没有额外的操作,只是将对应分辨率的地图也一起传入到第三个mathcData() 中.
/**
* 给定位姿初值,估计当前scan在当前图层中的位姿 ---- 位姿为世界系下的pose 、 dataContainer应与当前图层尺度匹配
* @param beginEstimateWorld 世界系下的位姿
* @param dataContainer 激光数据
* @param covMatrix scan-match的不确定性 -- 协方差矩阵
* @param maxIterations 最大的迭代次数
* @return
*/
Eigen::Vector3f matchData(const Eigen::Vector3f &beginEstimateWorld, const DataContainer &dataContainer, Eigen::Matrix3f &covMatrix, int maxIterations)
{
return scanMatcher->matchData(beginEstimateWorld, *gridMapUtil, dataContainer, covMatrix, maxIterations);
}
2.5.3 matchData()_3
第三个 matchData() 位于 Creating-2D-laser-slam-from-scratch/lesson4/include/lesson4/hector_mapping/matcher/ScanMatcher.h 中.
这回终于是真正进行扫描匹配的地方了.
首先说一下hector world坐标系与hector map坐标系
之前说的这个初始位姿一直都是world坐标系下的坐标,Hector中认为实际物理坐标表示成的坐标为world坐标,以左上角为原点的.
而hector map坐标就是用 world坐标再除以地图的分辨率0.05(也就是乘以20),再加上map坐标系的原点 得到的.也就是像素坐标.
这段代码先将给定的初始位姿转换成Hector的map坐标系下的像素坐标.
之后调用了 estimateTransformationLogLh() 进行计算位姿,之后又通过多次调用这个函数使结果更加准确.
求解的过程就在 estimateTransformationLogLh() 里.
这里的H矩阵是这个类的私有变量,代表hessian, 也就是要返回的协方差矩阵.
然后,将estimate从地图坐标系下转到world坐标系下,再返回.
/**
* 实际进行位姿估计的函数
* @param beginEstimateWorld 位姿初值
* @param gridMapUtil 网格地图工具,这里主要是用来做坐标变换
* @param dataContainer 激光数据
* @param covMatrix 协方差矩阵
* @param maxIterations 最大迭代次数
* @return
*/
Eigen::Vector3f matchData(const Eigen::Vector3f &beginEstimateWorld,
ConcreteOccGridMapUtil &gridMapUtil,
const DataContainer &dataContainer,
Eigen::Matrix3f &covMatrix, int maxIterations)
{
if (dataContainer.getSize() != 0)
{
// 1. 初始pose转换为地图尺度下的pose --仿射变换,包括位姿的尺度和偏移,旋转在 X Y 同尺度变化时保持不变
Eigen::Vector3f beginEstimateMap(gridMapUtil.getMapCoordsPose(beginEstimateWorld));
Eigen::Vector3f estimate(beginEstimateMap);
// 2. 第一次迭代
estimateTransformationLogLh(estimate, gridMapUtil, dataContainer);
int numIter = maxIterations;
/** 3. 多次迭代求解 **/
for (int i = 0; i < numIter; ++i)
{
estimateTransformationLogLh(estimate, gridMapUtil, dataContainer);
}
// 角度正则化
estimate[2] = util::normalize_angle(estimate[2]);
covMatrix = Eigen::Matrix3f::Zero();
//covMatrix.block<2,2>(0,0) = (H.block<2,2>(0,0).inverse());
//covMatrix.block<2,2>(0,0) = (H.block<2,2>(0,0));
// 使用Hessian矩阵近似协方差矩阵
covMatrix = H;
// 结果转换回物理坐标系下 -- 转换回实际尺度
return gridMapUtil.getWorldCoordsPose(estimate);
}
return beginEstimateWorld;
}
2.6 estimateTransformationLogLh()
这个函数也位于 Creating-2D-laser-slam-from-scratch/lesson4/include/lesson4/hector_mapping/matcher/ScanMatcher.h 中.
首先,这个函数通过getCompleteHessianDerivs() 计算出了协方差矩阵H , 以及 dTr向量.
之后,通过高斯牛顿法最终推倒出的求根公式: H的逆 乘以 dtr 求出 先验位姿与后验位姿间的增量. 也就是 X = H.inverse() * dTr --> H * X = dTr
最后再将这个增量加上之前的预测值,得到匹配之后的位姿.
/**
* 高斯牛顿估计位姿
* @param estimate 位姿初始值
* @param gridMapUtil 网格地图相关计算工具
* @param dataPoints 激光数据
* @return 提示是否有解 --- 貌似没用上
*/
bool estimateTransformationLogLh(Eigen::Vector3f &estimate,
ConcreteOccGridMapUtil &gridMapUtil,
const DataContainer &dataPoints)
{
/** 核心函数,计算H矩阵和dTr向量(b列向量)---- occGridMapUtil.h 中 **/
gridMapUtil.getCompleteHessianDerivs(estimate, dataPoints, H, dTr);
//std::cout << "\nH\n" << H << "\n";
//std::cout << "\ndTr\n" << dTr << "\n";
// 判断H是否可逆, 判断增量非0,避免无用计算
if ((H(0, 0) != 0.0f) && (H(1, 1) != 0.0f))
{
// 求解位姿增量
Eigen::Vector3f searchDir(H.inverse() * dTr);
// 角度增量不能太大
if (searchDir[2] > 0.2f)
{
searchDir[2] = 0.2f;
std::cout << "SearchDir angle change too large\n";
}
else if (searchDir[2] < -0.2f)
{
searchDir[2] = -0.2f;
std::cout << "SearchDir angle change too large\n";
}
/// 更新估计值 --- 结果在地图尺度下
updateEstimatedPose(estimate, searchDir);
return true;
}
return false;
}
void updateEstimatedPose(Eigen::Vector3f &estimate, const Eigen::Vector3f &change)
{
estimate += change;
}
由于篇幅限制,getCompleteHessianDerivs() 函数的讲解与hector的公式推倒将在下篇文章中进行说明
3 运行
3.1 运行
本篇文章对应的数据包, 请在我的公众号中回复 lesson1 获得,并将launch中的bag_filename更改成您实际的目录名。
通过如下命令运行本篇文章对应的程序
roslaunch lesson4 hector_slam.launch
因为hector中依赖了 laser_geometry 这个包,如果有同学启动失败,提示缺少这个包,手动安装一下就好了.
3.2 运行结果与分析
启动之后,会在rviz中显示出如下画面.
首先,要说明一点的是rviz中的 Fixed Frame 我设置成了odom,因为我map与odom是重合的,所以这块设置成odom也是可以的.
如果设置成map,odom_hector这块会显示不出来,会提示找不到 TF,这个原因我还不太清楚,猜测是发布map->odom的tf的时间戳设置成雷达的时间戳这块存在问题,但是我没有去深扣它.
Rviz的 Fixed Frame 的意义是:所有显示出来的信息,都要通过 TF 进行坐标变换,变换到Fixed Frame 下,如果在规定的时间内没有查到 TF 就会报错,显示不了信息.
所以我将Fixed Frame设置成odom后,显示的2个odometry插件就不再需要进行坐标变换,可以直接显示出来.
红色轨迹是小车上的编码器的结果,蓝色轨迹是hector_slam输出的里程计结果.
通过在建图的过程中对比雷达与地图的匹配,以及对比红色蓝色的轨迹,可以发现,hector计算出的里程计还是挺准确的,即使是回来的过程中,雷达与地图也是始终匹配上的.
但是,在回来的过程中,编码器产生的里程计与hector的里程计产生了较大的偏差.
通过最后小车停止时,雷达数据与地图依然能够很好的匹配这一现象,可以认为hector产生的里程计的准确程度大于小车自身的里程计.
至于走廊边缘那边界不清晰的问题,通过观看建图过程发现,主要是由于远距离的雷达点跳动产生的.
计算耗时
同时,终端会打印处如下信息,可以看到,执行一次回调的时间主要花费在了进行数据转换和扫描匹配上.如果时间打印的再详细些的话就会发现时间主要用在扫描匹配部分.
当运行时间长了之后,会发现进行扫描匹配的时间明显变长了.
数据转换与扫描匹配用时: 0.0942895 秒。
执行一次回调用时: 0.094357 秒。
数据转换与扫描匹配用时: 0.0934194 秒。
执行一次回调用时: 0.093491 秒。
# 一段时间之后
数据转换与扫描匹配用时: 0.119987 秒。
执行一次回调用时: 0.120077 秒。
数据转换与扫描匹配用时: 0.13254 秒。
执行一次回调用时: 0.133221 秒。
雷达是10hz的,处理一次回调就要花0.12秒了,这时就会在运算过程中丢弃雷达数据.**这个原因猜测是由于地图变大之后,与地图的匹配更加耗时导致的.**具体是哪一部分导致的还有待推敲。
我在代码里将多分辨率地图的层数设置成了3层,如果设置成2层的话也会减小时间消耗.但是匹配的效果就会变差.
hector里程计的频率
如果一直通过rostopic hz 来查看 雷达与odom_hector的频率的话,就会发现,到后期 odom_hector的频率已经从 10hz 下降到 7.9hz 了.
topic rate min_delta max_delta std_dev window
=================================================================
/laser_scan 9.986 0.09054 0.1107 0.003959 20
/map 0.5228 1.913 1.913 0.0 20
/odom_hector 10.2 0.08047 0.1209 0.01026 2
# 一段时间之后
topic rate min_delta max_delta std_dev window
=================================================================
/laser_scan 10.0 0.08059 0.1166 0.004054 1984
/map 0.5004 1.783 2.174 0.06475 1984
/odom_hector 7.979 0.08047 0.3229 0.02474 100
4 总结与Next
本篇文章对Hecot的代码进行了简单的重构,发布了map与odom的tf,实现了一个基于scan_to_map的里程计,并构建出了第一张较成功的栅格地图.
下一篇文章将对 Hector 论文中的公式进行手动推倒,以及相应的代码讲解.
再下一篇文章将使用里程计数据进行SLAM,将基于 Karto 进行相关代码的实现.
文章将在 公众号: 从零开始搭SLAM 进行同步更新,欢迎大家关注,可以在公众号中添加我的微信,进激光SLAM交流群,大家一起交流SLAM技术。
同时,也希望您将这个公众号推荐给您身边做激光SLAM的人们,大家共同进步。
如果您对我写的文章有什么建议,或者想要看哪方面功能如何实现的,请直接在公众号中回复,我可以收到,并将认真考虑您的建议。
开放原子开发者工作坊旨在鼓励更多人参与开源活动,与志同道合的开发者们相互交流开发经验、分享开发心得、获取前沿技术趋势。工作坊有多种形式的开发者活动,如meetup、训练营等,主打技术交流,干货满满,真诚地邀请各位开发者共同参与!
更多推荐
所有评论(0)