阅读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_link
if (!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矩阵与b列向量,涉及坐标变换,使用网格工具 ---- 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 coordinates
Eigen::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 vector
Eigen::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 point
Eigen::Vector2i scanBeginMapi(scanBeginMapf[0] + 0.5f, scanBeginMapf[1] + 0.5f);
//Get number of valid beams in current scan
int numValidElems = dataContainer.getSize();
//std::cout << "\n maxD: " << maxDist << " num: " << numValidElems << "\n";
//Iterate over all valid laser beams
for (int i = 0; i < numValidElems; ++i)
{
//Get map coordinates of current beam endpoint
Eigen::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 endpoint
Eigen::Vector2i scanEndMapi(scanEndMapf.cast<int>());
//Update map using a bresenham variant for drawing a line from beam start to beam endpoint in map coordinates
if (scanBeginMapi != scanEndMapi)
{
updateLineBresenhami(scanBeginMapi, scanEndMapi); /// Bresenhami 算法计算两点连线经过的所有格子
}
}
//Tell the map that it has been updated
this->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 case
if ((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 case
if ((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 dominant
if(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 dominant
int 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;
}
}