2D激光定位与建图

2D激光定位与建图

​ 关于自动驾驶与机器人中的SLAM技术第六章学习记录

1 扫描匹配

1.1 ICP2D

这部分基础暂时跳过

1.2 似然场

1.2.1 理论分析

来自高博书中：

  点到点的ICP 中，目标点与另一个点云中的最近邻之间计算了欧氏距离误差。可以想象成每个点与它的最近邻之间安装了一个弹簧。这些弹簧产生的拉力最终会把点云拉至能量最小的位置上。然而，在ICP 方法中，每迭代一次，我们就必须将这些弹簧重新安装一遍！

  换一种思路，提前对地图或者点云安装好这个“弹簧”，以节约耗时。这就引出“似然场法”。可以在地图中每一个点附近定义一个不断向外衰减的场。场存在一定的有效范围和分辨率。这个场可以随距离呈平方衰减，也可以是高斯衰减。当一个被测量点落在场附近时，我们可以用场的读数来作为该点的误差函数。

实际上：对于点云配准，如果两组点云能够很好匹配，那么在第一组点云附近形成一个距离场，第二组点云投影后的距离总和一定是最小的；而对于代价地图，判断是否是障碍物，那么就需要离障碍物越近，那么代价越大，故此衍生两种场。

总结：ICP和似然场本质都是最小化匹配点云距离，区别在于，似然场多了一个先验，投影过去直接就知道距离了，不用通过最近邻这一步来计算距离了！

场类型	值的物理意义	优化目标	数学形式	适用场景
高斯衰减场	点属于障碍物的概率（越近越大）	最大化	$e^{d^2/2\ast delt{a}^2}$	代价地图，导航等
距离场	点到点的距离（越近越小）	最小化	$d $或 $d^2$	ICP、点云配准

求解过程：已知目标函数（最小化距离总和），如何方程？如何求解这个方程？

① 构建目标函数：类似于ICP投影计算距离，似然场（世界系）无需计算，直接从模板中获取距离$\pi (p_i^W)$，然后最小化全部点云的距离即可

$$x^{\ast }=\arg \underset{x}{\min }\sum _{i=1}^n\Vert \pi (p_i^W){\Vert }_2^2,$$

② 计算雅可比矩阵：基于链式法则，先求解图像梯度，再求解似然场 ”图像“中 ”像素点$p_i^f$“ 对于投影点$p_i^W$的雅可比（分辨率转换方程），然后求投影点对姿态x的雅可比

$$\frac{\partial \pi }{\partial x}=\frac{\partial \pi }{\partial p_i^W}\frac{\partial p_i^W}{\partial x}=\frac{\partial \pi }{\partial p_i^f}\frac{\partial p_i^f}{\partial p_i^W}\frac{\partial p_i^W}{\partial x}$$

$$p_i^f=\alpha p_i^W+c,$$

$$\frac{\partial \pi }{\partial p_i^W}=\frac{\partial \pi }{\partial p_i^f}\frac{\partial p_i^f}{\partial p_i^W}=\alpha [\Delta {\pi }_x,\Delta {\pi }_y{]}^{\top }$$

$$\frac{\partial p_i^W}{\partial x}=\left[\begin{array}{cc}1& 0\\ 0& 1\\ -r_i\sin ({\rho }_i+\theta )& r_i\cos ({\rho }_i+\theta )\end{array}\right]\in {\mathbb{R}}^{3\times 2}$$

$$\frac{\partial \pi }{\partial x}=[\alpha \Delta {\pi }_x,\alpha \Delta {\pi }_y,-\alpha \Delta {\pi }_x{r}_i\sin ({\rho }_i+\theta )+\alpha \Delta {\pi }_y{r}_i\cos ({\rho }_i+\theta ){]}^{\top }.$$

还有一点，我们对一次scan或一个submap计算一个似然场，都是同样的过程，但更多为一个submap生成相应的似然场，用于地图间匹配，否则不是杀鸡用牛刀。

