阅读Hector代码时的笔记，更多Hector内容请看

https://blog.csdn.net/tiancailx/article/details/112978031

1 入口

hector_mapping/src/HectorMappingRos.cpp

1.1 scanCallback()

/*** 激光数据处理回调函数，将ros数据格式转换为算法中的格式，并转换成地图尺度，交由slamProcessor处理。* 算法中所有的计算都是在地图尺度下进行。  ROS部分的内容根据实际需求添加删减* @param scan  激光的数据ROS消息包*/
void HectorMappingRos::scanCallback(const sensor_msgs::LaserScan& scan)
{if (hectorDrawings){hectorDrawings->setTime(scan.header.stamp);}ros::WallTime startTime = ros::WallTime::now();// 不使用tf，默认雷达的坐标系就是base_linkif (!p_use_tf_scan_transformation_) // 未使用tf{if (rosLaserScanToDataContainer(scan, laserScanContainer,slamProcessor->getScaleToMap())){slamProcessor->update(laserScanContainer,slamProcessor->getLastScanMatchPose());}}else{ros::Duration dur (0.5);if (tf_.waitForTransform(p_base_frame_,scan.header.frame_id, scan.header.stamp,dur)){tf::StampedTransform laserTransform;tf_.lookupTransform(p_base_frame_,scan.header.frame_id, scan.header.stamp, laserTransform);//projector_.transformLaserScanToPointCloud(p_base_frame_ ,scan, pointCloud,tf_);/** 将laser scan 转换成 laser frame下的点云消息 **/projector_.projectLaser(scan, laser_point_cloud_,30.0);if (scan_point_cloud_publisher_.getNumSubscribers() > 0){scan_point_cloud_publisher_.publish(laser_point_cloud_);}Eigen::Vector3f startEstimate(Eigen::Vector3f::Zero());if(rosPointCloudToDataContainer(laser_point_cloud_, laserTransform, laserScanContainer, slamProcessor->getScaleToMap())){if (initial_pose_set_){initial_pose_set_ = false;startEstimate = initial_pose_;}else if (p_use_tf_pose_start_estimate_){try{tf::StampedTransform stamped_pose;tf_.waitForTransform(p_map_frame_,p_base_frame_, scan.header.stamp, ros::Duration(0.5));tf_.lookupTransform(p_map_frame_, p_base_frame_,  scan.header.stamp, stamped_pose);tfScalar yaw, pitch, roll;stamped_pose.getBasis().getEulerYPR(yaw, pitch, roll);startEstimate = Eigen::Vector3f(stamped_pose.getOrigin().getX(),stamped_pose.getOrigin().getY(), yaw);}catch(tf::TransformException e){ROS_ERROR("Transform from %s to %s failed\n", p_map_frame_.c_str(), p_base_frame_.c_str());startEstimate = slamProcessor->getLastScanMatchPose();}}else{startEstimate = slamProcessor->getLastScanMatchPose();}if (p_map_with_known_poses_){slamProcessor->update(laserScanContainer, startEstimate, true);}else{// 进入扫描匹配与地图更新slamProcessor->update(laserScanContainer, startEstimate);}}}else{ROS_INFO("lookupTransform %s to %s timed out. Could not transform laser scan into base_frame.", p_base_frame_.c_str(), scan.header.frame_id.c_str());return;}}// ...
}

1.2 update()

hector_mapping/include/hector_slam_lib/slam_main/HectorSlamProcessor.h

    /*** 对每一帧的激光数据进行处理* @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 扫描匹配

2.1 matchData()

hector_mapping/include/hector_slam_lib/slam_main/MapRepMultiMap.h

    /*** 地图匹配，通过金字塔求解当前激光帧的pose。* @param beginEstimateWorld* @param dataContainer* @param covMatrix* @return*/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.2 matchData()

