| 从零开始搭二维激光SLAM | 您所在的位置：网站首页 › W2321009054 › 从零开始搭二维激光SLAM |
从零开始搭二维激光SLAM
|
上篇文章讲解了如何在固定位置使用Hector构建单帧的栅格地图，以及知道了SLAM的本质就是将不同时刻的scan在正确的位置上写成栅格地图． 本篇文章将对 Hector 进行简单的重写，使得其代码更简单，更清晰． 这也是本系列教程第一次成功建出一张比较好的地图． 先放图，虽然有些瑕疵，但是整体还是不错的． 代码已经提交在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("odom_hector", 50); } tfB_ = new tf::TransformBroadcaster(); slamProcessor = new hectorslam::HectorSlamProcessor(static_cast(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 mapTopicStr.append("_" + boost::lexical_cast(i)); } std::string mapMetaTopicStr(mapTopicStr); mapMetaTopicStr.append("_metadata"); MapPublisherContainer &tmp = mapPubContainer[i]; tmp.mapPublisher_ = node_handle_.advertise(mapTopicStr, 1, true); tmp.mapMetadataPublisher_ = node_handle_.advertise(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(end_time_ - start_time_); std::cout 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 if ((currPoint.x // 将雷达数据的 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 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 // 根据地图层数对原始激光数据坐标进行缩小，得到对应图层尺寸的激光数据 dataContainers[index - 1].setFrom(dataContainer, static_cast(1.0 / pow(2.0, static_cast(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 H * X = dTr最后再将这个增量加上之前的预测值，得到匹配之后的位姿． /** * 高斯牛顿估计位姿 * @param estimate 位姿初始值 * @param gridMapUtil 网格地图相关计算工具 * @param dataPoints 激光数据 * @return 提示是否有解 －－－ 貌似没用上 */ bool estimateTransformationLogLh(Eigen::Vector3f &estimate, ConcreteOccGridMapUtil &gridMapUtil, const DataContainer &dataPoints) { /** 核心函数，计算H矩阵和dTr向量(ｂ列向量)---- occGridMapUtil.h 中 **/ gridMapUtil.getCompleteHessianDerivs(estimate, dataPoints, H, dTr); //std::cout searchDir[2] = 0.2f; std::cout 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计算出的里程计还是挺准确的，即使是回来的过程中，雷达与地图也是始终匹配上的． 但是，在回来的过程中，编码器产生的里程计与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的人们，大家共同进步。 如果您对我写的文章有什么建议，或者想要看哪方面功能如何实现的，请直接在公众号中回复，我可以收到，并将认真考虑您的建议。
|
话不多说，先说明一下对代码做了那些改变，然后再着重讲解一下 Hector是如何做scan match的．
红色轨迹是小车上的编码器的结果，蓝色轨迹是hector_slam输出的里程计结果．
【本文地址】