1.2.2 代码分析

① 距离场模板model_

距离场实际描述的是每一个像素与其最近的扫描点之间的距离函数。离一个点越远，其场却大，对应误差越大！

void LikelihoodField::BuildModel() {const int range = 20;  // 生成多少个像素的模板for (int x = -range; x <= range; ++x) {for (int y = -range; y <= range; ++y) {model_.emplace_back(x, y, std::sqrt((x * x) + (y * y)));		}}
}std::vector<ModelPoint> model_;				// x、y、场强residual_

② 迭代点，创建似然场field_

似然场（Likelihood Field）基于每一个点的距离场进行迭代更新（一帧点云 or 占据栅格地图）

如何处理多个点云的衰减范围重叠？当两个点 p1和 p2的衰减范围重叠时，同一位置可能被两个模板覆盖，导致值冲突。

代码里面重叠区域取了最小值-------因为保留最小值等价于记录最近障碍物的距离。----也就是说，取得是最近点的场！–满足目标函数构建最小距离。

-----------------------------------------为一帧点云创建似然场------------------------------------------------------field_ = cv::Mat(1000, 1000, CV_32F, 30.0);// p_f = α * P_w + c			其实栅格图、似然场图都可以看作图像，与真实物理世界存在一个内参K的变换（分辨率），类似于IPM图像那种
// 以0.05m/像素为分辨率，将激光点转换为像素坐标，原点为图像中心 
double real_angle = scan->angle_min + i * scan->angle_increment;
double x = scan->ranges[i] * std::cos(real_angle) * resolution_ + 500;
double y = scan->ranges[i] * std::sin(real_angle) * resolution_ + 500;for (auto& model_pt : model_) {				// 对当前点point(x, y)的场进行遍历int xx = int(x + model_pt.dx_);int yy = int(y + model_pt.dy_);if (xx >= 0 && xx < field_.cols && yy >= 0 && yy < field_.rows &&field_.at<float>(yy, xx) > model_pt.residual_) {		// field_即整个似然场field_.at<float>(yy, xx) = model_pt.residual_;			// 重叠区域取了最小值，说明记录最近的一个点的距离场！}
}---------------------------为栅格地图创建一个似然场，栅格中，处于占据状态的才是激光扫描点，所以只在哪里生成相应的似然场----------------
---------------------------实际上，每次到来一帧，就会更新栅格和似然场。这里似然场每次都是新建，是否可以保存原始，在此基础更新？---void LikelihoodField::SetFieldImageFromOccuMap(const cv::Mat& occu_map) {const int boarder = 25;field_ = cv::Mat(1000, 1000, CV_32F, 30.0);for (int x = boarder; x < occu_map.cols - boarder; ++x) {for (int y = boarder; y < occu_map.rows - boarder; ++y) {if (occu_map.at<uchar>(y, x) < 127) {       // 占据状态--生成似然场--因为本来匹配的就是激光点// 在该点生成一个modelfor (auto& model_pt : model_) {int xx = int(x + model_pt.dx_);int yy = int(y + model_pt.dy_);if (xx >= 0 && xx < field_.cols && yy >= 0 && yy < field_.rows &&field_.at<float>(yy, xx) > model_pt.residual_) {field_.at<float>(yy, xx) = model_pt.residual_;}}}}}
} 

③ 优化配准位姿

貌似之前用g2o时候，没有用过SE2这种sophus类型，也可能记错了

class VertexSE2 : public g2o::BaseVertex<3, SE2> {public:EIGEN_MAKE_ALIGNED_OPERATOR_NEWvoid setToOriginImpl() override { _estimate = SE2(); }void oplusImpl(const double* update) override {_estimate.translation()[0] += update[0];_estimate.translation()[1] += update[1];_estimate.so2() = _estimate.so2() * SO2::exp(update[2]);}bool read(std::istream& is) override { return true; }bool write(std::ostream& os) const override { return true; }
};

④ 可视化似然场

cv::Mat LikelihoodField::GetFieldImage() {cv::Mat image(field_.rows, field_.cols, CV_8UC3);for (int x = 0; x < field_.cols; ++x) {for (int y = 0; y < field_.rows; ++y) {         // 为了可视化，离点越远，距离越大，越亮，应该是白色，即255（初始化设置不会超过30，所以这里 30/30*255最大就是255）float r = field_.at<float>(y, x) * 255.0 / 30.0;image.at<cv::Vec3b>(y, x) = cv::Vec3b(uchar(r), uchar(r), uchar(r));}}return image;
}sad::Visualize2DScan(last_scan, SE2(), image, Vec3b(255, 0, 0));    // target是蓝的
sad::Visualize2DScan(current_scan, pose, image, Vec3b(0, 0, 255));  // source是红的

可以看出，越靠近点云，距离场越小，颜色越接近黑色，所以说很好理解，如果当前姿态下，匹配点云在这个距离场中

⑤ 多分辨率的似然场

用于位姿初值不太正确的场景，如回环检测；上面单分辨率通常都是用于运动过程连续帧的点云配准，因为姿态初值比较准确。

似然场图像分辨率resolution_ = {2.5, 5, 10, 20} pixel/m

似然场不同大小ratios_ = {0.125, 0.25, 0.5, 1.0};

同样的场景，真实距离不变，图像长宽变小，那么对应图像分辨率变大，或者说，一个像素对应的实际距离一定变大

1. 粗配准：低分辨率下，单栅格覆盖范围大，容忍初始误差，先快速匹配；
2. 精配准：将粗结果作为初值，在高分辨率场中优化细节。

优势：低层模糊但容错性强，高层精确，实现由粗到精的稳定匹配。

--------------------------------------初始化距离场模板、不同分辨率似然场---------------------------------------------------void MRLikelihoodField::BuildModel() {const int range = 20;  // 生成多少个像素的模板/// 生成模板金字塔图像--------------4钟分辨率的似然场field_ = {cv::Mat(125, 125, CV_32F, 30.0),cv::Mat(250, 250, CV_32F, 30.0),cv::Mat(500, 500, CV_32F, 30.0),cv::Mat(1000, 1000, CV_32F, 30.0),};for (int x = -range; x <= range; ++x) {for (int y = -range; y <= range; ++y) {model_.emplace_back(x, y, std::sqrt((x * x) + (y * y)));}}
}----------------------------------------迭代点更新似然场-------------------------------------------------if (occu_map.at<uchar>(y, x) < 127) {// 在该点生成一个model，在每个level中都填入for (int l = 0; l < levels_; ++l) {for (auto& model_pt : model_) {// 分辨率不同，坐标不同，坐标分别缩小ratios_ = {0.125, 0.25, 0.5, 1.0};int xx = int(x * ratios_[l] + model_pt.dx_);int yy = int(y * ratios_[l] + model_pt.dy_);if (xx >= 0 && xx < field_[l].cols && yy >= 0 && yy < field_[l].rows &&field_[l].at<float>(yy, xx) > model_pt.residual_) {field_[l].at<float>(yy, xx) = model_pt.residual_;}}}
}

2 占据栅格地图

2.1 栅格地图原理

  占据栅格地图应该满足：

1. 以栅格的形式存储每个格子被障碍物占据的概率。这个概率应该是0-1 之间的浮点数。
2. 栅格具有一定的分辨率，且通常是稠密的。
3. 占据概率会随着观测而发生改变。在数学上，占据概率的更新逻辑应该符合概率学要求。不过在工程上使用观测次数等更加简单明了的指标。

-------------------------------------每一个子地图会对应一个栅格地图，所以每次坐标要变换到子地图坐标系-----------------------------------occupancy_grid_ = cv::Mat(image_size_, image_size_, CV_8U, 127);-------------------------------------填充栅格某一个点------------------------------------------------------------void OccupancyMap::SetPoint(const Vec2i& pt, bool occupy) {int x = pt[0], y = pt[1];if (x < 0 || y < 0 || x >= occupancy_grid_.cols || y >= occupancy_grid_.rows) {if (occupy) {has_outside_pts_ = true;}return;}/// 这里设置了一个上下限----白色表示可以通行，黑色表示不可通行，初始127表示未知.// 如果占据状态，那么就减小，否则就增大。但这里会限制在117-137之间，不知道为什么uchar value = occupancy_grid_.at<uchar>(y, x);if (occupy) {if (value > 117) {occupancy_grid_.ptr<uchar>(y)[x] -= 1;}} else {if (value < 137) {occupancy_grid_.ptr<uchar>(y)[x] += 1;}}
}--------------------------------------------可视化这个栅格：127 未探索 0 障碍物 255 可通行-----------------------------------------cv::Mat OccupancyMap::GetOccupancyGridBlackWhite() const {cv::Mat image(image_size_, image_size_, CV_8UC3);for (int x = 0; x < occupancy_grid_.cols; ++x) {for (int y = 0; y < occupancy_grid_.rows; ++y) {if (occupancy_grid_.at<uchar>(y, x) == 127) {image.at<cv::Vec3b>(y, x) = cv::Vec3b(127, 127, 127);} else if (occupancy_grid_.at<uchar>(y, x) < 127) {image.at<cv::Vec3b>(y, x) = cv::Vec3b(0, 0, 0);} else if (occupancy_grid_.at<uchar>(y, x) > 127) {image.at<cv::Vec3b>(y, x) = cv::Vec3b(255, 255, 255);}}}return image;
}

栅格地图更新（”栅格化“）------通过下面两种方法进行更新

2.2 基于Bresenham 算法地图生成

void OccupancyMap::BresenhamFilling(const Vec2i& p1, const Vec2i& p2) {int dx = p2.x() - p1.x();int dy = p2.y() - p1.y();int ux = dx > 0 ? 1 : -1;       // p1是基准，p2是终点，所以p1用当前帧的坐标原点，遍历所有扫描点p2int uy = dy > 0 ? 1 : -1;dx = abs(dx);dy = abs(dy);int x = p1.x();int y = p1.y();if (dx > dy) {// 以x为增量int e = -dx;for (int i = 0; i < dx; ++i) {x += ux;e += 2 * dy;if (e >= 0) {y += uy;e -= 2 * dx;}if (Vec2i(x, y) != p2) {SetPoint(Vec2i(x, y), false);}}} else {int e = -dy;for (int i = 0; i < dy; ++i) {y += uy;e += 2 * dx;if (e >= 0) {x += ux;e -= 2 * dy;}if (Vec2i(x, y) != p2) {SetPoint(Vec2i(x, y), false);}}}
}

2.3 基于模板的地图生成

2.3.1 理论分析

  在长距离激光扫描（如100~200米）或需要高效更新的场景中，可以使用模板算法替代直线填充算法。模板预先计算每个栅格的角度和距离，更新时只需将模板中的距离与激光实测距离比较：

• 模板距离 < 激光距离 → 栅格标记为空闲（空白）。
• 模板距离 = 激光距离 → 栅格标记为占据。
• 模板距离 > 激光距离 → 栅格不更新。

  概率更新的工程优化：理论上，栅格应存储占据概率（0~1浮点数），但计算涉及对数运算（logit），效率较低。实际工程中可简化：

• 用8位灰度值（0~255）表示状态
  • 127（中值）：未知。
  • 观测到空闲 → 灰度值**+1**（上限255）。
  • 观测到占据 → 灰度值**-1**（下限0）。
• 优势：仅需整数加减，避免浮点运算，兼顾效率与统计意义。

2.3.2 代码分析

① 生成模板

void OccupancyMap::BuildModel() {for (int x = -model_size_; x <= model_size_; x++) {for (int y = -model_size_; y <= model_size_; y++) {Model2DPoint pt;    // 计算模板中的每个点----方形模板  pt.dx_ = x;pt.dy_ = y;         // 因为我们模板是图像，长度是像素长度，所以要转换到实际距离，毕竟作为激光测距的实际距离比较pt.range_ = sqrt(x * x + y * y) * inv_resolution_;      // 像素距离 * 0.05m/pixel=实际距离mpt.angle_ = std::atan2(y, x);pt.angle_ = pt.angle_ > M_PI ? pt.angle_ - 2 * M_PI : pt.angle_;  // limit in 2pimodel_.push_back(pt);}}
}

② 栅格化–更新栅格地图

这里angle = pt.angle_ - theta，不太明确2D激光数据的坐标系，扫描范围。这里代码应该是以某一个轴（+scan->angle_min）开始扫描的扇形区域

void OccupancyMap::AddLidarFrame(std::shared_ptr<Frame> frame, GridMethod method) {auto& scan = frame->scan_;// 此处不能直接使用frame->pose_submap_，因为frame可能来自上一个地图// 此时frame->pose_submap_还未更新，依旧是frame在上一个地图中的poseSE2 pose_in_submap = pose_.inverse() * frame->pose_;            // T_s_w * T_w_c = T_s_cfloat theta = pose_in_submap.so2().log();has_outside_pts_ = false;// 1. 计算激光雷达的末端点所在的网格std::set<Vec2i, less_vec<2>> endpoints;for (size_t i = 0; i < scan->ranges.size(); ++i) {if (scan->ranges[i] < scan->range_min || scan->ranges[i] > scan->range_max) {continue;}double real_angle = scan->angle_min + i * scan->angle_increment;double x = scan->ranges[i] * std::cos(real_angle);double y = scan->ranges[i] * std::sin(real_angle);// T_image_w * Twc * pc 获取栅格地图中的坐标端点endpoints------当前子地图对应的栅格地图endpoints.emplace(World2Image(frame->pose_ * Vec2d(x, y)));     // cur_scan--->word---->submap}// 2. 选择栅格化方法if (method == GridMethod::MODEL_POINTS) {// 3. 模板法（遍历模板，生成白色点）std::for_each(std::execution::par_unseq, model_.begin(), model_.end(), [&](const Model2DPoint& pt) {// 3.1 把当前帧的偏移量pose_.translation（激光传感器twc）变换到当前子地图的栅格地图中Vec2i pos_in_image = World2Image(frame->pose_.translation());   // 注意这里是  模板的点xy坐标(pt.dx_, pt.dy_)，所以无需坐标变换！// 以传感器为中心，直接加上模板点坐标即可Vec2i pw = pos_in_image + Vec2i(pt.dx_, pt.dy_);  // submap下   // 3.2 小距离内认为无物体if (pt.range_ < closest_th_) {SetPoint(pw, false);return;}// 3.3 pt.angle_是模板点角度，theta是当前帧在子地图下的角度 // 二维激光的扫描角度不是360°，也就是只有这一扫描范围内的模板是有效的，其它方向的模板都是无效的double angle = pt.angle_ - theta;  // 激光系下角度double range = FindRangeInAngle(angle, scan);if (range < scan->range_min || range > scan->range_max) {/// 某方向无测量值时，认为无效/// 但离机器比较近时，涂白-----------------------因为默认机器人旁边是可以通过的if (pt.range_ < endpoint_close_th_) {SetPoint(pw, false);}return;}// 3.4 末端点与车体连线上的点，涂白。非占据状态    激光测距range > 模板点pt.range_（非末端点） if (range > pt.range_ && endpoints.find(pw) == endpoints.end()) {SetPoint(pw, false);}});} else {.....}/// 5. 末端点涂黑----认为激光扫到的末端点是障碍物，表示占据状态std::for_each(endpoints.begin(), endpoints.end(), [this](const auto& pt) { SetPoint(pt, true); });
}

2.3.3 测试

简单的为每一帧点云创建一个栅格地图

("/pavo_scan_bottom", [&](Scan2d::Ptr scan) {sad::OccupancyMap oc_map;if (FLAGS_method == "model") {sad::evaluate_and_call([&]() {oc_map.AddLidarFrame(std::make_shared<sad::Frame>(scan),sad::OccupancyMap::GridMethod::MODEL_POINTS);},"Occupancy with model points");} else {sad::evaluate_and_call([&]() {oc_map.AddLidarFrame(std::make_shared<sad::Frame>(scan),sad::OccupancyMap::GridMethod::BRESENHAM);},"Occupancy with bresenham");}cv::imshow("occupancy map", oc_map.GetOccupancyGridBlackWhite());cv::waitKey(10);return true;})

3 子地图

3.1 理论

{W}：世界坐标系，{S}：子地图坐标系，{C}：当前激光扫描帧

$$T_{WC}=T_{WS}{T}_{SC}$$

这里将子地图的位姿$T_{WS}$分离，每一次求解的姿态$T_{SC}$实际上是以子地图为基准，$T_{SC}$计算后被固定，我们每次只需要更新子地图姿态$T_{WS}$就可以，不用去改动每一帧的姿态（每一帧的姿态可以通过上述关系求出，所以减少固定相对姿态$T_{SC}$）。

3.2 构建流程

1. 一个子地图对应一个似然场和一个栅格地图。
2. 我们总是把当前的扫描数据与当前的子地图进行匹配，得到该扫描数据在当前子地图中的位姿
3. 如果机器人发生移动或转动，我们按一定距离和角度来取关键帧。
4. 如果机器人移动范围超出了当前子地图，或者当前子地图包含的关键帧超出了一定数量，就建立一个新的子地图。新的子地图以当前帧为中心，它的位姿取为TWS = TWC 。此时新地图上面完全没有数据。为了方便后续配准，我们把旧的子地图里最近的一些关键帧拷贝至新的子地图中。
5. 把每个子地图的栅格地图合并起来后，就可以得到全局地图。

3.3 单个子地图流程

3.3.1 submap基本元素

一个子地图对应一个栅格地图occu_map_、一个似然场field_

field_通过栅格地图occu_map_生成，用于当前帧与子地图的匹配

class Submap {public:Submap(const SE2& pose) : pose_(pose) {occu_map_.SetPose(pose_);field_.SetPose(pose_);}private:SE2 pose_;  // submap的pose, Twssize_t id_ = 0;std::vector<std::shared_ptr<Frame>> frames_;  // 一个submap中的关键帧LikelihoodField field_;                       // 用于匹配OccupancyMap occu_map_;                       // 用于生成栅格地图
}------------------------设置姿态，pose即Tws，似然场貌似没啥用，但栅格地图会用来投影扫描帧点云，用以更新---------------------------------
void Submap::SetPose(const SE2& pose) {pose_ = pose;occu_map_.SetPose(pose);field_.SetPose(pose);
}

3.3.2 点云配准

基于当前的似然场优化当前帧姿态

bool Submap::MatchScan(std::shared_ptr<Frame> frame) {field_.SetSourceScan(frame->scan_);field_.AlignG2O(frame->pose_submap_);        // T_s_cframe->pose_ = pose_ * frame->pose_submap_;  // T_w_c = T_w_s * T_s_creturn true;
}

3.3.3 动态更新

① 系统接收一帧scan，优化完其姿态后，投影，更新栅格地图，栅格更新后，重新生成似然场（代码中每次都是新建一个mat）。

void Submap::AddScanInOccupancyMap(std::shared_ptr<Frame> frame) {occu_map_.AddLidarFrame(frame, OccupancyMap::GridMethod::MODEL_POINTS);  // 更新栅格地图中的格子field_.SetFieldImageFromOccuMap(occu_map_.GetOccupancyGrid());           // 更新场函数图像
}

② 如果说子地图位姿更新，重设每个frame的世界位姿

// 当子地图的位姿更新时，重设每个frame的世界位姿
void Submap::UpdateFramePoseWorld() {for (auto& frame : frames_) {frame->pose_ = pose_ * frame->pose_submap_;     // T_w_c = T_w_s * T_s_c}
}

3.3.4 检查是否存在落于子地图以外的点

说明是时候创建一个新的子地图了------------可以对比VSN中代码，创建sub的时机、布局位置

bool Submap::HasOutsidePoints() const { return occu_map_.HasOutsidePoints(); }if (current_submap_->HasOutsidePoints() || (current_submap_->NumFrames()) > 50) {/// 走出了submap或者单个submap中的关键帧较多ExpandSubmap();			
}void Mapping2D::ExpandSubmap() {// 当前submap作为历史地图放入loop closingif (loop_closing_) {loop_closing_->AddFinishedSubmap(current_submap_);}// 将当前submap替换成新的auto last_submap = current_submap_;// debugcv::imwrite("./data/ch6/submap_" + std::to_string(last_submap->GetId()) + ".png",last_submap->GetOccuMap().GetOccupancyGridBlackWhite());current_submap_ = std::make_shared<Submap>(current_frame_->pose_);      // Twc作为新子地图的姿态current_frame_->pose_submap_ = SE2();  // 这个归零，既然Twc作为新子地图的姿态，那么Tsc就应该是单位矩阵，平移0current_submap_->SetId(++submap_id_);current_submap_->AddKeyFrame(current_frame_);current_submap_->SetOccuFromOtherSubmap(last_submap);  // 把上一帧的数据也放进来，不让一个submap显得太空current_submap_->AddScanInOccupancyMap(current_frame_);all_submaps_.emplace_back(current_submap_);if (loop_closing_) {loop_closing_->AddNewSubmap(current_submap_);}LOG(INFO) << "create submap " << current_submap_->GetId()<< " with pose: " << current_submap_->GetPose().translation().transpose() << ", "<< current_submap_->GetPose().so2().log();
}

3.4 全局地图

class Mapping2D {...bool first_scan_ = true;std::shared_ptr<Frame> current_frame_ = nullptr;std::shared_ptr<Frame> last_frame_ = nullptr;SE2 motion_guess_;std::shared_ptr<Frame> last_keyframe_ = nullptr;std::shared_ptr<Submap> current_submap_ = nullptr;std::vector<std::shared_ptr<Submap>> all_submaps_;		// 包含所有子地图...
}

主流程：点云配准---->是否关键帧----->添加到栅格、帧库、更新似然场------->是否新增子地图

bool Mapping2D::ProcessScan(Scan2d::Ptr scan) {current_frame_ = std::make_shared<Frame>(scan);current_frame_->id_ = frame_id_++;if (last_frame_) {// set pose from last frame// current_frame_->pose_ = last_frame_->pose_;current_frame_->pose_ = last_frame_->pose_ * motion_guess_;     // 估计Twccurrent_frame_->pose_submap_ = last_frame_->pose_submap_;       // 估计Tsc}// 利用scan matching来匹配地图if (!first_scan_) {// 第一帧无法匹配，直接加入到occupancy mapcurrent_submap_->MatchScan(current_frame_);}// current_submap_->AddScanInOccupancyMap(current_frame_);   // 第一帧默认关键帧，所以下面会把第一帧直接加入occupancy mapfirst_scan_ = false;bool is_kf = IsKeyFrame();if (is_kf) {       AddKeyFrame();current_submap_->AddScanInOccupancyMap(current_frame_);// 处理回环检测if (loop_closing_) {loop_closing_->AddNewFrame(current_frame_);}if (current_submap_->HasOutsidePoints() || (current_submap_->NumFrames()) > 50) {/// 走出了submap或者单个submap中的关键帧较多ExpandSubmap();}}...if (last_frame_) {          // 预测一个两帧间的相对运动motion_guess_ = last_frame_->pose_.inverse() * current_frame_->pose_;}last_frame_ = current_frame_;return true;
}