hector_mapping/include/hector_slam_lib/slam_main/MapProcContainer.h

  /*** 给定位姿初值，估计当前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.3 matchData()

hector_mapping/include/hector_slam_lib/matcher/ScanMatcher.h

    /*** 实际进行位姿估计的函数* @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 (drawInterface){drawInterface->setScale(0.05f);drawInterface->setColor(0.0f, 1.0f, 0.0f);drawInterface->drawArrow(beginEstimateWorld);Eigen::Vector3f beginEstimateMap(gridMapUtil.getMapCoordsPose(beginEstimateWorld));drawScan(beginEstimateMap, gridMapUtil, dataContainer);drawInterface->setColor(1.0, 0.0, 0.0);}if (dataContainer.getSize() != 0){/// 1. 初始pose转换为地图尺度下的pose --仿射变换,包括位姿的尺度和偏移，旋转在 X Y 同尺度变化时保持不变Eigen::Vector3f beginEstimateMap(gridMapUtil.getMapCoordsPose(beginEstimateWorld));Eigen::Vector3f estimate(beginEstimateMap);/// 2. 一次迭代estimateTransformationLogLh(estimate, gridMapUtil, dataContainer);//bool notConverged = estimateTransformationLogLh(estimate, gridMapUtil, dataContainer);//std::cout << "\n cond: " << cond << " det: " << determinant << "\n";int numIter = maxIterations;/** 3. 多次迭代求解 **/for (int i = 0; i < numIter; ++i){//std::cout << "\nest:\n" << estimate;estimateTransformationLogLh(estimate, gridMapUtil, dataContainer);//notConverged = estimateTransformationLogLh(estimate, gridMapUtil, dataContainer);if (drawInterface){float invNumIterf = 1.0f / static_cast<float>(numIter);drawInterface->setColor(static_cast<float>(i) * invNumIterf, 0.0f, 0.0f);drawInterface->drawArrow(gridMapUtil.getWorldCoordsPose(estimate));//drawInterface->drawArrow(Eigen::Vector3f(0.0f, static_cast<float>(i)*0.05, 0.0f));}if (debugInterface){debugInterface->addHessianMatrix(H);}}if (drawInterface){drawInterface->setColor(0.0, 0.0, 1.0);drawScan(estimate, gridMapUtil, dataContainer);}/// 角度正则化estimate[2] = util::normalize_angle(estimate[2]);covMatrix = Eigen::Matrix3f::Zero();///　使用Hessian矩阵近似协方差矩阵covMatrix = H;///　结果转换回物理坐标系下 -- 转换回实际尺度return gridMapUtil.getWorldCoordsPose(estimate);}return beginEstimateWorld;}

2.4 estimateTransformationLogLh()

    /***  高斯牛顿估计位姿* @param estimate      位姿初始值* @param gridMapUtil   网格地图相关计算工具* @param dataPoints    激光数据* @return  提示是否有解　－－－　貌似没用上*/bool estimateTransformationLogLh(Eigen::Vector3f &estimate, ConcreteOccGridMapUtil &gridMapUtil, const DataContainer &dataPoints){/** 计算H矩阵与ｂ列向量，涉及坐标变换，使用网格工具 ---- occGridMapUtil.h 中 **/gridMapUtil.getCompleteHessianDerivs(estimate, dataPoints, H, dTr);//std::cout << "\nH\n" << H  << "\n";//std::cout << "\ndTr\n" << dTr  << "\n";//　判断H是否可逆if ((H(0, 0) != 0.0f) && (H(1, 1) != 0.0f)){//H += Eigen::Matrix3f::Identity() * 1.0f;/// 求解位姿增量Eigen::Vector3f searchDir(H.inverse() * dTr);//std::cout << "\nsearchdir\n" << searchDir  << "\n";/// 角度增量不能太大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;}

2.5 getCompleteHessianDerivs()

hector_mapping/include/hector_slam_lib/map/OccGridMapUtil.h

    /*** 使用当前pose投影dataPoints到地图，计算出 H 矩阵 b列向量， 理论部分详见Hector论文： 《A Flexible and Scalable SLAM System with Full 3D Motion Estimation》.* @param pose    地图系上的位姿* @param dataPoints  已转换为地图尺度的激光点数据* @param H   需要计算的 H矩阵* @param dTr  需要计算的 b列向量*/void getCompleteHessianDerivs(const Eigen::Vector3f& pose, const DataContainer& dataPoints, Eigen::Matrix3f& H, Eigen::Vector3f& dTr){int size = dataPoints.getSize();/** 1. 获取变换矩阵，用于将激光点转换到地图系上。 **/Eigen::Affine2f transform(getTransformForState(pose));float sinRot = sin(pose[2]);float cosRot = cos(pose[2]);H = Eigen::Matrix3f::Zero();dTr = Eigen::Vector3f::Zero();for (int i = 0; i < size; ++i) {/** 地图尺度下的激光body系激光点坐标 **/const Eigen::Vector2f& currPoint (dataPoints.getVecEntry(i));/** transform * currPoint 为地图系下的激光点坐标 **/Eigen::Vector3f transformedPointData(interpMapValueWithDerivatives(transform * currPoint));/// 这里获取的是 transformedPointData[0]--网格插值得分   transformedPointData[1]--x方向的梯度  transformedPointData[2] -- y方向梯度float funVal = 1.0f - transformedPointData[0];dTr[0] += transformedPointData[1] * funVal;dTr[1] += transformedPointData[2] * funVal;float rotDeriv = ((-sinRot * currPoint.x() - cosRot * currPoint.y()) * transformedPointData[1] + (cosRot * currPoint.x() - sinRot * currPoint.y()) * transformedPointData[2]);dTr[2] += rotDeriv * funVal;H(0, 0) += util::sqr(transformedPointData[1]);H(1, 1) += util::sqr(transformedPointData[2]);H(2, 2) += util::sqr(rotDeriv);H(0, 1) += transformedPointData[1] * transformedPointData[2];H(0, 2) += transformedPointData[1] * rotDeriv;H(1, 2) += transformedPointData[2] * rotDeriv;}/// 上面就是按照公式计算H、b，具体公式见论文。/// 其中H是对称矩阵，只算上三角就行， 减少计算量。H(1, 0) = H(0, 1);H(2, 0) = H(0, 2);H(2, 1) = H(1, 2);}

3 地图更新

3.1 updateByScan()

hector_mapping/include/hector_slam_lib/slam_main/MapProcContainer.h

  /*** 有Scan数据更新地图* @param dataContainer   当前scan激光数据* @param robotPoseWorld  当前scan世界系下位姿*/void updateByScan(const DataContainer &dataContainer, const Eigen::Vector3f &robotPoseWorld){if (mapMutex){mapMutex->lockMap();} //加锁，禁止其他线程竞争地图资源/// 更新地图gridMap->updateByScan(dataContainer, robotPoseWorld);if (mapMutex){mapMutex->unlockMap();} //地图解锁}

3.2 updateByScan()

hector_mapping/include/hector_slam_lib/map/OccGridMapBase.h

    /*** 地图更新函数！！！ 使用scan的位姿和激光数据更新地图。英文注释已经很清楚了==* Updates the map using the given scan data and robot pose* @param dataContainer Contains the laser scan data* @param robotPoseWorld The 2D robot pose in world coordinates*/void updateByScan(const DataContainer& dataContainer, const Eigen::Vector3f& robotPoseWorld){currMarkFreeIndex = currUpdateIndex + 1;currMarkOccIndex = currUpdateIndex + 2;//Get pose in map coordinates from pose in world coordinatesEigen::Vector3f mapPose(this->getMapCoordsPose(robotPoseWorld));//Get a 2D homogenous transform that can be left-multiplied to a robot coordinates vector to get world coordinates of that vectorEigen::Affine2f poseTransform((Eigen::Translation2f(mapPose[0], mapPose[1]) * Eigen::Rotation2Df(mapPose[2])));///实际上就是变换矩阵//Get start point of all laser beams in map coordinates (same for all beams, stored in robot coords in dataContainer)Eigen::Vector2f scanBeginMapf(poseTransform * dataContainer.getOrigo());//Get integer vector of laser beams start pointEigen::Vector2i scanBeginMapi(scanBeginMapf[0] + 0.5f, scanBeginMapf[1] + 0.5f);//Get number of valid beams in current scanint numValidElems = dataContainer.getSize();//std::cout << "\n maxD: " << maxDist << " num: " << numValidElems << "\n";//Iterate over all valid laser beamsfor (int i = 0; i < numValidElems; ++i){//Get map coordinates of current beam endpointEigen::Vector2f scanEndMapf(poseTransform * (dataContainer.getVecEntry(i)));//std::cout << "\ns\n" << scanEndMapf << "\n";//add 0.5 to beam endpoint vector for following integer cast (to round, not truncate)scanEndMapf.array() += (0.5f);//Get integer map coordinates of current beam endpointEigen::Vector2i scanEndMapi(scanEndMapf.cast<int>());//Update map using a bresenham variant for drawing a line from beam start to beam endpoint in map coordinatesif (scanBeginMapi != scanEndMapi){updateLineBresenhami(scanBeginMapi, scanEndMapi); /// Bresenhami 算法计算两点连线经过的所有格子}}//Tell the map that it has been updatedthis->setUpdated();/// 记录更新序号//Increase update index (used for updating grid cells only once per incoming scan)currUpdateIndex += 3; // currUpdateIndex 被吃掉了呗}

3.3 updateLineBresenhami()

    inline void updateLineBresenhami( const Eigen::Vector2i& beginMap, const Eigen::Vector2i& endMap, unsigned int max_length = UINT_MAX){int x0 = beginMap[0];int y0 = beginMap[1];//check if beam start point is inside map, cancel update if this is not the caseif ((x0 < 0) || (x0 >= this->getSizeX()) || (y0 < 0) || (y0 >= this->getSizeY())) {return;}int x1 = endMap[0];int y1 = endMap[1];//std::cout << " x: "<< x1 << " y: " << y1 << " length: " << length << "     ";//check if beam end point is inside map, cancel update if this is not the caseif ((x1 < 0) || (x1 >= this->getSizeX()) || (y1 < 0) || (y1 >= this->getSizeY())) {return;}int dx = x1 - x0;int dy = y1 - y0;unsigned int abs_dx = abs(dx);unsigned int abs_dy = abs(dy);int offset_dx = util::sign(dx);int offset_dy = util::sign(dy) * this->sizeX;unsigned int startOffset = beginMap.y() * this->sizeX + beginMap.x();//if x is dominantif(abs_dx >= abs_dy){ /// 激光束穿过的点更新一次Free状态int error_y = abs_dx / 2;bresenham2D(abs_dx, abs_dy, error_y, offset_dx, offset_dy, startOffset);}else{//otherwise y is dominantint error_x = abs_dy / 2;bresenham2D(abs_dy, abs_dx, error_x, offset_dy, offset_dx, startOffset);}unsigned int endOffset = endMap.y() * this->sizeX + endMap.x();this->bresenhamCellOcc(endOffset);/// 激光束端点更新一次Occupied状态}

3.4 bresenham2D()

    inline void bresenham2D( unsigned int abs_da, unsigned int abs_db, int error_b, int offset_a, int offset_b, unsigned int offset){this->bresenhamCellFree(offset);unsigned int end = abs_da-1;for(unsigned int i = 0; i < end; ++i){offset += offset_a;error_b += abs_db;if((unsigned int)error_b >= abs_da){offset += offset_b;error_b -= abs_da;}/// 激光束穿过的网格被更新一次free状态this->bresenhamCellFree(offset);}}

3.5 bresenhamCellFree()

    inline void bresenhamCellFree(unsigned int offset){ConcreteCellType& cell (this->getCell(offset));/// 单次scan中,每个网格只允许更新一次free状态（可能有多条激光束穿过同一个网格）if (cell.updateIndex < currMarkFreeIndex) {concreteGridFunctions.updateSetFree(cell);cell.updateIndex = currMarkFreeIndex;}}

3.6 bresenhamCellOcc()

    inline void bresenhamCellOcc(unsigned int offset){ConcreteCellType& cell (this->getCell(offset));/// 单次scan中，每个网格只允许更新一次occupied状态if (cell.updateIndex < currMarkOccIndex) {//if this cell has been updated as free in the current iteration, revert this/// 如果击中的网格在当前scan中曾被更新为free状态，那么撤销次free的更新，以occupied状态为准。if (cell.updateIndex == currMarkFreeIndex) {concreteGridFunctions.updateUnsetFree(cell);}concreteGridFunctions.updateSetOccupied(cell);//std::cout << " setOcc " << "\n";cell.updateIndex = currMarkOccIndex;}}

Hector代码笔记相关推荐

CSDN技术主题月----“深度学习”代码笔记专栏
from: CSDN技术主题月----"深度学习"代码笔记专栏 2016-09-13 nigelyq 技术专题 Hi,各位用户 CSDN技术主题月代码笔记专栏会每月在CODE博客为 ...
看完师兄的代码笔记，我失眠了
祝大家中秋节快乐! 最近很多公司的秋季招聘都已经启动了. 想必大家(尤其是经历过求职面试的)都知道,数据结构和算法在求职笔试/面试中的重要性. 纵观如今的互联网公司面试,清一色地都在重点考查这块,不开 ...
LSTM TF核心实现代码笔记
LSTM TF核心实现代码笔记 1. LSTM TF里的核心代码实现 2. 代码详细讲解 1. LSTM TF里的核心代码实现 LSTM网络的核心实现是在这个包里tensorflow/python.k ...
2018年最新Spring Boot视频教程附代码笔记资料（50G）
1. Spring Boot 项目实战 ----- 技术栈博客企业前后端链接:https://pan.baidu.com/s/1hueViq4 密码:4ma8 2.Spring Boot 项目实 ...
Python Text Processing with NLTK 2.0 Cookbook代码笔记
如下是<Python Text Processing with NLTK 2.0 Cookbook>一书部分章节的代码笔记. Tokenizing text into sentences ...
Transformer课程第8课NER案例代码笔记-IOB标记
Transformer课程第8课NER案例代码笔记-IOB标记 NER Tags and IOB Format 训练集和测试集都是包含餐厅相关文本(主要是评论和查询)的单个文件,其中每个单词都有一个 ...
2018尚硅谷SpringBoot视频教程附代码+笔记+课件(内含Docker）
尚硅谷SpringBoot视频教程(内含Docker)附代码+笔记+课件下载地址:百度网盘
吴恩达机器学习MATLAB代码笔记（1）梯度下降
吴恩达机器学习MATLAB代码笔记(1)梯度下降单变量线性回归 1.标记数据点(Plotting the Date) fprintf('Plotting Data') data = load('D: ...
Transformer课程第8课NER案例代码笔记-部署简介
Transformer课程第8课NER案例代码笔记 BERT微调器 NER是信息提取的子任务,旨在将非结构化文本中提到的命名实体定位并分类为预定义类别,如人名.组织.位置.医疗代码.时间表达式.数量 ...
【SFND_Lidar_Obstacle_Detection】代码笔记
源代码链接: https://github.com/williamhyin/SFND_Lidar_Obstacle_Detection 激光雷达数据: x,y,z,indensity(可用于评价物体的 ...

Hector代码笔记