cartographer LocalTrajectoryBuil

LocalTrajectoryBuilder2D::AddRangeData

        LocalTrajectoryBuilder2D::AddRangeData(
                const std::string &sensor_id,
                const sensor::TimedPointCloudData &unsynchronized_data) {
            //std::cout<<"LocalTrajectoryBuilder2D::AddRangeData" <<std::endl;
            //1.同步不同传感器的点云数据，得到 较好的点云数据   该点云数据的坐标为 tracking
            auto synchronized_data =
                    range_data_collator_.AddRangeData(sensor_id, unsynchronized_data);
            if (synchronized_data.ranges.empty()) {
                //范围数据整理器填充缓冲区
                LOG(INFO) << "Range data collator filling buffer.";
                return nullptr;
            }

            const common::Time &time = synchronized_data.time;
            // Initialize Rextrapolator now if we do not ever use an IMU. 不是用IMU时 初始化外推器
            if (!options_.use_imu_data()) {
                InitializeExtrapolator(time);
            }

            if (extrapolator_ == nullptr) {
                // Until we've initialized the extrapolator with our first IMU message, we
                // cannot compute the orientation of the rangefinder.
                LOG(INFO) << "Extrapolator not yet initialized.";
                return nullptr;
            }

            CHECK(!synchronized_data.ranges.empty());
            // TODO(gaschler): Check if this can strictly be 0.
            CHECK_LE(synchronized_data.ranges.back().point_time[3], 0.f);
            const common::Time time_first_point =
                    time +
                    common::FromSeconds(synchronized_data.ranges.front().point_time[3]);
            if (time_first_point < extrapolator_->GetLastPoseTime()) {
                LOG(INFO) << "Extrapolator is still initializing.";
                return nullptr;
            }

            if (num_accumulated_ == 0) {
                accumulation_started_ = std::chrono::steady_clock::now();
            }
//此处得到每个点云的pos是为了补偿在激光扫描360度（20Hz的话一圈就是50ms）的过程中机器人会产生运动，这会对激光点云产生影响，基于此pos可以消除扫描过程中运动对激光扫描的影响。
            std::vector<transform::Rigid3f> range_data_poses;
            range_data_poses.reserve(synchronized_data.ranges.size());
            bool warned = false;
            for (const auto &range : synchronized_data.ranges) {
                common::Time time_point = time + common::FromSeconds(range.point_time[3]);
                if (time_point < extrapolator_->GetLastExtrapolatedTime()) {
                    if (!warned) {
                        LOG(ERROR)
                                << "Timestamp of individual range data point jumps backwards from "
                                << extrapolator_->GetLastExtrapolatedTime() << " to " << time_point;
                        warned = true;
                    }
                    time_point = extrapolator_->GetLastExtrapolatedTime();
                }
                range_data_poses.push_back(
                        extrapolator_->ExtrapolatePose(time_point).cast<float>());
            }
            //计算local系下的激光点云坐标，并只保留（1，80）m间的点云数据，其他数据设为missing
            if (num_accumulated_ == 0) {
                // 'accumulated_range_data_.origin' is uninitialized until the last
                // accumulation.
                accumulated_range_data_ = sensor::RangeData{{},
                                                            {},
                                                            {}};
            }

            // Drop any returns below the minimum range and convert returns beyond the
            // maximum range into misses.
            for (size_t i = 0; i < synchronized_data.ranges.size(); ++i) {
//hit是估算的激光点打的障碍物在当前scan中的坐标
                const Eigen::Vector4f &hit = synchronized_data.ranges[i].point_time;
//origin_in_local是当前激光点的原点在当前scan中的坐标，
//hit_in_local和origin_in_local都左乘了range_data_poses因为激光雷达不知道自己中心也发生了运动
                const Eigen::Vector3f origin_in_local =
                        range_data_poses[i] *
                        synchronized_data.origins.at(synchronized_data.ranges[i].origin_index);
                const Eigen::Vector3f hit_in_local = range_data_poses[i] * hit.head<3>();
//delta是激光点打中的障碍物相对于激光点原点的坐标
                const Eigen::Vector3f delta = hit_in_local - origin_in_local;
//range是delta的模长，用来判断该激光点是否在min_range和max_range范围内
                const float range = delta.norm();
                if (range >= options_.min_range()) {
                    if (range <= options_.max_range()) {
                        accumulated_range_data_.returns.push_back(hit_in_local);
                    } else {
                        accumulated_range_data_.misses.push_back(
                                origin_in_local +
                                options_.missing_data_ray_length() / range * delta);
                    }
                }
            }
            ++num_accumulated_;

            if (num_accumulated_ >= options_.num_accumulated_range_data()) {
                num_accumulated_ = 0;
                const transform::Rigid3d gravity_alignment = transform::Rigid3d::Rotation(
                        extrapolator_->EstimateGravityOrientation(time));
                // TODO(gaschler): This assumes that 'range_data_poses.back()' is at time
                // 'time'.
                accumulated_range_data_.origin = range_data_poses.back().translation();
                return AddAccumulatedRangeData(
                        time,
                        TransformToGravityAlignedFrameAndFilter(
                                gravity_alignment.cast<float>() * range_data_poses.back().inverse(),
                                accumulated_range_data_),
                        gravity_alignment);
            }
            return nullptr;
        }