jiaoda_avp

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. 构建残差函数，优化估计位姿