LocalizationBuilder
PoseGraph* pose_graph_ros::NodeHandle node_handleodometry_sub_semantic_scan_sub_semantic_img_sub_intial_pose_sub_global_map_pub_markers_pub_timers_shared_ptr<LocalTrajectoryBuilder> local_trajectory_builder
调用ScanMatch
调用local_trajectory_build->AddOdometryData
获取时间同步数据调用isSynced
当前图像转语义点云调用convertImg2Cloud
队头submap点云数达最大値倍时，设置set_insertion_finished(true)
LinearBounds
int min_xint max_xint min_yint max_y
调用local_trajectory_build->AddSemanticScan
调用extrapolator->AddOdometry
Match1. 用旋转变换点云2. 调用GenerateRotatedScans
调用InsertIntoSubmap
Constraint
uint submap_iduint node_iddouble translation_weightdouble rotation_weightV3d relative_poseenum tag // 全局 or 局部约束
ActiveSubmaps
vector<shared_ptr<Submap>> submaps_ValueConversionTables conversion_tablesunique_ptr<ProbabilityGridRangeDataInserter> range_data_inserter
PoseGraphData
1. 判断extrapolator是否为空需要重置2. 递推当前时刻估计位姿3. 点云转至全局坐标，累加至accmulated_semantic_data4. 发布accmu点云，active_submap[0]点云5. 叠加4次帧，转点云至当前时刻坐标系6. 调用AddAccumulatedSemantics
Data
PointCloud filtered_semantic_dataV3d local_pose
PubSemanticMap
计算当前2d odom_pose调用addSensorData
MapLimits
double resolution_Eigen::V2d max_CellLimits cell_llimits_
GetCellIndex(V2f& point)GetCellCenter(Array2i cell_index)bool Contains(Array2i& cell_index)
调用pose_graph->AddNode
Run
RunOptimization
创建地图，轨迹发布器
PoseExtrapolator
deque<V3d> timed_pose_queue_deque<V3d> odom_pose_queuedeque<double> timestamp_queueV3d last_odometry_pose_
CellLimits
num_x_cellsnum_y_cells
LocalTrajectoryBuilder
ros::NodeHandle node_handleaccumulated_semantic_scan_pub_active_submaps_pointcloud_pub_estimated_path_pub_estimated_odom_pub_noise_path_pub_MotionFilter motion_filterunique_ptr<PoseExtrapolator> extrapolator_ActiveSubmaps active_submaps_RealTimeCorrelativeScanMatcher real_time_correlative_scan_matcher_CeresScanMatcher2D ceres_scan_matcher_CeresICP2D ceres_icp_matcher_
OptimizationProblem
TrajectoryNode
span style=\
MatchingResult
Vector3d local_posePointCloud semantic_dataunique_ptr<InsertionResult> insertion_result
调用active_submaps.InsertSemanticData
遍历submaps，将点云插入每个submap中
订阅odometry，image回调
调用motion_filter->IsSimilar
若submaps为空或最后一个submap点云数量达最大值，创建新的
ValueConversionTables
MotionFilter
V3d last_pose_uint total_ = 0
bool isSimilar(V3d& pose)
创建定时器
InternalSubmapData
shared_ptr<Submap> submapSubmapState state = SubmapState::kNoConstraintSearchset<uint> node_ids
发布all_submap
RealTimeCorrelativeScanMatcher
ros::NodeHandle node_handleimage_transport::Publisher grid_map_pub
Match()ScoreCandidates()
SearchParameters
int num_angular_perturbationdouble angular_perturbationdouble resolutionint num_scansvector<LinearBounds> linear_bounds
SearchParameters()ShrinkToFit()
读取配置参数
Submap
调用点云addSensorData
SubmapSpec2D
V3d global_pose
回调数据入缓存队列
GridMap
MapLimits limits_vector<uint16> correspondence_cost_cells_min_correspondence_cost = 0.1max_correspondence_cost = 0.9vector<int> update_indices_Eigen::AlignedBox2i known_cells_box_ValueConversionTables* conversion_tables_vector<float>* value_to_correspondence_cost_table_
GlobalTrajectoryBuilder
PoseGraph* pose_graph_deque<Odometry> odom_queue_syncdeque<CompressedImage> img_queue_syncCompressedImage current_imgOdometry current_odomnode_handle_odometry_sub_semantic_scan_sub_semantic_img_sub_lidar_odometry_sub_shared_ptr<LocalTrajectoryBuilder> local_trajectory_builder_
PoseGraph
Candidate2D
int scan_indexint x_index_offsetint y_index_offsetdouble xdouble ydouble orientationfloat score
operator<()operator>()
InsertionResult
shared_ptr<TrajectoryNode::Data> constant_datavector<shared_ptr<Submap>> insertion_submaps
NodeSpec2D
V3d local_pose_2dV3d global_pose_2d
MatchSemantic1. 构建残差函数，优化估计位姿
