cartographer中的load_trajectory sc

std::unique_ptr<transform::Rigid2d> LocalTrajectoryBuilder2D::ScanMatch(
                const common::Time time, const transform::Rigid2d &pose_prediction,
                const sensor::RangeData &gravity_aligned_range_data) {
            //std::cout<<"LocalTrajectoryBuilder2D::ScanMatch1" <<std::endl;
            std::shared_ptr<const Submap2D> matching_submap =
                    active_submaps_.submaps().front();
            // The online correlative scan matcher will refine the initial estimate for
            // the Ceres scan matcher.
            transform::Rigid2d initial_ceres_pose = pose_prediction;
            sensor::AdaptiveVoxelFilter adaptive_voxel_filter(
                    options_.adaptive_voxel_filter_options());
            //对激光雷达数据进行滤波 & 转换成点云数据  这里的点云数据是在平面机器人坐标系中
            const sensor::PointCloud filtered_gravity_aligned_point_cloud =
                    adaptive_voxel_filter.Filter(gravity_aligned_range_data.returns);
            // std::cout<<"LocalTrajectoryBuilder2D::ScanMatch2" <<std::endl;
            if (filtered_gravity_aligned_point_cloud.empty()) {
                return nullptr;
            }
            //3. 配置文件中是否需要用csm来优化ceres-scan-match的初始解
            if (options_.use_online_correlative_scan_matching()) {
                CHECK_EQ(options_.submaps_options().grid_options_2d().grid_type(),
                         proto::GridOptions2D_GridType_PROBABILITY_GRID);
                //通过csm和滤波器过后的2d平面的　激光雷达数据来进行位姿优化传入预测的初始位姿＼激光雷达数据＼栅格地图
                // 返回一个更好的值initial_ceres_pose
                double score = real_time_correlative_scan_matcher_.Match(
                        pose_prediction, filtered_gravity_aligned_point_cloud,
                        *static_cast<const ProbabilityGrid *>(matching_submap->grid()),
                        &initial_ceres_pose);
                //std::cout<<"LocalTrajectoryBuilder2D::ScanMatch23" <<std::endl;
                kFastCorrelativeScanMatcherScoreMetric->Observe(score);
            }
            auto pose_observation = common::make_unique<transform::Rigid2d>();
            //最终通过ceres_scan_match来得到最终的位姿，看来上一步的scanmatch是为了给ceres一个好的初始值
            ceres::Solver::Summary summary;
            ceres_scan_matcher_.Match(pose_prediction.translation(), initial_ceres_pose,
                                      filtered_gravity_aligned_point_cloud,
                                      *matching_submap->grid(), pose_observation.get(),
                                      &summary);
            //5.如果扫描获得位姿，则跟新当前时刻的状态,下面这几个Observer好像没干什么事
//可能是给开发者留的回调，也可能是以后再补充
            if (pose_observation) {
                kCeresScanMatcherCostMetric->Observe(summary.final_cost);
                double residual_distance =
                        (pose_observation->translation() - pose_prediction.translation())
                                .norm();
                kScanMatcherResidualDistanceMetric->Observe(residual_distance);
                double residual_angle = std::abs(pose_observation->rotation().angle() -
                                                 pose_prediction.rotation().angle());
                kScanMatcherResidualAngleMetric->Observe(residual_angle);
            }
            //std::cout<<"LocalTrajectoryBuilder2D::ScanMatch5" <<std::endl;
            return pose_observation;
        }