此篇文章仅仅为了梳理Sensor Fusion for Localization & Mapping ：第六章graph-optimization优化数据流。
源码参考：https://github.com/AlexGeControl/Sensor-Fusion。
一、节点：data_pretreat_flow
订阅数据：
1.点云数据
2.imu数据
3.速度数据/速度、角速度，话题 /kitti/oxts/gps/vel
4.imu、lidar tf link 转化矩阵
发布数据：
1.去畸变后点云数据 话题 cloud_topic
2.同步时间戳后imu数据
3.同步后gps位置、速度数据， 话题 /synced_pos_vel
4.雷达坐标系下，位姿、速度数据 /synced_gnss（相当于gnss/imu在lidar坐标系的里程计数据）
二、节点：front_end_node
订阅数据：
去畸变雷达数据 话题 cloud_topic
发布数据：
雷达估计位姿 话题 odom_topic
1.读数据
2.判断是否有数据
3.判断数据的有效性
4.执行
判断是否首次执行
首次执行，单位矩阵设置为初始位姿，
非首次执行：
（1）根据pcl库，移除无效点云
pcl::removeNaNFromPointCloud
参数：输入点云，输出有效点云，输出定义了frame:包含点云数据和点云位姿
注：
using POINT = pcl::PointXYZ
uing CLOUD = pcl::PointCloud<POINT>
uing CLOUD_PTR = CLOUD ::Ptr
CLOUD cloud 和 CLOUD_PTR cloud_ptr
则两者可以相互转化 cloud = *cloud_ptr
只是为了说明pcl::PointCloud<POINT>::Ptr 为指针类型。。。
（2）对点云数据进行滤波
CloudData::CLOUD_PTR filtered_cloud_ptr(new CloudData::CLOUD());
frame_filter_ptr_->Filter(current_frame_.cloud_data.cloud_ptr, filtered_cloud_ptr);
（3）建立局部地图
if (local_map_frames_.size() == 0) {
current_frame_.pose = init_pose_;
UpdateWithNewFrame(current_frame_);
cloud_pose = current_frame_.pose;
return true;
}
这一步的目的是为了把关键帧的点云保存下来，由于用的是共享指针，所以直接复制只是复制了一个指针而已，此时无论你放多少个关键帧在容器里，这些关键帧点云指针都是指向的同一个点云。
bool FrontEnd::UpdateWithNewFrame(const Frame& new_key_frame) {
Frame key_frame = new_key_frame;
key_frame.cloud_data.cloud_ptr.reset(new CloudData::CLOUD(*new_key_frame.cloud_data.cloud_ptr));
// keep only the latest local_frame_num_ frames:
local_map_frames_.push_back(key_frame);
while (local_map_frames_.size() > static_cast<size_t>(local_frame_num_)) {
local_map_frames_.pop_front();
}
// transform all local frame measurements to map frame
// to create local map:
local_map_ptr_.reset(new CloudData::CLOUD());
CloudData::CLOUD_PTR transformed_cloud_ptr(new CloudData::CLOUD());
//这一段相当于对点云的拼接，依次将局部地图local_map_frames_中点云拼接，形成local_map
/*
举例：将第一帧点云local_map_frames_.at(0)通过转换矩阵 local_map_frames_.at(0).pose
得到第二帧点云坐标系下点云transformed_cloud_ptr，然后将拼接相加得到新点云。
其中， local_map_frames_.at(0).pose是通过前后两帧点云求得的位姿转化矩阵(align)。
local_map_ptr_ 指向最终拼接完成的点云地图，即局部地图。
*/
for (size_t i = 0; i < local_map_frames_.size(); ++i) {
pcl::transformPointCloud(
*local_map_frames_.at(i).cloud_data.cloud_ptr,
*transformed_cloud_ptr,
local_map_frames_.at(i).pose
);
*local_map_ptr_ += *transformed_cloud_ptr;
}
// scan-to-map matching:
// set target as local map:
//local_map_frames_大小小于10帧，直接输入点云作为配准点云，大于10帧，滤波后点云(局部地图)作为配准点云
if (local_map_frames_.size() < 10) {
registration_ptr_->SetInputTarget(local_map_ptr_);
} else {
CloudData::CLOUD_PTR filtered_local_map_ptr(new CloudData::CLOUD());
local_map_filter_ptr_->Filter(local_map_ptr_, filtered_local_map_ptr);
registration_ptr_->SetInputTarget(filtered_local_map_ptr);
}
return true;
}
（4）点云匹配求位姿(NDT)
NDT：1.根据target cloud 划分为栅格，统计落在各栅格中的点，根据各栅格中的点，计算各栅格均值、协方差、构建高斯分布; 2.不断迭代将source cloud 旋转、平移到target cloud ，计算联合概率，直到联合概率最大，得到最终的R,t.(本质上还是一个凸优化问题)
使用正态分布变换NDT进行配准，基本流程：
1.setInputSource 设置需要配准的点云
2.setInputTarget 设置点云配准目标
3.align(output, init_pose) 参数：配准，计算输入点云与目标点云的之间的转换矩阵final_transformation_，
并同时对输入点云input_做转换，并输出到output中。
CloudData::CLOUD_PTR result_cloud_ptr(new CloudData::CLOUD());
registration_ptr_->ScanMatch(filtered_cloud_ptr, predict_pose, result_cloud_ptr, current_frame_.pose);
cloud_pose = current_frame_.pose;
（5）基于匀速模型更新点云预测
step_pose = last_pose.inverse() * current_frame_.pose;
predict_pose = current_frame_.pose * step_pose;
last_pose = current_frame_.pose;
predice_pose其实作为align点云配准的初始估计。提升点云配准的速度和精度。
（6）更新关键帧
按照前后两帧位姿相距大于设定距离key_frame_distance_取关键帧。
if (fabs(last_key_frame_pose(0,3) - current_frame_.pose(0,3)) +
fabs(last_key_frame_pose(1,3) - current_frame_.pose(1,3)) +
fabs(last_key_frame_pose(2,3) - current_frame_.pose(2,3)) > key_frame_distance_) {
UpdateWithNewFrame(current_frame_);
last_key_frame_pose = current_frame_.pose;
}
三、节点lio_back_end_node
订阅
1.去畸变点云数据
2.局部点云地图位姿(雷达里程计)
3.imu预积分位姿
4.轮速计预积分位姿
5.同步的gps全局位置
6.闭环关键帧位姿
发布
1.优化后局部点云位姿
2.关键帧位姿
3.优化后关键帧序列轨迹
1.读数据
2.增加回环位姿用于后端优化
（1）当检测到回环时，增加回环约束，调用AddPRVAGRelativePoseEdge接口。
InsertLoopClosurePose();
bool LIOBackEnd::InsertLoopPose(const LoopPose& loop_pose) {
if (!graph_optimizer_config_.use_loop_close)
return false;
// get vertex IDs:
const int vertex_index_i = loop_pose.index0;
const int vertex_index_j = loop_pose.index1;
// get relative pose measurement:
Eigen::Matrix4d relative_pose = loop_pose.pose.cast<double>();
// add constraint, lidar frontend / loop closure detection:
graph_optimizer_ptr_->AddPRVAGRelativePoseEdge(
vertex_index_i, vertex_index_j,
relative_pose, graph_optimizer_config_.close_loop_noise
);
// update loop closure count:
++counter_.loop_closure;
LOG(INFO) << "Add loop closure: " << loop_pose.index0 << "," << loop_pose.index1 << std::endl;
return true;
}
AddPRVAGRelativePoseEdge接口是封装的一个g2o双边接口，输入为顶点、回环相对位姿测量和信息矩阵。
AddPRVAGRelativePoseEdge具体实现：
a.初始化
初始化回环相对位姿边
g2o::EdgePRVAGRelativePose *edge(new g2o::EdgePRVAGRelativePose());
而不同的边又封装成类，如EdgePRVAGRelativePose，在类构造函数中实现，参数分别为errorDim, errorType, Vertex1Type, Vertex2Type。
class EdgePRVAGRelativePose : public g2o::BaseBinaryEdge<6, Vector6d, g2o::VertexPRVAG, g2o::VertexPRVAG> {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
static const int INDEX_P = 0;
static const int INDEX_R = 3;
EdgePRVAGRelativePose()
: g2o::BaseBinaryEdge<6, Vector6d, g2o::VertexPRVAG, g2o::VertexPRVAG>() {
}
virtual void computeError() override {
const g2o::VertexPRVAG* v0 = static_cast<const g2o::VertexPRVAG*>(_vertices[0]);
const g2o::VertexPRVAG* v1 = static_cast<const g2o::VertexPRVAG*>(_vertices[1]);
const Eigen::Vector3d &pos_i = v0->estimate().pos;
const Sophus::SO3d &ori_i = v0->estimate().ori;
const Eigen::Vector3d &pos_j = v1->estimate().pos;
const Sophus::SO3d &ori_j = v1->estimate().ori;
const Eigen::Vector3d &pos_ij = _measurement.block<3, 1>(INDEX_P, 0);
const Eigen::Vector3d &ori_ij = _measurement.block<3, 1>(INDEX_R, 0);
_error.block(INDEX_P, 0, 3, 1) = ori_i.inverse() * (pos_j - pos_i) - pos_ij;
_error.block(INDEX_R, 0, 3, 1) = (Sophus::SO3d::exp(ori_ij).inverse()*ori_i.inverse()*ori_j).log();
}
virtual void setMeasurement(const Vector6d& m) override {
_measurement = m;
}
virtual bool read(std::istream& is) override {
Vector6d v;
is >> v(INDEX_P + 0) >> v(INDEX_P + 1) >> v(INDEX_P + 2)
>> v(INDEX_R + 0) >> v(INDEX_R + 1) >> v(INDEX_R + 2);
setMeasurement(v);
for (int i = 0; i < information().rows(); ++i) {
for (int j = i; j < information().cols(); ++j) {
is >> information()(i, j);
// update cross-diagonal element:
if (i != j) {
information()(j, i) = information()(i, j);
}
}
}
return true;
}
virtual bool write(std::ostream& os) const override {
Vector6d v = _measurement;
os << v(INDEX_P + 0) << " " <<v(INDEX_P + 1) << " " << v(INDEX_P + 2) << " "
<< v(INDEX_R + 0) << " " <<v(INDEX_R + 1) << " " << v(INDEX_R + 2) << " ";
for (int i = 0; i < information().rows(); ++i)
for (int j = i; j < information().cols(); ++j)
os << " " << information()(i, j);
return os.good();
}
};
b.设置顶点
g2o::VertexPRVAG* vertex_i = dynamic_cast<g2o::VertexPRVAG*>(graph_ptr_->vertex(vertex_index_i));
g2o::VertexPRVAG* vertex_j = dynamic_cast<g2o::VertexPRVAG*>(graph_ptr_->vertex(vertex_index_j));
edge->vertices()[0] = vertex_i;
edge->vertices()[1] = vertex_j;
c.设置测量（用于完成边误差计算）
// b. set measurement
Vector6d measurement = Vector6d::Zero();
// b.1. position:
measurement.block<3, 1>(g2o::EdgePRVAGRelativePose::INDEX_P, 0) = relative_pose.block<3, 1>(0, 3);
// b.2. orientation, so3:
measurement.block<3, 1>(g2o::EdgePRVAGRelativePose::INDEX_R, 0) = Sophus::SO3d(
Eigen::Quaterniond(relative_pose.block<3, 3>(0, 0).cast<double>())
).log();
edge->setMeasurement(measurement);
d.设置信息矩阵
// c. set information matrix:
Eigen::MatrixXd information_matrix = CalculateSe3EdgeInformationMatrix(noise);
edge->setInformation(information_matrix);
e.设置鲁棒核函数
// d. set loss function:
if (need_robust_kernel_) {
AddRobustKernel(edge, robust_kernel_name_, robust_kernel_size_);
}
将上述信息用于g2o优化
graph_ptr_->addEdge
（2）更新回环计数
++counter_.loop_closure
3.若有点云数据、局部地图点云位姿、gnss、imu则，更新updateBackEnd()
（1）判断数据是否有效，通过时间戳判断
（2）进入后端优化
如果首次执行，则将雷达里程计在map frame 的位姿作为初始位姿（将雷达里程计在map frame 的位姿与惯导位姿对齐）
if (!odometry_inited) {
// the origin of lidar odometry frame in map frame as init pose:
odom_init_pose = current_gnss_pose_data_.pose * current_laser_odom_data_.pose.inverse();
odometry_inited = true;
}
（3）更新IMU预积分UpdateIMUPreIntegration();
UpdateIMUPreIntegration();
a.首次进行预积分进行初始化
重置imu状态
ResetState(init_imu_data);
将预积分状态量、方差、雅克比矩阵赋初值
b.更新状态UpdateState
aa.中值积分计算预积分状态量
bb.更新F、B矩阵，计算方差P
P = F*P*F + B*Q*B
cc.更新雅克比矩阵J
J = F*J
(4)更新轮速计预积分
UpdateOdoPreIntegration()
a.首次进行预积分进行初始化
重置imu状态，将预积分状态量、方差、雅克比矩阵赋初值
ResetState(init_imu_data);
b.更新状态UpdateState
aa.中值积分计算预积分状态量
bb.更新F、B矩阵，计算方差P
P = F*P*F + B*Q*B
cc.更新雅克比矩阵J
J = F*J
可以看出轮速计预积分和imu预积分一样，只是预积分状态量不一样，预积分对状态量雅克比矩阵不一样，其它完全一致。
（5）执行优化
输入参数：当前帧点云、当前帧点云位姿、当前帧gnss位置、当前帧imu数据
a.重置参数 ResetParam()
将变量标志has_new_key_frame_、has_new_optimized_置为false。
b.判断是否为新的关键帧MaybeNewKeyFrame()
MaybeNewKeyFrame()
aa.如果是第一帧：
初始化imu预积分和轮速计预积分。并将关键帧标志has_new_optimized_置为true。
if (key_frames_deque_.size() == 0) {
// 初始化IMU预积分
// init IMU pre-integrator:
if ( imu_pre_integrator_ptr_ ) {
imu_pre_integrator_ptr_->Init(imu_data);
}
// 初始化编码器预积分
// init odometer pre-integrator:
if ( odo_pre_integrator_ptr_ ) {
gnss_odom.GetVelocityData(velocity_data);
odo_pre_integrator_ptr_->Init(velocity_data);
}
last_laser_pose = laser_odom;
last_gnss_pose = gnss_odom;
has_new_key_frame_ = true;
}
bb.判断是否为新的关键帧
判断当前帧与上一帧是否足够远，如果大于设定距离，则说明当前帧是新的关键帧，则需要重置预积分(相当于判断当前帧为关键帧时做的预处理)。
重置预积分：传入参数当前帧imu数据，预积分变量。先更新imu预积分，将预积分结果赋值给预积分变量imu_pre_integration，再重置预积分更新相关变量，并将关键帧标志has_new_optimized_置为true。
从这里可以看出预积分不断进行，但每次新来一帧关键帧会重置预积分。
// whether the current scan is far away enough from last key frame:
if (fabs(laser_odom.pose(0,3) - last_laser_pose.pose(0,3)) +
fabs(laser_odom.pose(1,3) - last_laser_pose.pose(1,3)) +
fabs(laser_odom.pose(2,3) - last_laser_pose.pose(2,3)) > key_frame_distance_) {
// 如果是新的关键帧，则需要重置预积分器
if ( imu_pre_integrator_ptr_ ) {
imu_pre_integrator_ptr_->Reset(imu_data, imu_pre_integration_);
}
if ( odo_pre_integrator_ptr_ ) {
gnss_odom.GetVelocityData(velocity_data);
odo_pre_integrator_ptr_->Reset(velocity_data, odo_pre_integration_);
}
last_laser_pose = laser_odom;
last_gnss_pose = gnss_odom;
has_new_key_frame_ = true;
}
轮速计预积分和imu预积分处理方式一致。
cc.若标志has_new_optimized_为true，上一步判断当前帧为关键帧
aaa.将当前关键帧点云保存下来
// a. first write new key scan to disk:
std::string file_path = key_frames_path_ + "/key_frame_" + std::to_string(key_frames_deque_.size()) + ".pcd";
pcl::io::savePCDFileBinary(file_path, *cloud_data.cloud_ptr);
bbb.将当前关键帧下，雷达里程计、gnss分别赋值分别给key_frame_deque_, key_gnss_deque_。
current_key_gnss_.time = current_key_frame_.time = laser_odom.time;
current_key_gnss_.index = current_key_frame_.index = key_frames_deque_.size();
// b. create key frame index for lidar scan, relative pose measurement:
current_key_frame_.pose = laser_odom.pose;
// c. create key frame index for GNSS measurement, full LIO state:
current_key_gnss_.pose = gnss_odom.pose;
current_key_gnss_.vel.v = gnss_odom.vel.v;
current_key_gnss_.vel.w = gnss_odom.vel.w;
// add to cache for later evo evaluation:
key_frames_deque_.push_back(current_key_frame_);
key_gnss_deque_.push_back(current_key_gnss_);
c.如果MaybeNewKeyFrame()判断当前帧为新的关键帧，增加节点和边AddNodeAndEdge()
通过AddNodeAndEdge()增加节点和边，相当于添加不同测量的约束
aa.如果没有全局约束gnss，则将第一帧关键帧固定，若有gnss则不固定第一帧关键帧。固定第一帧关键帧表示，第一帧不做优化。
if (!graph_optimizer_config_.use_gnss && graph_optimizer_ptr_->GetNodeNum() == 0) {
graph_optimizer_ptr_->AddPRVAGNode(current_key_frame_, true);
} else {
graph_optimizer_ptr_->AddPRVAGNode(current_key_gnss_, false);
}
添加顶点，被封装为g2o增加节点接口AddPRVAGNode()
初始化
g2o::VertexPRVAG *vertex(new g2o::VertexPRVAG());
VertexPRVAG是继承g2o顶点BaseVertex<15, PRVAG>参数表示：stateDim, stateType
class VertexPRVAG : public g2o::BaseVertex<15, PRVAG> {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
virtual void setToOriginImpl() override {
_estimate = PRVAG();
}
virtual void oplusImpl(const double *update) override {
// 顶点更新
//
// TODO: do update
//
_estimate.pos += Eigen::Vector3d(
update[PRVAG::INDEX_POS + 0], update[PRVAG::INDEX_POS + 1], update[PRVAG::INDEX_POS + 2]
);
_estimate.ori = _estimate.ori * Sophus::SO3d::exp(
Eigen::Vector3d(
update[PRVAG::INDEX_ORI + 0], update[PRVAG::INDEX_ORI + 1], update[PRVAG::INDEX_ORI + 2]
)
);
_estimate.vel += Eigen::Vector3d(
update[PRVAG::INDEX_VEL + 0], update[PRVAG::INDEX_VEL + 1], update[PRVAG::INDEX_VEL + 2]
);
Eigen::Vector3d d_b_a_i(
update[PRVAG::INDEX_B_A + 0], update[PRVAG::INDEX_B_A + 1], update[PRVAG::INDEX_B_A + 2]
);
Eigen::Vector3d d_b_g_i(
update[PRVAG::INDEX_B_G + 0], update[PRVAG::INDEX_B_G + 1], update[PRVAG::INDEX_B_G + 2]
);
_estimate.b_a += d_b_a_i ;
_estimate.b_g += d_b_g_i;
updateDeltaBiases(d_b_a_i, d_b_g_i);
}
bool isUpdated(void) const {
return _is_updated; }
void updateDeltaBiases(
const Eigen::Vector3d &d_b_a_i,
const Eigen::Vector3d &d_b_g_i
) {
std::lock_guard<std::mutex> l(_m);
_is_updated = true;
_d_b_a_i += d_b_a_i;
_d_b_g_i += d_b_g_i;
}
void getDeltaBiases(Eigen::Vector3d &d_b_a_i, Eigen::Vector3d &d_b_g_i) {
std::lock_guard<std::mutex> l(_m);
d_b_a_i = _d_b_a_i;
d_b_g_i = _d_b_g_i;
_d_b_a_i = _d_b_g_i = Eigen::Vector3d::Zero();
_is_updated = false;
}
virtual bool read(std::istream &in) {
return true; }
virtual bool write(std::ostream &out) const {
return true; }
private:
std::mutex _m;
bool _is_updated = false;
Eigen::Vector3d _d_b_a_i = Eigen::Vector3d::Zero();
Eigen::Vector3d _d_b_g_i = Eigen::Vector3d::Zero();
};
设置顶点编号
因为之前已经添加过回环约束顶点
vertex->setId(graph_ptr_->vertices().sizes());
设置测量（用于完成边误差计算），此处可以在自定义边中完成setMeasure纯虚函数具体实现。
// b. set vertex state:
g2o::PRVAG measurement;
measurement.time = lio_key_frame.time;
measurement.pos = lio_key_frame.pose.block<3, 1>(0, 3).cast<double>();
measurement.ori = Sophus::SO3d(
Eigen::Quaterniond(lio_key_frame.pose.block<3, 3>(0, 0).cast<double>())
);
// transform linear velocity from body frame to navigation frame:
measurement.vel = measurement.ori * lio_key_frame.vel.v.cast<double>();
measurement.b_a = lio_key_frame.bias.accel.cast<double>();
measurement.b_g = lio_key_frame.bias.gyro.cast<double>();
vertex->setEstimate(measurement);
设置是否需要固定顶点
// for first vertex:
if (need_fix) {
vertex->setFixed(true);
}
添加顶点
graph_ptr_->addVertex(vertex);
bb.设置连接的顶点
// get num. of vertices:
const int N = graph_optimizer_ptr_->GetNodeNum();
// get vertex IDs:
const int vertex_index_i = N - 2;
const int vertex_index_j = N - 1;
cc.添加雷达里程计相对位姿约束
if ( N > 1 ) {
// get relative pose measurement:
Eigen::Matrix4d relative_pose = (last_key_frame_.pose.inverse() * current_key_frame_.pose).cast<double>();
// add constraint, lidar frontend / loop closure detection:
graph_optimizer_ptr_->AddPRVAGRelativePoseEdge(
vertex_index_i, vertex_index_j,
relative_pose, graph_optimizer_config_.odom_edge_noise
);
}
dd.添加gnss先验单边约束
if ( graph_optimizer_config_.use_gnss ) {
// get prior position measurement:
Eigen::Vector3d pos = current_key_gnss_.pose.block<3, 1>(0, 3).cast<double>();
// add constraint, GNSS position:
graph_optimizer_ptr_->AddPRVAGPriorPosEdge(
vertex_index_j,
pos, graph_optimizer_config_.gnss_noise
);
}
ee.添加imu预积分约束
if ( N > 1 && graph_optimizer_config_.use_imu_pre_integration ) {
// add constraint, IMU pre-integraion:
graph_optimizer_ptr_->AddPRVAGIMUPreIntegrationEdge(
vertex_index_i, vertex_index_j,
imu_pre_integration_
);
}
ff.添加轮式里程计约束
if ( N > 1 && graph_optimizer_config_.use_odo_pre_integration ) {
// add constraint, odo pre-integraion:
graph_optimizer_ptr_->AddPRVAGOdoPreIntegrationEdge(
vertex_index_i, vertex_index_j,
odo_pre_integration_
);
}
gg.保留当前关键帧，且计数
last_key_frame_ = current_key_frame_;
++counter_.key_frame;
d.执行优化判断 MaybeOptimized()
aa.执行优化条件：有足够的关键帧或者有足够的回环，则need_optimize置为true
当前设置关键帧数量为50帧，回环帧数量10帧。
bb.若need_optimize为真，则执行优化
graph_optimizer_ptr_->Optimize()
和前面设置顶点和边一样，调用优化也封装成接口Optimize()
bool G2oGraphOptimizer::Optimize() {
static int optimize_cnt = 0;
if(graph_ptr_->edges().size() < 1) {
return false;
}
TicToc optimize_time;
graph_ptr_->initializeOptimization();
graph_ptr_->computeInitialGuess();
graph_ptr_->computeActiveErrors();
graph_ptr_->setVerbose(false);
double chi2 = graph_ptr_->chi2();
int iterations = graph_ptr_->optimize(max_iterations_num_);
LOG(INFO) << std::endl
<< "------ Finish Iteration " << ++optimize_cnt << " of Backend Optimization -------" << std::endl
<< "\tNum. Vertices: " << graph_ptr_->vertices().size() << std::endl
<< "\tNum. Edges: " << graph_ptr_->edges().size() << std::endl
<< "\tNum. Iterations: " << iterations << "/" << max_iterations_num_ << std::endl
<< "\tTime Consumption: " << optimize_time.toc() << std::endl
<< "\tCost Change: " << chi2 << "--->" << graph_ptr_->chi2()
<< std::endl << std::endl;
return true;
}
cc.优化完成设置优化标志has_new_optimized_ 为true
has_new_optimized_ = true;
4.发布数据
（1）发布激光里程计
（2）有新关键帧时
a.发布当前点云数据
b.发布关键帧，包括关键帧位姿
c.发布gnss数据，包括惯导位姿、速度
（3）优化完成时
发布优化后关键帧、包括位姿
至此完成。