《自动驾驶与机器人中的SLAM技术》ch9：自动驾驶车辆的离线地图构建

1 点云建图的流程

2 前端实现

2.1 前端流程

2.2 前端结果

3 后端位姿图优化与异常值剔除

3.1 两阶段优化流程

3.2 优化结果

① 第一阶段优化结果

② 第二阶段优化结果

4 回环检测

4.1 回环检测流程

① 遍历第一阶段优化轨迹中的关键帧。

② 并发计算候选回环对是否成立

③ 记录所有成功经过筛选的回环候选对，并在第二阶段优化时读取回环检测的结果

4.2 回环检测结果

5 轨迹可视化

6 地图导出

7 地图分块

8 本章小结

完整的点云建图可以看成是一个 RTK、IMU、轮速计、激光的综合优化问题。大部分 L4 级别的自动驾驶任务都需要一张完整的、与 RTK 对准的点云地图来进行地图标注、高精定位等任务。

1 点云建图的流程

与在线 SLAM 系统不一样，地图构建系统完全可以工作在离线模式下。离线系统的一大好处是有很强的确定性。模块与模块之间也没有线程、资源上的调度问题，而在线系统往往要考虑线程间的等待关系。

1. 首先，我们从给定的 ROS 包出解出 IMU、RTK 和激光数据。由于数据集的差异性，并非所有的数据集都含有轮速信息（而且往往轮速信息的格式与车相关，各有不同），因此我们主要使用 IMU 和激光数据来组成 LIO 系统。这部分内容会直接使用第8章的 IESKF LIO 代码。
2. 大部分自动驾驶车辆还会携带 RTK 设备或者组合导航设备。这些设备能给出车辆在物理世界中的位置，但可能受到信号影响。我们在 LIO 系统中按照一定距离来收集点云关键帧，然后按照 RTK 或组合导航的位姿给每个关键帧赋一个 RTK 位姿，作为观测。本书主要以NCLT 数据集为例，而 NCLT 数据集的 RTK 信号属于单天线方案，仅有平移信息而不含位姿信息。
3. 接下来，我们使用 LIO 作为相邻帧运动观测，使用 RTK 位姿作为绝对坐标观测，优化整条轨迹并判定各关键帧的 RTK 有效性。这称为第一阶段优化。如果 RTK 正常，此时我们会得到一条和 RTK 大致符合的轨迹。然而在实际环境中，RTK 会存在许多无效观测，我们也需要在算法中加以判定。
4. 我们在上一步的基础上对地图进行回环检测。检测算法可以简单地使用基于位置（欧氏距离）的回环检测，并使用 NDT 或常见的配准算法计算它们的相对位姿。本例使用多分辨率的 NDT 匹配作为回环检测方法。
5. 最后，我们再把这些信息放到统一的位姿图中进行优化，消除累计误差带来的重影，同时也移除 RTK 失效区域。这称为第二阶段优化。在确定了位姿以后，我们就按照这些位姿来导出点云地图。为了方便地图加载与查看，我们还会对点云地图进行切片处理。处理完之后的点云就可以用来进行高精定位或者地图标注了。

int main(int argc, char** argv) {
    google::InitGoogleLogging(argv[0]);
    FLAGS_stderrthreshold = google::INFO;
    FLAGS_colorlogtostderr = true;
    google::ParseCommandLineFlags(&argc, &argv, true);

    LOG(INFO) << "testing frontend";
    sad::Frontend frontend(FLAGS_config_yaml);
    if (!frontend.Init()) {
        LOG(ERROR) << "failed to init frontend.";
        return -1;
    }

    frontend.Run();

    sad::Optimization opti(FLAGS_config_yaml);
    if (!opti.Init(1)) {
        LOG(ERROR) << "failed to init opti1.";
        return -1;
    }
    opti.Run();

    sad::LoopClosure lc(FLAGS_config_yaml);
    if (!lc.Init()) {
        LOG(ERROR) << "failed to init loop closure.";
        return -1;
    }
    lc.Run();

    sad::Optimization opti2(FLAGS_config_yaml);
    if (!opti2.Init(2)) {
        LOG(ERROR) << "failed to init opti2.";
        return -1;
    }
    opti2.Run();

    LOG(INFO) << "done.";
    return 0;
}

2 前端实现

前端代码运行后会得到每个关键帧对应的 LIO 位姿、RTK 位姿以及扫描到的点云 scan。数据都会被存储在 data/ch9/keyframes.txt

存储顺序为：id_、timestamp_、rtk_heading_valid_、rtk_valid_、rtk_inlier_、lidar_pose_、rtk_pose_、opti_pose_1_、opti_pose_2_

0 1357847247.39686394 0 1 1 -1.46139387880958999e-06 -4.83414494836381997e-05 -5.27170121118259501e-06 -0.00234364570459886113 -7.70544167884409974e-05 4.34271791134374974e-05 0.999997249746972239 0.00277232000311427923 0.00764066664148633015 -0.00289528565243002802 0 0 0 1 0 0 0 0 0 0 1 0 0 0 0 0 0 1 
1 1357847253.14514446 0 1 1 -0.0670883829896546102 0.126191846878685315 0.00866291020077865175 0.0426005749366538539 0.000238472378827936458 0.084511669858249247 0.995511382056358696 -0.000874104033612869979 0.102182349318781368 0.00650536902844988177 0 0 0 1 0 0 0 0 0 0 1 0 0 0 0 0 0 1 
2 1357847253.64538193 0 1 1 -0.00427907445498453259 0.0132525322323276649 0.000541220831102592155 0.0943471987625134206 0.0112016335156973779 0.157637486857287762 0.982915841885542818 -0.0758422918343611557 0.333011746302124156 0.0181280469104047465 0 0 0 1 0 0 0 0 0 0 1 0 0 0 0 0 0 1

2.1 前端流程

1. 将 ROS 包中的 RTK 数据提取出来，放在 RTK 消息队列中，按采集时间排序。
2. 用第一个有效的 RTK 数据作为地图原点，将其他 RTK 读数减去地图原点后，作为 RTK 的位置观测。
3. 用 IMU 和激光数据运行 IESKF LIO，得到当前 State (current_time_, R_, p_, v_, bg_, ba_)。然后根据当前 State 通过 Frontend::ExtractKeyFrame() 函数按照距离和角度阈值判断当前雷达 scan 是否为关键帧。如果是关键帧的话，通过插值获取当前 State 的 RTK 位姿。

时间同步部分：

2.2 前端结果

IMU 静止初始化结果：

I0113 22:19:50.345458 409430 static_imu_init.cc:86] mean acce: -0.133286 0.0166332 009.80364
I0113 22:19:50.345479 409430 static_imu_init.cc:109] IMU 初始化成功，初始化时间= 9.9985, bg = 00.00159973 0000.003007 -0.00239681, ba = 07.40016e-05 -9.23492e-06 0-0.00544309, gyro sq = 00.00067222 3.29092e-06 3.63085e-05, acce sq = 7.68385e-06 00.00197847 1.57072e-06, grav = 0000.13336 -0.0166424 00-9.80908, norm: 9.81
I0113 22:19:50.345501 409430 static_imu_init.cc:113] mean gyro: 00.00159973 0000.003007 -0.00239681 acce: 07.40016e-05 -9.23492e-06 0-0.00544309
imu try init true time:1357847247.28515291
I0113 22:19:50.345526 409430 lio_iekf.cc:149] IMU初始化成功

前端实现全流程代码：

void Frontend::Run() {
    sad::RosbagIO rosbag_io(bag_path_, DatasetType::NCLT);

    // 先提取RTK pose，注意NCLT只有平移部分
    rosbag_io
        .AddAutoRTKHandle([this](GNSSPtr gnss) {
            gnss_.emplace(gnss->unix_time_, gnss);
            return true;
        })
        .Go();
    rosbag_io.CleanProcessFunc();  // 不再需要处理RTK

    RemoveMapOrigin();

    // 再运行LIO
    rosbag_io
        .AddAutoPointCloudHandle([&](sensor_msgs::PointCloud2::Ptr cloud) -> bool {
            lio_->PCLCallBack(cloud);
            ExtractKeyFrame(lio_->GetCurrentState());
            return true;
        })
        .AddImuHandle([&](IMUPtr imu) {
            lio_->IMUCallBack(imu);
            return true;
        })
        .Go();
    lio_->Finish();

    // 保存运行结果
    SaveKeyframes();

    LOG(INFO) << "done.";
}

判断当前雷达 scan 是否为关键帧代码：

// 主代码
lio_->PCLCallBack(cloud);
ExtractKeyFrame(lio_->GetCurrentState());

// 当前 state 是否能提取到关键帧。如果提取到关键帧，插值获取当前 state 的 RTK 位姿
void Frontend::ExtractKeyFrame(const sad::NavStated& state) {
    if (last_kf_ == nullptr) {
        if (!lio_->GetCurrentScan()) {
            // LIO没完成初始化
            return;
        }
        // 第一个帧
        auto kf = std::make_shared<Keyframe>(state.timestamp_, kf_id_++, state.GetSE3(), lio_->GetCurrentScan());
        FindGPSPose(kf);
        kf->SaveAndUnloadScan("/home/wu/slam_in_autonomous_driving/data/ch9/");
        keyframes_.emplace(kf->id_, kf);
        last_kf_ = kf;
    } else {
        // 计算当前state与kf之间的相对运动阈值
        SE3 delta = last_kf_->lidar_pose_.inverse() * state.GetSE3();
        if (delta.translation().norm() > kf_dis_th_ || delta.so3().log().norm() > kf_ang_th_deg_ * math::kDEG2RAD) {
            auto kf = std::make_shared<Keyframe>(state.timestamp_, kf_id_++, state.GetSE3(), lio_->GetCurrentScan());
            FindGPSPose(kf);
            keyframes_.emplace(kf->id_, kf);
            kf->SaveAndUnloadScan("/home/wu/slam_in_autonomous_driving/data/ch9/");
            LOG(INFO) << "生成关键帧" << kf->id_;
            last_kf_ = kf;
        }
    }
}

RTK 位姿插值代码：

插值详见《自动驾驶与机器人中的SLAM技术》ch7：基于 ESKF 的松耦合 LIO 系统插值部分。

假设比例 $s$ 计算公式如下，其中 $t$ 为待插值的时刻， $t_0$ 为起始时刻， $t_s$ 为结束时刻：

$s_t=\frac{t - t_0}{t_s-t_0}$

旋转部分插值：四元数球面线性插值 (SLERP)，确保旋转路径在旋转空间中的弧长最短。

$q_t = q_{t_0}(q_{t_0}^{-1}q_{t_s})^{s_t}$

平移部分插值：平移向量线性插值 (LERP)

$p_t=(1-s_t)p_{t_0} + s_tp_{t_s}$

注意：这种去畸变的方法前提是滤波器本身有效。如果滤波器失效或位姿发散，去畸变算法也就随之发散了。

void Frontend::FindGPSPose(std::shared_ptr<Keyframe> kf) {
    SE3 pose;
    GNSSPtr match;
    // pose 为插值结果
    if (math::PoseInterp<GNSSPtr>(
            kf->timestamp_, gnss_, [](const GNSSPtr& gnss) -> SE3 { return gnss->utm_pose_; }, pose, match)) {
        kf->rtk_pose_ = pose;
        kf->rtk_valid_ = true;
    } else {
        kf->rtk_valid_ = false;
    }
}


/**
 * pose 插值算法
 * @tparam T    数据类型
 * @param query_time 查找时间
 * @param data  数据
 * @param take_pose_func 从数据中取pose的谓词
 * @param result 查询结果
 * @param best_match_iter 查找到的最近匹配
 *
 * NOTE 要求query_time必须在data最大时间和最小时间之间，不会外推
 * data的map按时间排序
 * @return 插值是否成功
 */
template <typename T>
bool PoseInterp(double query_time, const std::map<double, T>& data, const std::function<SE3(const T&)>& take_pose_func,
                SE3& result, T& best_match) {
    if (data.empty()) {
        LOG(INFO) << "data is empty";
        return false;
    }

    if (query_time > data.rbegin()->first) {
        LOG(INFO) << "query time is later than last, " << std::setprecision(18) << ", query: " << query_time
                  << ", end time: " << data.rbegin()->first;

        return false;
    }

    auto match_iter = data.begin();
    for (auto iter = data.begin(); iter != data.end(); ++iter) {
        auto next_iter = iter;
        next_iter++;

        if (iter->first < query_time && next_iter->first >= query_time) {
            match_iter = iter;
            break;
        }
    }

    auto match_iter_n = match_iter;
    match_iter_n++;
    assert(match_iter_n != data.end());

    double dt = match_iter_n->first - match_iter->first;
    double s = (query_time - match_iter->first) / dt;  // s=0 时为第一帧，s=1时为next

    SE3 pose_first = take_pose_func(match_iter->second);
    SE3 pose_next = take_pose_func(match_iter_n->second);
    result = {pose_first.unit_quaternion().slerp(s, pose_next.unit_quaternion()),
              pose_first.translation() * (1 - s) + pose_next.translation() * s};
    best_match = s < 0.5 ? match_iter->second : match_iter_n->second;
    return true;
}

3 后端位姿图优化与异常值剔除

后端图优化需要包含以下因子：

RTK 因子：3 维的单元边（称为绝对位姿约束边），连接到每个关键帧，即顶点。误差为GNSS 估计位姿（由雷达位姿转换而来）与 RTK 位姿的平移差。当 RTK 存在外参（RTK 不在车辆中心）时，它的平移观测需要经过转换之后才能正确作用于车体的位移。
雷达里程计因子：6 维的二元边（称为相对运动约束边），对每个关键帧，连接当前关键帧和后续 5 个关键帧。误差为两帧之间的相对运动差，即 $T_{w1}^{-1}T_{w2}$ 。
回环因子：和雷达里程计因子是同一种边，对所有的回环候选关键帧对添加相对运动约束边。

/**
 * 只有平移的GNSS
 * 此时需要提供RTK外参 TBG，才能正确施加约束
 */
class EdgeGNSSTransOnly : public g2o::BaseUnaryEdge<3, Vec3d, VertexPose> {
   public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

    /**
     * 指定位姿顶点、RTK观测 t_WG、外参TGB
     * @param v
     * @param obs
     */
    EdgeGNSSTransOnly(VertexPose* v, const Vec3d& obs, const SE3& TBG) : TBG_(TBG) {
        setVertex(0, v);
        setMeasurement(obs);
    }

    void computeError() override {
        VertexPose* v = (VertexPose*)_vertices[0];
        // RTK 读数为 T_WG
        _error = (v->estimate() * TBG_).translation() - _measurement;
    };

    // void linearizeOplus() override {
    //     VertexPose* v = (VertexPose*)_vertices[0];
    //     // jacobian 6x6
    //     _jacobianOplusXi.setZero();
    //     _jacobianOplusXi.block<3, 3>(0, 0) = (_measurement.so3().inverse() * v->estimate().so3()).jr_inv();  // dR/dR
    //     _jacobianOplusXi.block<3, 3>(3, 3) = Mat3d::Identity();                                              // dp/dp
    // }

    virtual bool read(std::istream& in) { return true; }
    virtual bool write(std::ostream& out) const { return true; }

   private:
    SE3 TBG_;
};


/**
 * 6 自由度相对运动
 * 误差的平移在前，角度在后
 * 观测：T12
 */
class EdgeRelativeMotion : public g2o::BaseBinaryEdge<6, SE3, VertexPose, VertexPose> {
   public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    EdgeRelativeMotion() = default;
    EdgeRelativeMotion(VertexPose* v1, VertexPose* v2, const SE3& obs) {
        setVertex(0, v1);
        setVertex(1, v2);
        setMeasurement(obs);
    }

    void computeError() override {
        VertexPose* v1 = (VertexPose*)_vertices[0];
        VertexPose* v2 = (VertexPose*)_vertices[1];
        SE3 T12 = v1->estimate().inverse() * v2->estimate();
        _error = (_measurement.inverse() * v1->estimate().inverse() * v2->estimate()).log();
    };

    virtual bool read(std::istream& is) override {
        double data[7];
        for (int i = 0; i < 7; i++) {
            is >> data[i];
        }
        Quatd q(data[6], data[3], data[4], data[5]);
        q.normalize();
        setMeasurement(SE3(q, Vec3d(data[0], data[1], data[2])));
        for (int i = 0; i < information().rows() && is.good(); i++) {
            for (int j = i; j < information().cols() && is.good(); j++) {
                is >> information()(i, j);
                if (i != j) information()(j, i) = information()(i, j);
            }
        }
        return true;
    }

    virtual bool write(std::ostream& os) const override {
        os << "EDGE_SE3:QUAT ";
        auto* v1 = static_cast<VertexPose*>(_vertices[0]);
        auto* v2 = static_cast<VertexPose*>(_vertices[1]);
        os << v1->id() << " " << v2->id() << " ";
        SE3 m = _measurement;
        Eigen::Quaterniond q = m.unit_quaternion();
        os << m.translation().transpose() << " ";
        os << q.coeffs()[0] << " " << q.coeffs()[1] << " " << q.coeffs()[2] << " " << q.coeffs()[3] << " ";

        // information matrix
        for (int i = 0; i < information().rows(); i++) {
            for (int j = i; j < information().cols(); j++) {
                os << information()(i, j) << " ";
            }
        }
        os << std::endl;
        return true;
    }

   private:
};

3.1 两阶段优化流程

优化分为两阶段，顺序为第一阶段优化 $\rightarrow$ 回环检测 $\rightarrow$ 第二阶段优化。在每阶段的优化中，又分别进行了两次优化。

1. 首先使用 Optimization::Init() 函数初始化优化问题，加载关键帧参数及配置参数。若为第二阶段，则加载筛选后的回环检测。随后进入 Optimization::Run() 函数进行优化处理。
2. 如果是单天线 RTK 方案（没有姿态信息），且处于第一阶段，就调用一次 ICP 将雷达位姿和 RTK 位姿对齐。
3. 建立优化问题，包括优化器的创建、顶点（第一阶段使用 LIO 位姿作为顶点位姿估计值，这也是第一阶段第一次优化前 lidar 误差为 0 的原因；第二阶段使用第一阶段优化后的位姿作为估计值）创建、RTK 边创建（①带有鲁棒核函数②第二阶段的信息矩阵添加了0.01的乘积因子，降低了权重）、LIO 边的创建（没有鲁棒核函数）。若为第二阶段，则进行回环检测边的创建（带有鲁棒核函数）。
4. 进行第一次优化，此时 RTK 边和回环检测边带有鲁棒核函数。
5. 遍历回环检测边和 RTK 边，通过阈值筛选异常边，将异常边的等级设置为 1（表示不优化）；对于非异常边，去掉其鲁棒核函数。
6. 进行第二次优化，得到某一阶段的最终优化结果。
7. 保存结果。第一阶段优化结果保存到 opti_pose_1_，第二阶段优化结果保存到 opti_pose_2_。

总体来说就是。使用

接下来是代码部分：

1. 首先使用 Optimization::Init() 函数初始化优化问题，加载关键帧参数及配置参数。若为第二阶段，则加载筛选后的回环检测。随后进入 Optimization::Run() 函数进行优化处理。

bool Optimization::Init(int stage) {
    stage_ = stage;

    if (!LoadKeyFrames("/home/wu/slam_in_autonomous_driving/data/ch9/keyframes.txt", keyframes_)) {
        LOG(ERROR) << "cannot load keyframes.txt";
        return false;
    }

    LOG(INFO) << "keyframes: " << keyframes_.size();

    // 读参数
    auto yaml = YAML::LoadFile(yaml_);
    rtk_outlier_th_ = yaml["rtk_outlier_th"].as<double>();
    lidar_continuous_num_ = yaml["lidar_continuous_num"].as<int>();
    rtk_has_rot_ = yaml["rtk_has_rot"].as<bool>();

    rtk_pos_noise_ = yaml["rtk_pos_noise"].as<double>();
    rtk_ang_noise_ = yaml["rtk_ang_noise"].as<double>() * math::kDEG2RAD;
    rtk_height_noise_ratio_ = yaml["rtk_height_noise_ratio"].as<double>();

    std::vector<double> rtk_ext_t = yaml["rtk_ext"]["t"].as<std::vector<double>>();
    TBG_ = SE3(SO3(), Vec3d(rtk_ext_t[0], rtk_ext_t[1], rtk_ext_t[2]));
    LOG(INFO) << "TBG = \n" << TBG_.matrix();

    // my 两阶段不同之处
    if (stage_ == 2) {
        LoadLoopCandidates();
    }
    return true;
}

2. 如果是单天线 RTK 方案（没有姿态信息），且处于第一阶段，就调用一次 ICP 将雷达位姿和 RTK 位姿对齐。

void Optimization::Run() {
    LOG(INFO) << "running optimization on stage " << stage_;
    // my 两阶段不同之处
    if (!rtk_has_rot_ && stage_ == 1) {
        InitialAlign();
    }

    BuildProblem();  // 建立问题

    SaveG2O("/home/wu/slam_in_autonomous_driving/data/ch9/before.g2o");
    LOG(INFO) << "RTK 误差：" << print_info(gnss_edge_, rtk_outlier_th_);
    LOG(INFO) << "RTK 平移误差：" << print_info(gnss_trans_edge_, rtk_outlier_th_);
    LOG(INFO) << "lidar 误差：" << print_info(lidar_edge_, 0);

    Solve();           // 带着RK求解一遍
    RemoveOutliers();  // 移除异常值
    Solve();           // 再求解一遍

    SaveG2O("/home/wu/slam_in_autonomous_driving/data/ch9/after.g2o");

    SaveResults();  // 保存结果
    LOG(INFO) << "done";
}

3. 建立优化问题，包括优化器的创建、顶点（第一阶段使用 LIO 位姿作为顶点位姿估计值，第二阶段使用第一阶段优化后的位姿作为估计值）创建、RTK 边创建（①带有鲁棒核函数②第二阶段的信息矩阵添加了0.01的乘积因子，降低了权重）、LIO 边的创建（没有鲁棒核函数）。若为第二阶段，则进行回环检测边的创建（带有鲁棒核函数）。

void Optimization::BuildProblem() {
    using BlockSolverType = g2o::BlockSolverX;
    using LinearSolverType = g2o::LinearSolverEigen<BlockSolverType::PoseMatrixType>;
    auto* solver = new g2o::OptimizationAlgorithmLevenberg(
        g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));

    optimizer_.setAlgorithm(solver);

    AddVertices();
    AddRTKEdges();
    AddLidarEdges();
    AddLoopEdges();
}

void Optimization::AddVertices() {
    for (auto& kfp : keyframes_) {
        auto kf = kfp.second;

        // make g2o vertex for this kf
        auto v = new VertexPose();
        v->setId(kf->id_);

        // my 两阶段不同之处
        if (stage_ == 1) {
            v->setEstimate(kf->lidar_pose_);
        } else {
            v->setEstimate(kf->opti_pose_1_);
        }

        optimizer_.addVertex(v);
        vertices_.emplace(kf->id_, v);
    }
    LOG(INFO) << "vertex: " << vertices_.size();
}

void Optimization::AddRTKEdges() {
    /// RTK 噪声设置
    Mat3d info_pos = Mat3d::Identity() * 1.0 / (rtk_pos_noise_ * rtk_pos_noise_);
    info_pos(2, 2) = 1.0 / (rtk_height_noise_ratio_ * rtk_pos_noise_ * rtk_height_noise_ratio_ * rtk_pos_noise_);
    Mat3d info_ang = Mat3d::Identity() * 1.0 / (rtk_ang_noise_ * rtk_ang_noise_);
    Mat6d info_all = Mat6d::Identity();
    info_all.block<3, 3>(0, 0) = info_ang;
    info_all.block<3, 3>(3, 3) = info_pos;
    // 信息矩阵：分配观测值权重
    LOG(INFO) << "Info of rtk trans: " << info_pos.diagonal().transpose();

    // my 两阶段不同之处
    if (stage_ == 2) {
        info_pos *= 0.01;
        info_all *= 0.01;
    }

    for (auto& kfp : keyframes_) {
        auto kf = kfp.second;
        if (!kf->rtk_valid_) {
            continue;
        }

        if (kf->rtk_heading_valid_) {
            auto edge = new EdgeGNSS(vertices_.at(kf->id_), kf->rtk_pose_);
            edge->setInformation(info_all);
            auto rk = new g2o::RobustKernelHuber();
            rk->setDelta(rtk_outlier_th_);
            edge->setRobustKernel(rk);
            optimizer_.addEdge(edge);
            gnss_edge_.emplace_back(edge);
        } else {
            auto edge = new EdgeGNSSTransOnly(vertices_.at(kf->id_), kf->rtk_pose_.translation(), TBG_);
            edge->setInformation(info_pos);
            auto rk = new g2o::RobustKernelCauchy();
            rk->setDelta(rtk_outlier_th_);
            edge->setRobustKernel(rk);
            optimizer_.addEdge(edge);
            gnss_trans_edge_.emplace_back(edge);
        }
    }

    LOG(INFO) << "gnss edges: " << gnss_edge_.size() << ", " << gnss_trans_edge_.size();
}

void Optimization::AddLidarEdges() {
    const double lidar_pos_noise = 0.01, lidar_ang_noise = 0.1 * math::kDEG2RAD;  // RTK 观测的噪声
    Mat3d info_pos = Mat3d::Identity() * 1.0 / (lidar_pos_noise * lidar_pos_noise);
    Mat3d info_ang = Mat3d::Identity() * 1.0 / (lidar_ang_noise * lidar_ang_noise);
    Mat6d info_all = Mat6d::Identity();
    info_all.block<3, 3>(0, 0) = info_pos;
    info_all.block<3, 3>(3, 3) = info_ang;

    for (auto iter = keyframes_.begin(); iter != keyframes_.end(); ++iter) {
        auto iter_next = iter;
        for (int i = 0; i < lidar_continuous_num_; ++i) {
            iter_next++;

            if (iter_next == keyframes_.end()) {
                break;
            }

            // 添加iter和iter_next之间的相邻运动
            // 注意：这里观测用的是 lidar_pose_，而在第二阶段时 v->setEstimate(kf->opti_pose_1_) 添加的是 opti_pose_1_，所以 LIDAR 误差不为 0。
            auto edge = new EdgeRelativeMotion(vertices_.at(iter->second->id_), vertices_.at(iter_next->second->id_),
                                               iter->second->lidar_pose_.inverse() * iter_next->second->lidar_pose_);
            edge->setInformation(info_all);
            optimizer_.addEdge(edge);
            lidar_edge_.emplace_back(edge);
        }
    }

    LOG(INFO) << "lidar edges: " << lidar_edge_.size();
}

void Optimization::AddLoopEdges() {
    // my 两阶段不同之处
    if (stage_ == 1) {
        return;
    }

    const double loop_pos_noise = 0.1, loop_ang_noise = 0.5 * math::kDEG2RAD;  // RTK 观测的噪声
    Mat3d info_pos = Mat3d::Identity() * 1.0 / (loop_pos_noise * loop_pos_noise);
    Mat3d info_ang = Mat3d::Identity() * 1.0 / (loop_ang_noise * loop_ang_noise);
    Mat6d info_all = Mat6d::Identity();
    info_all.block<3, 3>(0, 0) = info_pos;
    info_all.block<3, 3>(3, 3) = info_ang;

    const double loop_rk_th = 5.2;

    for (const auto& lc : loop_candidates_) {
        auto edge = new EdgeRelativeMotion(vertices_.at(lc.idx1_), vertices_.at(lc.idx2_), lc.Tij_);
        edge->setInformation(info_all);
        auto rk = new g2o::RobustKernelCauchy();
        rk->setDelta(loop_rk_th);
        edge->setRobustKernel(rk);
        optimizer_.addEdge(edge);
        loop_edge_.emplace_back(edge);
    }
}

void Optimization::Solve() {
    optimizer_.setVerbose(true);
    optimizer_.initializeOptimization(0);
    optimizer_.optimize(100);

    LOG(INFO) << "RTK 误差：" << print_info(gnss_edge_, rtk_outlier_th_);
    LOG(INFO) << "RTK 平移误差：" << print_info(gnss_trans_edge_, rtk_outlier_th_);
    LOG(INFO) << "lidar 误差：" << print_info(lidar_edge_, 0);
    LOG(INFO) << "loop 误差：" << print_info(loop_edge_, 0);
}

4. 进行第一次优化，此时 RTK 边和回环检测边带有鲁棒核函数。

Solve();           // 带着RK求解一遍

5. 遍历回环检测边和 RTK 边，通过阈值筛选异常边，将异常边的等级设置为 1（表示不优化）；对于非异常边，去掉其鲁棒核函数。

void Optimization::RemoveOutliers() {
    // 主要用于移除GNSS的异常值
    int cnt_outlier_removed = 0; // ：统计当前移除的异常值数量。
    auto remove_outlier = [&cnt_outlier_removed](g2o::OptimizableGraph::Edge* e) {
        if (e->chi2() > e->robustKernel()->delta()) {
            // level 等级设置成 1，表示不优化（默认情况下，g2o 只处理 level=0 的边）
            e->setLevel(1);
            cnt_outlier_removed++;
        } else {
            // 剩下的边的误差均小于鲁棒核函数的默认值，也就不需要该函数了，所以设置鲁棒核函数为空（nullptr），这样优化过程中直接按照原二次误差处理。
            e->setRobustKernel(nullptr);
        }
    };

    // 移除GNSS的异常值
    std::for_each(gnss_edge_.begin(), gnss_edge_.end(), remove_outlier);
    std::for_each(gnss_trans_edge_.begin(), gnss_trans_edge_.end(), remove_outlier);
    LOG(INFO) << "gnss outlier: " << cnt_outlier_removed << "/" << gnss_edge_.size() + gnss_trans_edge_.size();

    // 移除回环检测的异常值
    cnt_outlier_removed = 0;
    std::for_each(loop_edge_.begin(), loop_edge_.end(), remove_outlier);
    LOG(INFO) << "loop outlier: " << cnt_outlier_removed << "/" << loop_edge_.size();
}

6. 进行第二次优化，得到某一阶段的最终优化结果。

Solve();           // 再求解一遍

7. 保存结果。第一阶段优化结果保存到 opti_pose_1_，第二阶段优化结果保存到 opti_pose_2_。

void Optimization::SaveResults() {
    for (auto& v : vertices_) {
        if (stage_ == 1) {
            keyframes_.at(v.first)->opti_pose_1_ = v.second->estimate();
        } else {
            keyframes_.at(v.first)->opti_pose_2_ = v.second->estimate();
        }
    }

    // 比较优化pose和rtk pose
    std::vector<double> rtk_trans_error;
    for (auto& kfp : keyframes_) {
        auto kf = kfp.second;
        Vec3d tWG = kf->rtk_pose_.translation();
        Vec3d t_opti = (kf->opti_pose_1_ * TBG_).translation();
        double n = (tWG - t_opti).head<2>().norm();
        rtk_trans_error.emplace_back(n);
    }

    std::sort(rtk_trans_error.begin(), rtk_trans_error.end());
    LOG(INFO) << "med error: " << rtk_trans_error[rtk_trans_error.size() / 2];

    // 写入文件
    system("rm /home/wu/slam_in_autonomous_driving/data/ch9/keyframes.txt");
    std::ofstream fout("/home/wu/slam_in_autonomous_driving/data/ch9/keyframes.txt");
    for (auto& kfp : keyframes_) {
        kfp.second->Save(fout);
    }
    fout.close();
}

3.2 优化结果

① 第一阶段优化结果

wu@WP:~/slam_in_autonomous_driving/bin$ ./run_optimization --stage=1

I0114 13:20:35.330343 421782 run_optimization.cc:23] testing optimization
I0114 13:20:35.335601 421782 keyframe.cc:78] Loaded kfs: 1143
I0114 13:20:35.335613 421782 optimization.cc:52] keyframes: 1143
I0114 13:20:35.346560 421782 optimization.cc:66] TBG = 
000001 000000 000000 000000
000000 000001 000000 000.24
000000 000000 000001 -0.283
000000 000000 000000 000001
I0114 13:20:35.346596 421782 optimization.cc:76] running optimization on stage 1
I0114 13:20:35.346643 421782 optimization.cc:335] p1: -215.594 -47.5573 -5.71106, p2: 218.772 17.5467 1.19319
I0114 13:20:35.346962 421782 optimization.cc:364] initial trans: 
0-0.989348 000.141275 0-0.035093 00-1.58926
0-0.141523 0-0.989924 0.00467225 000.768323
-0.0340794 0.00958895 000.999373 000.383868
0000000000 0000000000 0000000000 0000000001
I0114 13:20:35.347182 421782 optimization.cc:146] vertex: 1143
I0114 13:20:35.347186 421782 optimization.cc:158] Info of rtk trans: 000.25 000.25 0.0025
I0114 13:20:35.347465 421782 optimization.cc:191] gnss edges: 0, 1143
I0114 13:20:35.349598 421782 optimization.cc:221] lidar edges: 5700
I0114 13:20:35.402307 421782 optimization.cc:85] RTK 误差：
I0114 13:20:35.402323 421782 optimization.cc:86] RTK 平移误差：数量: 1143, 均值: 5.418336, 中位数: 0.338018, 0.1分位: 0.099467, 0.9分位: 1.155777, 0.95分位：29.841078, 最大值: 166.835762, 阈值: 1.000000
I0114 13:20:35.402400 421782 optimization.cc:87] lidar 误差：数量: 5700, 均值: 0.000000, 中位数: 0.000000, 0.1分位: 0.000000, 0.9分位: 0.000000, 0.95分位：0.000000, 最大值: 0.000000, 阈值: 0.000000
iteration= 0	 chi2= 645.444572	 time= 0.045737	 cumTime= 0.045737	 edges= 6843	 schur= 0	 lambda= 13.285494	 levenbergIter= 1
iteration= 1	 chi2= 640.432056	 time= 0.035216	 cumTime= 0.080953	 edges= 6843	 schur= 0	 lambda= 4.428498	 levenbergIter= 1
iteration= 2	 chi2= 627.241357	 time= 0.0353586	 cumTime= 0.116312	 edges= 6843	 schur= 0	 lambda= 1.476166	 levenbergIter= 1
iteration= 3	 chi2= 596.229379	 time= 0.0352488	 cumTime= 0.15156	 edges= 6843	 schur= 0	 lambda= 0.492055	 levenbergIter= 1
iteration= 4	 chi2= 541.185154	 time= 0.0350486	 cumTime= 0.186609	 edges= 6843	 schur= 0	 lambda= 0.164018	 levenbergIter= 1
iteration= 5	 chi2= 490.442325	 time= 0.0349324	 cumTime= 0.221541	 edges= 6843	 schur= 0	 lambda= 0.054673	 levenbergIter= 1
iteration= 6	 chi2= 475.227538	 time= 0.0350432	 cumTime= 0.256585	 edges= 6843	 schur= 0	 lambda= 0.018224	 levenbergIter= 1
iteration= 7	 chi2= 472.835370	 time= 0.0350574	 cumTime= 0.291642	 edges= 6843	 schur= 0	 lambda= 0.006075	 levenbergIter= 1
iteration= 8	 chi2= 470.709964	 time= 0.0349512	 cumTime= 0.326593	 edges= 6843	 schur= 0	 lambda= 0.002025	 levenbergIter= 1
iteration= 9	 chi2= 468.734946	 time= 0.0353598	 cumTime= 0.361953	 edges= 6843	 schur= 0	 lambda= 0.000675	 levenbergIter= 1
iteration= 10	 chi2= 468.114726	 time= 0.0350006	 cumTime= 0.396954	 edges= 6843	 schur= 0	 lambda= 0.000225	 levenbergIter= 1
iteration= 11	 chi2= 468.066097	 time= 0.0350694	 cumTime= 0.432023	 edges= 6843	 schur= 0	 lambda= 0.000075	 levenbergIter= 1
iteration= 12	 chi2= 468.065040	 time= 0.0349778	 cumTime= 0.467001	 edges= 6843	 schur= 0	 lambda= 0.000050	 levenbergIter= 1
iteration= 13	 chi2= 468.065030	 time= 0.0349623	 cumTime= 0.501963	 edges= 6843	 schur= 0	 lambda= 0.000033	 levenbergIter= 1
iteration= 14	 chi2= 468.065030	 time= 0.0553328	 cumTime= 0.557296	 edges= 6843	 schur= 0	 lambda= 0.728149	 levenbergIter= 6
iteration= 15	 chi2= 468.065030	 time= 0.0351333	 cumTime= 0.592429	 edges= 6843	 schur= 0	 lambda= 0.485432	 levenbergIter= 1
iteration= 16	 chi2= 468.065030	 time= 0.0351449	 cumTime= 0.627574	 edges= 6843	 schur= 0	 lambda= 0.323622	 levenbergIter= 1
iteration= 17	 chi2= 468.065029	 time= 0.0471594	 cumTime= 0.674733	 edges= 6843	 schur= 0	 lambda= 13.807856	 levenbergIter= 4
iteration= 18	 chi2= 468.065029	 time= 0.035124	 cumTime= 0.709857	 edges= 6843	 schur= 0	 lambda= 9.205237	 levenbergIter= 1
iteration= 19	 chi2= 468.065029	 time= 0.0351963	 cumTime= 0.745054	 edges= 6843	 schur= 0	 lambda= 6.136825	 levenbergIter= 1
iteration= 20	 chi2= 468.065029	 time= 0.0350088	 cumTime= 0.780063	 edges= 6843	 schur= 0	 lambda= 4.091217	 levenbergIter= 1
iteration= 21	 chi2= 468.065029	 time= 0.0350847	 cumTime= 0.815147	 edges= 6843	 schur= 0	 lambda= 2.727478	 levenbergIter= 1
iteration= 22	 chi2= 468.065029	 time= 0.0352051	 cumTime= 0.850352	 edges= 6843	 schur= 0	 lambda= 1.818318	 levenbergIter= 1
iteration= 23	 chi2= 468.065029	 time= 0.0349688	 cumTime= 0.885321	 edges= 6843	 schur= 0	 lambda= 1.212212	 levenbergIter= 1
iteration= 24	 chi2= 468.065029	 time= 0.035229	 cumTime= 0.92055	 edges= 6843	 schur= 0	 lambda= 0.808142	 levenbergIter= 1
iteration= 25	 chi2= 468.065029	 time= 0.0353063	 cumTime= 0.955856	 edges= 6843	 schur= 0	 lambda= 0.538761	 levenbergIter= 1
iteration= 26	 chi2= 468.065029	 time= 0.0350386	 cumTime= 0.990895	 edges= 6843	 schur= 0	 lambda= 0.359174	 levenbergIter= 1
iteration= 27	 chi2= 468.065029	 time= 0.0351134	 cumTime= 1.02601	 edges= 6843	 schur= 0	 lambda= 0.239449	 levenbergIter= 1
iteration= 28	 chi2= 468.065029	 time= 0.0351127	 cumTime= 1.06112	 edges= 6843	 schur= 0	 lambda= 0.159633	 levenbergIter= 1
iteration= 29	 chi2= 468.065029	 time= 0.034973	 cumTime= 1.09609	 edges= 6843	 schur= 0	 lambda= 0.106422	 levenbergIter= 1
iteration= 30	 chi2= 468.065029	 time= 0.0390458	 cumTime= 1.13514	 edges= 6843	 schur= 0	 lambda= 0.141896	 levenbergIter= 2
iteration= 31	 chi2= 468.065028	 time= 0.0350559	 cumTime= 1.1702	 edges= 6843	 schur= 0	 lambda= 0.094597	 levenbergIter= 1
iteration= 32	 chi2= 468.065028	 time= 0.0349438	 cumTime= 1.20514	 edges= 6843	 schur= 0	 lambda= 0.063065	 levenbergIter= 1
iteration= 33	 chi2= 468.065028	 time= 0.0472793	 cumTime= 1.25242	 edges= 6843	 schur= 0	 lambda= 2.690767	 levenbergIter= 4
iteration= 34	 chi2= 468.065028	 time= 0.0349359	 cumTime= 1.28735	 edges= 6843	 schur= 0	 lambda= 1.793845	 levenbergIter= 1
iteration= 35	 chi2= 468.065028	 time= 0.0350663	 cumTime= 1.32242	 edges= 6843	 schur= 0	 lambda= 1.195896	 levenbergIter= 1
iteration= 36	 chi2= 468.065028	 time= 0.0349947	 cumTime= 1.35742	 edges= 6843	 schur= 0	 lambda= 0.797264	 levenbergIter= 1
iteration= 37	 chi2= 468.065028	 time= 0.0350037	 cumTime= 1.39242	 edges= 6843	 schur= 0	 lambda= 0.531510	 levenbergIter= 1
iteration= 38	 chi2= 468.065028	 time= 0.0471563	 cumTime= 1.43958	 edges= 6843	 schur= 0	 lambda= 22.677739	 levenbergIter= 4
iteration= 39	 chi2= 468.065028	 time= 0.0349358	 cumTime= 1.47451	 edges= 6843	 schur= 0	 lambda= 15.118493	 levenbergIter= 1
iteration= 40	 chi2= 468.065028	 time= 0.0351167	 cumTime= 1.50963	 edges= 6843	 schur= 0	 lambda= 10.078995	 levenbergIter= 1
iteration= 41	 chi2= 468.065028	 time= 0.0350639	 cumTime= 1.54469	 edges= 6843	 schur= 0	 lambda= 6.719330	 levenbergIter= 1
iteration= 42	 chi2= 468.065028	 time= 0.0350967	 cumTime= 1.57979	 edges= 6843	 schur= 0	 lambda= 4.479553	 levenbergIter= 1
iteration= 43	 chi2= 468.065028	 time= 0.0350343	 cumTime= 1.61482	 edges= 6843	 schur= 0	 lambda= 2.986369	 levenbergIter= 1
iteration= 44	 chi2= 468.065028	 time= 0.0349	 cumTime= 1.64972	 edges= 6843	 schur= 0	 lambda= 1.990913	 levenbergIter= 1
iteration= 45	 chi2= 468.065028	 time= 0.0471932	 cumTime= 1.69692	 edges= 6843	 schur= 0	 lambda= 84.945606	 levenbergIter= 4
iteration= 46	 chi2= 468.065028	 time= 0.0350559	 cumTime= 1.73197	 edges= 6843	 schur= 0	 lambda= 56.630404	 levenbergIter= 1
iteration= 47	 chi2= 468.065028	 time= 0.035382	 cumTime= 1.76735	 edges= 6843	 schur= 0	 lambda= 37.753602	 levenbergIter= 1
iteration= 48	 chi2= 468.065028	 time= 0.0349866	 cumTime= 1.80234	 edges= 6843	 schur= 0	 lambda= 25.169068	 levenbergIter= 1
iteration= 49	 chi2= 468.065028	 time= 0.0351412	 cumTime= 1.83748	 edges= 6843	 schur= 0	 lambda= 16.779379	 levenbergIter= 1
iteration= 50	 chi2= 468.065028	 time= 0.0351712	 cumTime= 1.87265	 edges= 6843	 schur= 0	 lambda= 11.186253	 levenbergIter= 1
iteration= 51	 chi2= 468.065028	 time= 0.0349284	 cumTime= 1.90758	 edges= 6843	 schur= 0	 lambda= 7.457502	 levenbergIter= 1
iteration= 52	 chi2= 468.065028	 time= 0.0354512	 cumTime= 1.94303	 edges= 6843	 schur= 0	 lambda= 4.971668	 levenbergIter= 1
iteration= 53	 chi2= 468.065028	 time= 0.0350837	 cumTime= 1.97812	 edges= 6843	 schur= 0	 lambda= 3.314445	 levenbergIter= 1
iteration= 54	 chi2= 468.065028	 time= 0.0352127	 cumTime= 2.01333	 edges= 6843	 schur= 0	 lambda= 2.209630	 levenbergIter= 1
iteration= 55	 chi2= 468.065028	 time= 0.0349936	 cumTime= 2.04832	 edges= 6843	 schur= 0	 lambda= 1.473087	 levenbergIter= 1
iteration= 56	 chi2= 468.065028	 time= 0.0349023	 cumTime= 2.08323	 edges= 6843	 schur= 0	 lambda= 0.982058	 levenbergIter= 1
iteration= 57	 chi2= 468.065028	 time= 0.0470382	 cumTime= 2.13026	 edges= 6843	 schur= 0	 lambda= 41.901135	 levenbergIter= 4
iteration= 58	 chi2= 468.065028	 time= 0.0349818	 cumTime= 2.16525	 edges= 6843	 schur= 0	 lambda= 27.934090	 levenbergIter= 1
iteration= 59	 chi2= 468.065028	 time= 0.0349831	 cumTime= 2.20023	 edges= 6843	 schur= 0	 lambda= 18.622726	 levenbergIter= 1
iteration= 60	 chi2= 468.065028	 time= 0.0351551	 cumTime= 2.23538	 edges= 6843	 schur= 0	 lambda= 12.415151	 levenbergIter= 1
iteration= 61	 chi2= 468.065028	 time= 0.035053	 cumTime= 2.27044	 edges= 6843	 schur= 0	 lambda= 8.276767	 levenbergIter= 1
iteration= 62	 chi2= 468.065028	 time= 0.0350606	 cumTime= 2.3055	 edges= 6843	 schur= 0	 lambda= 5.517845	 levenbergIter= 1
iteration= 63	 chi2= 468.065028	 time= 0.0432361	 cumTime= 2.34873	 edges= 6843	 schur= 0	 lambda= 29.428506	 levenbergIter= 3
iteration= 64	 chi2= 468.065028	 time= 0.035078	 cumTime= 2.38381	 edges= 6843	 schur= 0	 lambda= 19.619004	 levenbergIter= 1
iteration= 65	 chi2= 468.065028	 time= 0.0431555	 cumTime= 2.42697	 edges= 6843	 schur= 0	 lambda= 104.634688	 levenbergIter= 3
iteration= 66	 chi2= 468.065028	 time= 0.0352503	 cumTime= 2.46222	 edges= 6843	 schur= 0	 lambda= 69.756459	 levenbergIter= 1
iteration= 67	 chi2= 468.065028	 time= 0.0350938	 cumTime= 2.49731	 edges= 6843	 schur= 0	 lambda= 46.504306	 levenbergIter= 1
iteration= 68	 chi2= 468.065028	 time= 0.0349346	 cumTime= 2.53225	 edges= 6843	 schur= 0	 lambda= 31.002871	 levenbergIter= 1
iteration= 69	 chi2= 468.065028	 time= 0.0350919	 cumTime= 2.56734	 edges= 6843	 schur= 0	 lambda= 20.668580	 levenbergIter= 1
iteration= 70	 chi2= 468.065028	 time= 0.0391162	 cumTime= 2.60645	 edges= 6843	 schur= 0	 lambda= 27.558107	 levenbergIter= 2
iteration= 71	 chi2= 468.065028	 time= 0.0349068	 cumTime= 2.64136	 edges= 6843	 schur= 0	 lambda= 18.372071	 levenbergIter= 1
iteration= 72	 chi2= 468.065028	 time= 0.0350233	 cumTime= 2.67638	 edges= 6843	 schur= 0	 lambda= 12.248048	 levenbergIter= 1
iteration= 73	 chi2= 468.065028	 time= 0.034933	 cumTime= 2.71132	 edges= 6843	 schur= 0	 lambda= 8.165365	 levenbergIter= 1
iteration= 74	 chi2= 468.065028	 time= 0.0390611	 cumTime= 2.75038	 edges= 6843	 schur= 0	 lambda= 10.887153	 levenbergIter= 2
iteration= 75	 chi2= 468.065028	 time= 0.0350384	 cumTime= 2.78542	 edges= 6843	 schur= 0	 lambda= 7.258102	 levenbergIter= 1
iteration= 76	 chi2= 468.065028	 time= 0.0349391	 cumTime= 2.82036	 edges= 6843	 schur= 0	 lambda= 4.838735	 levenbergIter= 1
iteration= 77	 chi2= 468.065028	 time= 0.0349793	 cumTime= 2.85533	 edges= 6843	 schur= 0	 lambda= 3.225823	 levenbergIter= 1
iteration= 78	 chi2= 468.065028	 time= 0.0350315	 cumTime= 2.89037	 edges= 6843	 schur= 0	 lambda= 2.150549	 levenbergIter= 1
iteration= 79	 chi2= 468.065028	 time= 0.0354239	 cumTime= 2.92579	 edges= 6843	 schur= 0	 lambda= 1.433699	 levenbergIter= 1
iteration= 80	 chi2= 468.065028	 time= 0.0470712	 cumTime= 2.97286	 edges= 6843	 schur= 0	 lambda= 61.171167	 levenbergIter= 4
iteration= 81	 chi2= 468.065028	 time= 0.0350588	 cumTime= 3.00792	 edges= 6843	 schur= 0	 lambda= 40.780778	 levenbergIter= 1
iteration= 82	 chi2= 468.065028	 time= 0.0349826	 cumTime= 3.0429	 edges= 6843	 schur= 0	 lambda= 27.187185	 levenbergIter= 1
iteration= 83	 chi2= 468.065028	 time= 0.0349392	 cumTime= 3.07784	 edges= 6843	 schur= 0	 lambda= 18.124790	 levenbergIter= 1
iteration= 84	 chi2= 468.065028	 time= 0.0349629	 cumTime= 3.1128	 edges= 6843	 schur= 0	 lambda= 12.083193	 levenbergIter= 1
iteration= 85	 chi2= 468.065028	 time= 0.0352133	 cumTime= 3.14802	 edges= 6843	 schur= 0	 lambda= 8.055462	 levenbergIter= 1
iteration= 86	 chi2= 468.065028	 time= 0.0351204	 cumTime= 3.18314	 edges= 6843	 schur= 0	 lambda= 5.370308	 levenbergIter= 1
iteration= 87	 chi2= 468.065028	 time= 0.0430952	 cumTime= 3.22623	 edges= 6843	 schur= 0	 lambda= 28.641644	 levenbergIter= 3
iteration= 88	 chi2= 468.065028	 time= 0.0352413	 cumTime= 3.26147	 edges= 6843	 schur= 0	 lambda= 19.094429	 levenbergIter= 1
iteration= 89	 chi2= 468.065028	 time= 0.0351148	 cumTime= 3.29659	 edges= 6843	 schur= 0	 lambda= 12.729619	 levenbergIter= 1
iteration= 90	 chi2= 468.065028	 time= 0.0433217	 cumTime= 3.33991	 edges= 6843	 schur= 0	 lambda= 67.891303	 levenbergIter= 3
iteration= 91	 chi2= 468.065028	 time= 0.0350147	 cumTime= 3.37493	 edges= 6843	 schur= 0	 lambda= 45.260869	 levenbergIter= 1
iteration= 92	 chi2= 468.065028	 time= 0.0431807	 cumTime= 3.41811	 edges= 6843	 schur= 0	 lambda= 241.391301	 levenbergIter= 3
iteration= 93	 chi2= 468.065028	 time= 0.0349405	 cumTime= 3.45305	 edges= 6843	 schur= 0	 lambda= 160.927534	 levenbergIter= 1
iteration= 94	 chi2= 468.065028	 time= 0.0349453	 cumTime= 3.48799	 edges= 6843	 schur= 0	 lambda= 107.285023	 levenbergIter= 1
iteration= 95	 chi2= 468.065028	 time= 0.0390688	 cumTime= 3.52706	 edges= 6843	 schur= 0	 lambda= 143.046697	 levenbergIter= 2
iteration= 96	 chi2= 468.065028	 time= 0.0350537	 cumTime= 3.56212	 edges= 6843	 schur= 0	 lambda= 95.364465	 levenbergIter= 1
iteration= 97	 chi2= 468.065028	 time= 0.0429644	 cumTime= 3.60508	 edges= 6843	 schur= 0	 lambda= 508.610478	 levenbergIter= 3
iteration= 98	 chi2= 468.065028	 time= 0.0350958	 cumTime= 3.64018	 edges= 6843	 schur= 0	 lambda= 339.073652	 levenbergIter= 1
iteration= 99	 chi2= 468.065028	 time= 0.0350706	 cumTime= 3.67525	 edges= 6843	 schur= 0	 lambda= 226.049101	 levenbergIter= 1
I0114 13:20:39.189853 421782 optimization.cc:255] RTK 误差：
I0114 13:20:39.189865 421782 optimization.cc:256] RTK 平移误差：数量: 1143, 均值: 5.423604, 中位数: 0.064771, 0.1分位: 0.008310, 0.9分位: 0.498921, 0.95分位：32.240261, 最大值: 173.001502, 阈值: 1.000000
I0114 13:20:39.189934 421782 optimization.cc:257] lidar 误差：数量: 5700, 均值: 0.002259, 中位数: 0.000477, 0.1分位: 0.000013, 0.9分位: 0.007259, 0.95分位：0.011946, 最大值: 0.028087, 阈值: 0.000000
I0114 13:20:39.191107 421782 optimization.cc:258] loop 误差：
I0114 13:20:39.191128 421782 optimization.cc:278] gnss outlier: 103/1143
I0114 13:20:39.191130 421782 optimization.cc:283] loop outlier: 0/0
iteration= 0	 chi2= 110.934465	 time= 0.0470475	 cumTime= 0.0470475	 edges= 6740	 schur= 0	 lambda= 13.285428	 levenbergIter= 1
iteration= 1	 chi2= 110.909744	 time= 0.0351188	 cumTime= 0.0821663	 edges= 6740	 schur= 0	 lambda= 4.428476	 levenbergIter= 1
iteration= 2	 chi2= 110.870717	 time= 0.0348684	 cumTime= 0.117035	 edges= 6740	 schur= 0	 lambda= 1.476159	 levenbergIter= 1
iteration= 3	 chi2= 110.825776	 time= 0.0349312	 cumTime= 0.151966	 edges= 6740	 schur= 0	 lambda= 0.492053	 levenbergIter= 1
iteration= 4	 chi2= 110.792967	 time= 0.0351164	 cumTime= 0.187082	 edges= 6740	 schur= 0	 lambda= 0.164018	 levenbergIter= 1
iteration= 5	 chi2= 110.779861	 time= 0.0354022	 cumTime= 0.222484	 edges= 6740	 schur= 0	 lambda= 0.061012	 levenbergIter= 1
iteration= 6	 chi2= 110.777612	 time= 0.0354614	 cumTime= 0.257946	 edges= 6740	 schur= 0	 lambda= 0.040675	 levenbergIter= 1
iteration= 7	 chi2= 110.777317	 time= 0.0350879	 cumTime= 0.293034	 edges= 6740	 schur= 0	 lambda= 0.027116	 levenbergIter= 1
iteration= 8	 chi2= 110.777119	 time= 0.0350583	 cumTime= 0.328092	 edges= 6740	 schur= 0	 lambda= 0.018078	 levenbergIter= 1
iteration= 9	 chi2= 110.776952	 time= 0.0351737	 cumTime= 0.363266	 edges= 6740	 schur= 0	 lambda= 0.012052	 levenbergIter= 1
iteration= 10	 chi2= 110.776786	 time= 0.035232	 cumTime= 0.398498	 edges= 6740	 schur= 0	 lambda= 0.008034	 levenbergIter= 1
iteration= 11	 chi2= 110.776656	 time= 0.0352243	 cumTime= 0.433722	 edges= 6740	 schur= 0	 lambda= 0.005356	 levenbergIter= 1
iteration= 12	 chi2= 110.776583	 time= 0.035121	 cumTime= 0.468843	 edges= 6740	 schur= 0	 lambda= 0.003571	 levenbergIter= 1
iteration= 13	 chi2= 110.776518	 time= 0.0351649	 cumTime= 0.504008	 edges= 6740	 schur= 0	 lambda= 0.002381	 levenbergIter= 1
iteration= 14	 chi2= 110.776499	 time= 0.035041	 cumTime= 0.539049	 edges= 6740	 schur= 0	 lambda= 0.001587	 levenbergIter= 1
iteration= 15	 chi2= 110.776499	 time= 0.0470622	 cumTime= 0.586111	 edges= 6740	 schur= 0	 lambda= 0.067715	 levenbergIter= 4
iteration= 16	 chi2= 110.776499	 time= 0.0351206	 cumTime= 0.621232	 edges= 6740	 schur= 0	 lambda= 0.045143	 levenbergIter= 1
iteration= 17	 chi2= 110.776499	 time= 0.0349234	 cumTime= 0.656155	 edges= 6740	 schur= 0	 lambda= 0.030095	 levenbergIter= 1
iteration= 18	 chi2= 110.776499	 time= 0.0351566	 cumTime= 0.691312	 edges= 6740	 schur= 0	 lambda= 0.020064	 levenbergIter= 1
iteration= 19	 chi2= 110.776499	 time= 0.0391649	 cumTime= 0.730476	 edges= 6740	 schur= 0	 lambda= 0.026751	 levenbergIter= 2
iteration= 20	 chi2= 110.776499	 time= 0.0431754	 cumTime= 0.773652	 edges= 6740	 schur= 0	 lambda= 0.142674	 levenbergIter= 3
iteration= 21	 chi2= 110.776499	 time= 0.0388819	 cumTime= 0.812534	 edges= 6740	 schur= 0	 lambda= 0.190232	 levenbergIter= 2
iteration= 22	 chi2= 110.776498	 time= 0.0355803	 cumTime= 0.848114	 edges= 6740	 schur= 0	 lambda= 0.126822	 levenbergIter= 1
iteration= 23	 chi2= 110.776498	 time= 0.0352904	 cumTime= 0.883404	 edges= 6740	 schur= 0	 lambda= 0.084548	 levenbergIter= 1
iteration= 24	 chi2= 110.776498	 time= 0.0350743	 cumTime= 0.918479	 edges= 6740	 schur= 0	 lambda= 0.056365	 levenbergIter= 1
iteration= 25	 chi2= 110.776498	 time= 0.0432064	 cumTime= 0.961685	 edges= 6740	 schur= 0	 lambda= 0.300614	 levenbergIter= 3
iteration= 26	 chi2= 110.776498	 time= 0.0350856	 cumTime= 0.996771	 edges= 6740	 schur= 0	 lambda= 0.200409	 levenbergIter= 1
iteration= 27	 chi2= 110.776498	 time= 0.0511913	 cumTime= 1.04796	 edges= 6740	 schur= 0	 lambda= 136.812830	 levenbergIter= 5
iteration= 28	 chi2= 110.776498	 time= 0.0352956	 cumTime= 1.08326	 edges= 6740	 schur= 0	 lambda= 91.208553	 levenbergIter= 1
iteration= 29	 chi2= 110.776498	 time= 0.0350566	 cumTime= 1.11831	 edges= 6740	 schur= 0	 lambda= 60.805702	 levenbergIter= 1
iteration= 30	 chi2= 110.776498	 time= 0.0393775	 cumTime= 1.15769	 edges= 6740	 schur= 0	 lambda= 81.074269	 levenbergIter= 2
iteration= 31	 chi2= 110.776498	 time= 0.0350615	 cumTime= 1.19275	 edges= 6740	 schur= 0	 lambda= 54.049513	 levenbergIter= 1
iteration= 32	 chi2= 110.776498	 time= 0.0349912	 cumTime= 1.22774	 edges= 6740	 schur= 0	 lambda= 36.033009	 levenbergIter= 1
iteration= 33	 chi2= 110.776498	 time= 0.0349487	 cumTime= 1.26269	 edges= 6740	 schur= 0	 lambda= 24.022006	 levenbergIter= 1
iteration= 34	 chi2= 110.776498	 time= 0.0350243	 cumTime= 1.29772	 edges= 6740	 schur= 0	 lambda= 16.014671	 levenbergIter= 1
iteration= 35	 chi2= 110.776498	 time= 0.0429543	 cumTime= 1.34067	 edges= 6740	 schur= 0	 lambda= 85.411576	 levenbergIter= 3
iteration= 36	 chi2= 110.776498	 time= 0.0351099	 cumTime= 1.37578	 edges= 6740	 schur= 0	 lambda= 56.941051	 levenbergIter= 1
iteration= 37	 chi2= 110.776498	 time= 0.0350871	 cumTime= 1.41087	 edges= 6740	 schur= 0	 lambda= 37.960700	 levenbergIter= 1
iteration= 38	 chi2= 110.776498	 time= 0.0352381	 cumTime= 1.44611	 edges= 6740	 schur= 0	 lambda= 25.307134	 levenbergIter= 1
iteration= 39	 chi2= 110.776498	 time= 0.043321	 cumTime= 1.48943	 edges= 6740	 schur= 0	 lambda= 134.971379	 levenbergIter= 3
iteration= 40	 chi2= 110.776498	 time= 0.0352493	 cumTime= 1.52468	 edges= 6740	 schur= 0	 lambda= 89.980920	 levenbergIter= 1
iteration= 41	 chi2= 110.776498	 time= 0.0353187	 cumTime= 1.56	 edges= 6740	 schur= 0	 lambda= 59.987280	 levenbergIter= 1
iteration= 42	 chi2= 110.776498	 time= 0.0394095	 cumTime= 1.59941	 edges= 6740	 schur= 0	 lambda= 79.983040	 levenbergIter= 2
iteration= 43	 chi2= 110.776498	 time= 0.0350835	 cumTime= 1.63449	 edges= 6740	 schur= 0	 lambda= 53.322026	 levenbergIter= 1
iteration= 44	 chi2= 110.776498	 time= 0.0397917	 cumTime= 1.67428	 edges= 6740	 schur= 0	 lambda= 71.096035	 levenbergIter= 2
iteration= 45	 chi2= 110.776498	 time= 0.0351542	 cumTime= 1.70943	 edges= 6740	 schur= 0	 lambda= 47.397357	 levenbergIter= 1
iteration= 46	 chi2= 110.776498	 time= 0.0349756	 cumTime= 1.74441	 edges= 6740	 schur= 0	 lambda= 31.598238	 levenbergIter= 1
iteration= 47	 chi2= 110.776498	 time= 0.0350925	 cumTime= 1.7795	 edges= 6740	 schur= 0	 lambda= 21.065492	 levenbergIter= 1
iteration= 48	 chi2= 110.776498	 time= 0.0351852	 cumTime= 1.81469	 edges= 6740	 schur= 0	 lambda= 14.043661	 levenbergIter= 1
iteration= 49	 chi2= 110.776498	 time= 0.0350873	 cumTime= 1.84978	 edges= 6740	 schur= 0	 lambda= 9.362441	 levenbergIter= 1
iteration= 50	 chi2= 110.776498	 time= 0.0352412	 cumTime= 1.88502	 edges= 6740	 schur= 0	 lambda= 6.241627	 levenbergIter= 1
iteration= 51	 chi2= 110.776498	 time= 0.0431349	 cumTime= 1.92815	 edges= 6740	 schur= 0	 lambda= 33.288679	 levenbergIter= 3
iteration= 52	 chi2= 110.776498	 time= 0.0351016	 cumTime= 1.96325	 edges= 6740	 schur= 0	 lambda= 22.192452	 levenbergIter= 1
iteration= 53	 chi2= 110.776498	 time= 0.0353896	 cumTime= 1.99864	 edges= 6740	 schur= 0	 lambda= 14.794968	 levenbergIter= 1
iteration= 54	 chi2= 110.776498	 time= 0.0431881	 cumTime= 2.04183	 edges= 6740	 schur= 0	 lambda= 78.906497	 levenbergIter= 3
iteration= 55	 chi2= 110.776498	 time= 0.0359193	 cumTime= 2.07775	 edges= 6740	 schur= 0	 lambda= 52.604332	 levenbergIter= 1
iteration= 56	 chi2= 110.776498	 time= 0.0432479	 cumTime= 2.121	 edges= 6740	 schur= 0	 lambda= 280.556435	 levenbergIter= 3
iteration= 57	 chi2= 110.776498	 time= 0.0352556	 cumTime= 2.15625	 edges= 6740	 schur= 0	 lambda= 187.037624	 levenbergIter= 1
iteration= 58	 chi2= 110.776498	 time= 0.0352096	 cumTime= 2.19146	 edges= 6740	 schur= 0	 lambda= 124.691749	 levenbergIter= 1
iteration= 59	 chi2= 110.776498	 time= 0.0357311	 cumTime= 2.22719	 edges= 6740	 schur= 0	 lambda= 83.127833	 levenbergIter= 1
iteration= 60	 chi2= 110.776498	 time= 0.0352255	 cumTime= 2.26242	 edges= 6740	 schur= 0	 lambda= 55.418555	 levenbergIter= 1
iteration= 61	 chi2= 110.776498	 time= 0.0356248	 cumTime= 2.29804	 edges= 6740	 schur= 0	 lambda= 36.945703	 levenbergIter= 1
iteration= 62	 chi2= 110.776498	 time= 0.0355421	 cumTime= 2.33359	 edges= 6740	 schur= 0	 lambda= 24.630469	 levenbergIter= 1
iteration= 63	 chi2= 110.776498	 time= 0.0437838	 cumTime= 2.37737	 edges= 6740	 schur= 0	 lambda= 131.362501	 levenbergIter= 3
iteration= 64	 chi2= 110.776498	 time= 0.0355388	 cumTime= 2.41291	 edges= 6740	 schur= 0	 lambda= 87.575001	 levenbergIter= 1
iteration= 65	 chi2= 110.776498	 time= 0.0438357	 cumTime= 2.45674	 edges= 6740	 schur= 0	 lambda= 467.066670	 levenbergIter= 3
iteration= 66	 chi2= 110.776498	 time= 0.0354844	 cumTime= 2.49223	 edges= 6740	 schur= 0	 lambda= 311.377780	 levenbergIter= 1
iteration= 67	 chi2= 110.776498	 time= 0.0351674	 cumTime= 2.5274	 edges= 6740	 schur= 0	 lambda= 207.585187	 levenbergIter= 1
iteration= 68	 chi2= 110.776498	 time= 0.035127	 cumTime= 2.56252	 edges= 6740	 schur= 0	 lambda= 138.390125	 levenbergIter= 1
iteration= 69	 chi2= 110.776498	 time= 0.0393006	 cumTime= 2.60182	 edges= 6740	 schur= 0	 lambda= 184.520166	 levenbergIter= 2
iteration= 70	 chi2= 110.776498	 time= 0.0391779	 cumTime= 2.641	 edges= 6740	 schur= 0	 lambda= 246.026888	 levenbergIter= 2
iteration= 71	 chi2= 110.776498	 time= 0.0433099	 cumTime= 2.68431	 edges= 6740	 schur= 0	 lambda= 1312.143403	 levenbergIter= 3
iteration= 72	 chi2= 110.776498	 time= 0.035241	 cumTime= 2.71955	 edges= 6740	 schur= 0	 lambda= 874.762269	 levenbergIter= 1
iteration= 73	 chi2= 110.776498	 time= 0.035148	 cumTime= 2.7547	 edges= 6740	 schur= 0	 lambda= 583.174846	 levenbergIter= 1
iteration= 74	 chi2= 110.776498	 time= 0.0355041	 cumTime= 2.79021	 edges= 6740	 schur= 0	 lambda= 388.783231	 levenbergIter= 1
iteration= 75	 chi2= 110.776498	 time= 0.0353253	 cumTime= 2.82553	 edges= 6740	 schur= 0	 lambda= 259.188820	 levenbergIter= 1
iteration= 76	 chi2= 110.776498	 time= 0.0353136	 cumTime= 2.86084	 edges= 6740	 schur= 0	 lambda= 172.792547	 levenbergIter= 1
iteration= 77	 chi2= 110.776498	 time= 0.0435316	 cumTime= 2.90438	 edges= 6740	 schur= 0	 lambda= 921.560250	 levenbergIter= 3
iteration= 78	 chi2= 110.776498	 time= 0.0465395	 cumTime= 2.95092	 edges= 6740	 schur= 0	 lambda= 4914.988002	 levenbergIter= 3
iteration= 79	 chi2= 110.776498	 time= 0.0358473	 cumTime= 2.98676	 edges= 6740	 schur= 0	 lambda= 3276.658668	 levenbergIter= 1
iteration= 80	 chi2= 110.776498	 time= 0.0396393	 cumTime= 3.0264	 edges= 6740	 schur= 0	 lambda= 4368.878224	 levenbergIter= 2
iteration= 81	 chi2= 110.776498	 time= 0.0353733	 cumTime= 3.06178	 edges= 6740	 schur= 0	 lambda= 2912.585483	 levenbergIter= 1
iteration= 82	 chi2= 110.776498	 time= 0.0397982	 cumTime= 3.10157	 edges= 6740	 schur= 0	 lambda= 3883.447310	 levenbergIter= 2
iteration= 83	 chi2= 110.776498	 time= 0.039892	 cumTime= 3.14147	 edges= 6740	 schur= 0	 lambda= 5177.929747	 levenbergIter= 2
iteration= 84	 chi2= 110.776498	 time= 0.0355231	 cumTime= 3.17699	 edges= 6740	 schur= 0	 lambda= 3451.953165	 levenbergIter= 1
iteration= 85	 chi2= 110.776498	 time= 0.0438638	 cumTime= 3.22085	 edges= 6740	 schur= 0	 lambda= 18410.416878	 levenbergIter= 3
iteration= 86	 chi2= 110.776498	 time= 0.0437571	 cumTime= 3.26461	 edges= 6740	 schur= 0	 lambda= 98188.890014	 levenbergIter= 3
iteration= 87	 chi2= 110.776498	 time= 0.0354665	 cumTime= 3.30008	 edges= 6740	 schur= 0	 lambda= 65459.260010	 levenbergIter= 1
iteration= 88	 chi2= 110.776498	 time= 0.0390789	 cumTime= 3.33915	 edges= 6740	 schur= 0	 lambda= 87279.013346	 levenbergIter= 2
iteration= 89	 chi2= 110.776498	 time= 0.0391281	 cumTime= 3.37828	 edges= 6740	 schur= 0	 lambda= 116372.017795	 levenbergIter= 2
iteration= 90	 chi2= 110.776498	 time= 0.0392514	 cumTime= 3.41753	 edges= 6740	 schur= 0	 lambda= 155162.690393	 levenbergIter= 2
iteration= 91	 chi2= 110.776498	 time= 0.0388398	 cumTime= 3.45637	 edges= 6740	 schur= 0	 lambda= 206883.587191	 levenbergIter= 2
iteration= 92	 chi2= 110.776498	 time= 0.0432031	 cumTime= 3.49958	 edges= 6740	 schur= 0	 lambda= 1103379.131683	 levenbergIter= 3
iteration= 93	 chi2= 110.776498	 time= 0.0431585	 cumTime= 3.54274	 edges= 6740	 schur= 0	 lambda= 5884688.702312	 levenbergIter= 3
iteration= 94	 chi2= 110.776498	 time= 0.0554472	 cumTime= 3.59818	 edges= 6740	 schur= 0	 lambda= 128552986264.900513	 levenbergIter= 6
iteration= 95	 chi2= 110.776498	 time= 0.0509725	 cumTime= 3.64916	 edges= 6740	 schur= 0	 lambda= 87758838623505.406250	 levenbergIter= 5
iteration= 96	 chi2= 110.776498	 time= 0.0730046	 cumTime= 3.72216	 edges= 6740	 schur= 0	 lambda= 3161845383386291517557101821952.000000	 levenbergIter= 10
I0114 13:20:43.021499 421782 optimization.cc:255] RTK 误差：
I0114 13:20:43.021515 421782 optimization.cc:256] RTK 平移误差：数量: 1040, 均值: 0.091952, 中位数: 0.046066, 0.1分位: 0.007426, 0.9分位: 0.227438, 0.95分位：0.305799, 最大值: 0.964747, 阈值: 1.000000
I0114 13:20:43.021579 421782 optimization.cc:257] lidar 误差：数量: 5700, 均值: 0.002657, 中位数: 0.000573, 0.1分位: 0.000015, 0.9分位: 0.008337, 0.95分位：0.013616, 最大值: 0.032128, 阈值: 0.000000
I0114 13:20:43.022814 421782 optimization.cc:258] loop 误差：
I0114 13:20:43.061812 421782 optimization.cc:306] med error: 0.496898
I0114 13:20:43.108528 421782 optimization.cc:96] done

wu@WP:~/slam_in_autonomous_driving/data/ch9$ g2o_viewer before.g2o

wu@WP:~/slam_in_autonomous_driving/data/ch9$ g2o_viewer after.g2o

② 第二阶段优化结果

注意：完成回环检测后才进行第二阶段优化。

wu@WP:~/slam_in_autonomous_driving/bin$ ./run_optimization --stage=2

I0114 14:47:06.440253 425119 run_optimization.cc:23] testing optimization
I0114 14:47:06.444443 425119 keyframe.cc:78] Loaded kfs: 1143
I0114 14:47:06.444453 425119 optimization.cc:52] keyframes: 1143
I0114 14:47:06.444612 425119 optimization.cc:66] TBG = 
000001 000000 000000 000000
000000 000001 000000 000.24
000000 000000 000001 -0.283
000000 000000 000000 000001
I0114 14:47:06.444928 425119 optimization.cc:401] loaded loops: 249
I0114 14:47:06.444943 425119 optimization.cc:76] running optimization on stage 2
I0114 14:47:06.445127 425119 optimization.cc:146] vertex: 1143
I0114 14:47:06.445129 425119 optimization.cc:158] Info of rtk trans: 000.25 000.25 0.0025
I0114 14:47:06.445425 425119 optimization.cc:191] gnss edges: 0, 1143
I0114 14:47:06.447690 425119 optimization.cc:221] lidar edges: 5700
I0114 14:47:06.498154 425119 optimization.cc:85] RTK 误差：
I0114 14:47:06.498178 425119 optimization.cc:86] RTK 平移误差：数量: 1143, 均值: 0.054123, 中位数: 0.000632, 0.1分位: 0.000078, 0.9分位: 0.004924, 0.95分位：0.321209, 最大值: 1.727218, 阈值: 1.000000
I0114 14:47:06.498303 425119 optimization.cc:87] lidar 误差：数量: 5700, 均值: 0.002657, 中位数: 0.000573, 0.1分位: 0.000015, 0.9分位: 0.008335, 0.95分位：0.013617, 最大值: 0.032125, 阈值: 0.000000
iteration= 0	 chi2= 732.273825	 time= 0.0669518	 cumTime= 0.0669518	 edges= 7092	 schur= 0	 lambda= 14.600698	 levenbergIter= 1
iteration= 1	 chi2= 692.103366	 time= 0.0427196	 cumTime= 0.109671	 edges= 7092	 schur= 0	 lambda= 4.866899	 levenbergIter= 1
iteration= 2	 chi2= 683.780205	 time= 0.044161	 cumTime= 0.153832	 edges= 7092	 schur= 0	 lambda= 1.622300	 levenbergIter= 1
iteration= 3	 chi2= 676.140472	 time= 0.042287	 cumTime= 0.196119	 edges= 7092	 schur= 0	 lambda= 0.540767	 levenbergIter= 1
iteration= 4	 chi2= 668.287330	 time= 0.0447868	 cumTime= 0.240906	 edges= 7092	 schur= 0	 lambda= 0.180256	 levenbergIter= 1
iteration= 5	 chi2= 663.433612	 time= 0.042204	 cumTime= 0.28311	 edges= 7092	 schur= 0	 lambda= 0.060085	 levenbergIter= 1
iteration= 6	 chi2= 661.863390	 time= 0.0435874	 cumTime= 0.326697	 edges= 7092	 schur= 0	 lambda= 0.020028	 levenbergIter= 1
iteration= 7	 chi2= 661.592309	 time= 0.0476941	 cumTime= 0.374392	 edges= 7092	 schur= 0	 lambda= 0.006676	 levenbergIter= 1
iteration= 8	 chi2= 661.365199	 time= 0.0439028	 cumTime= 0.418294	 edges= 7092	 schur= 0	 lambda= 0.002225	 levenbergIter= 1
iteration= 9	 chi2= 661.154271	 time= 0.0424439	 cumTime= 0.460738	 edges= 7092	 schur= 0	 lambda= 0.000742	 levenbergIter= 1
iteration= 10	 chi2= 661.091033	 time= 0.044019	 cumTime= 0.504757	 edges= 7092	 schur= 0	 lambda= 0.000257	 levenbergIter= 1
iteration= 11	 chi2= 661.085559	 time= 0.0424286	 cumTime= 0.547186	 edges= 7092	 schur= 0	 lambda= 0.000171	 levenbergIter= 1
iteration= 12	 chi2= 661.084967	 time= 0.0433414	 cumTime= 0.590527	 edges= 7092	 schur= 0	 lambda= 0.000114	 levenbergIter= 1
iteration= 13	 chi2= 661.082050	 time= 0.0438021	 cumTime= 0.634329	 edges= 7092	 schur= 0	 lambda= 0.000076	 levenbergIter= 1
iteration= 14	 chi2= 661.081452	 time= 0.0514515	 cumTime= 0.685781	 edges= 7092	 schur= 0	 lambda= 0.000102	 levenbergIter= 2
iteration= 15	 chi2= 661.080845	 time= 0.059497	 cumTime= 0.745278	 edges= 7092	 schur= 0	 lambda= 0.000541	 levenbergIter= 3
iteration= 16	 chi2= 661.080815	 time= 0.0684689	 cumTime= 0.813747	 edges= 7092	 schur= 0	 lambda= 0.023099	 levenbergIter= 4
iteration= 17	 chi2= 661.080765	 time= 0.0422402	 cumTime= 0.855987	 edges= 7092	 schur= 0	 lambda= 0.015399	 levenbergIter= 1
iteration= 18	 chi2= 661.080750	 time= 0.0428142	 cumTime= 0.898801	 edges= 7092	 schur= 0	 lambda= 0.010266	 levenbergIter= 1
iteration= 19	 chi2= 661.080717	 time= 0.0429474	 cumTime= 0.941749	 edges= 7092	 schur= 0	 lambda= 0.006844	 levenbergIter= 1
iteration= 20	 chi2= 661.080716	 time= 0.0430636	 cumTime= 0.984812	 edges= 7092	 schur= 0	 lambda= 0.004563	 levenbergIter= 1
iteration= 21	 chi2= 661.080708	 time= 0.0417634	 cumTime= 1.02658	 edges= 7092	 schur= 0	 lambda= 0.003042	 levenbergIter= 1
iteration= 22	 chi2= 661.080707	 time= 0.0790561	 cumTime= 1.10563	 edges= 7092	 schur= 0	 lambda= 2.076534	 levenbergIter= 5
iteration= 23	 chi2= 661.080707	 time= 0.0417019	 cumTime= 1.14733	 edges= 7092	 schur= 0	 lambda= 1.384356	 levenbergIter= 1
iteration= 24	 chi2= 661.080707	 time= 0.0437479	 cumTime= 1.19108	 edges= 7092	 schur= 0	 lambda= 0.922904	 levenbergIter= 1
iteration= 25	 chi2= 661.080707	 time= 0.0679209	 cumTime= 1.259	 edges= 7092	 schur= 0	 lambda= 39.377243	 levenbergIter= 4
iteration= 26	 chi2= 661.080707	 time= 0.0683598	 cumTime= 1.32736	 edges= 7092	 schur= 0	 lambda= 1680.095684	 levenbergIter= 4
iteration= 27	 chi2= 661.080707	 time= 0.0412889	 cumTime= 1.36865	 edges= 7092	 schur= 0	 lambda= 1120.063789	 levenbergIter= 1
iteration= 28	 chi2= 661.080707	 time= 0.0605974	 cumTime= 1.42925	 edges= 7092	 schur= 0	 lambda= 5973.673543	 levenbergIter= 3
iteration= 29	 chi2= 661.080707	 time= 0.0507893	 cumTime= 1.48004	 edges= 7092	 schur= 0	 lambda= 7964.898058	 levenbergIter= 2
iteration= 30	 chi2= 661.080707	 time= 0.0432925	 cumTime= 1.52333	 edges= 7092	 schur= 0	 lambda= 5309.932038	 levenbergIter= 1
iteration= 31	 chi2= 661.080707	 time= 0.0418494	 cumTime= 1.56518	 edges= 7092	 schur= 0	 lambda= 3539.954692	 levenbergIter= 1
iteration= 32	 chi2= 661.080707	 time= 0.0520767	 cumTime= 1.61726	 edges= 7092	 schur= 0	 lambda= 4719.939590	 levenbergIter= 2
iteration= 33	 chi2= 661.080707	 time= 0.0429237	 cumTime= 1.66018	 edges= 7092	 schur= 0	 lambda= 3146.626393	 levenbergIter= 1
iteration= 34	 chi2= 661.080707	 time= 0.0599151	 cumTime= 1.7201	 edges= 7092	 schur= 0	 lambda= 16782.007430	 levenbergIter= 3
iteration= 35	 chi2= 661.080707	 time= 0.0419952	 cumTime= 1.76209	 edges= 7092	 schur= 0	 lambda= 11188.004953	 levenbergIter= 1
iteration= 36	 chi2= 661.080707	 time= 0.0519325	 cumTime= 1.81402	 edges= 7092	 schur= 0	 lambda= 14917.339938	 levenbergIter= 2
iteration= 37	 chi2= 661.080707	 time= 0.0427233	 cumTime= 1.85675	 edges= 7092	 schur= 0	 lambda= 9944.893292	 levenbergIter= 1
iteration= 38	 chi2= 661.080707	 time= 0.0515514	 cumTime= 1.9083	 edges= 7092	 schur= 0	 lambda= 13259.857723	 levenbergIter= 2
iteration= 39	 chi2= 661.080707	 time= 0.0514173	 cumTime= 1.95971	 edges= 7092	 schur= 0	 lambda= 17679.810297	 levenbergIter= 2
iteration= 40	 chi2= 661.080707	 time= 0.0605415	 cumTime= 2.02026	 edges= 7092	 schur= 0	 lambda= 94292.321583	 levenbergIter= 3
iteration= 41	 chi2= 661.080707	 time= 0.0431446	 cumTime= 2.0634	 edges= 7092	 schur= 0	 lambda= 62861.547722	 levenbergIter= 1
iteration= 42	 chi2= 661.080707	 time= 0.0520464	 cumTime= 2.11545	 edges= 7092	 schur= 0	 lambda= 83815.396963	 levenbergIter= 2
iteration= 43	 chi2= 661.080707	 time= 0.0529544	 cumTime= 2.1684	 edges= 7092	 schur= 0	 lambda= 111753.862617	 levenbergIter= 2
iteration= 44	 chi2= 661.080707	 time= 0.0520558	 cumTime= 2.22046	 edges= 7092	 schur= 0	 lambda= 149005.150156	 levenbergIter= 2
iteration= 45	 chi2= 661.080707	 time= 0.0427795	 cumTime= 2.26324	 edges= 7092	 schur= 0	 lambda= 99336.766771	 levenbergIter= 1
iteration= 46	 chi2= 661.080707	 time= 0.0538824	 cumTime= 2.31712	 edges= 7092	 schur= 0	 lambda= 132449.022361	 levenbergIter= 2
iteration= 47	 chi2= 661.080707	 time= 0.0512105	 cumTime= 2.36833	 edges= 7092	 schur= 0	 lambda= 176598.696481	 levenbergIter= 2
iteration= 48	 chi2= 661.080707	 time= 0.0517327	 cumTime= 2.42006	 edges= 7092	 schur= 0	 lambda= 235464.928641	 levenbergIter= 2
iteration= 49	 chi2= 661.080707	 time= 0.0512654	 cumTime= 2.47133	 edges= 7092	 schur= 0	 lambda= 313953.238189	 levenbergIter= 2
iteration= 50	 chi2= 661.080707	 time= 0.0433526	 cumTime= 2.51468	 edges= 7092	 schur= 0	 lambda= 209302.158792	 levenbergIter= 1
iteration= 51	 chi2= 661.080707	 time= 0.0484793	 cumTime= 2.56316	 edges= 7092	 schur= 0	 lambda= 279069.545056	 levenbergIter= 2
iteration= 52	 chi2= 661.080707	 time= 0.0483123	 cumTime= 2.61147	 edges= 7092	 schur= 0	 lambda= 372092.726742	 levenbergIter= 2
iteration= 53	 chi2= 661.080707	 time= 0.0404669	 cumTime= 2.65194	 edges= 7092	 schur= 0	 lambda= 248061.817828	 levenbergIter= 1
iteration= 54	 chi2= 661.080707	 time= 0.048849	 cumTime= 2.70079	 edges= 7092	 schur= 0	 lambda= 330749.090437	 levenbergIter= 2
iteration= 55	 chi2= 661.080707	 time= 0.0485885	 cumTime= 2.74938	 edges= 7092	 schur= 0	 lambda= 440998.787250	 levenbergIter= 2
iteration= 56	 chi2= 661.080707	 time= 0.0402083	 cumTime= 2.78959	 edges= 7092	 schur= 0	 lambda= 293999.191500	 levenbergIter= 1
iteration= 57	 chi2= 661.080707	 time= 0.0484342	 cumTime= 2.83802	 edges= 7092	 schur= 0	 lambda= 391998.922000	 levenbergIter= 2
iteration= 58	 chi2= 661.080707	 time= 0.0481979	 cumTime= 2.88622	 edges= 7092	 schur= 0	 lambda= 522665.229333	 levenbergIter= 2
iteration= 59	 chi2= 661.080707	 time= 0.0565311	 cumTime= 2.94275	 edges= 7092	 schur= 0	 lambda= 2787547.889776	 levenbergIter= 3
iteration= 60	 chi2= 661.080707	 time= 0.0483864	 cumTime= 2.99113	 edges= 7092	 schur= 0	 lambda= 3716730.519702	 levenbergIter= 2
iteration= 61	 chi2= 661.080707	 time= 0.080179	 cumTime= 3.07131	 edges= 7092	 schur= 0	 lambda= 81193217113.057663	 levenbergIter= 6
iteration= 62	 chi2= 661.080707	 time= 0.0563714	 cumTime= 3.12768	 edges= 7092	 schur= 0	 lambda= 433030491269.640869	 levenbergIter= 3
iteration= 63	 chi2= 661.080707	 time= 0.0646127	 cumTime= 3.1923	 edges= 7092	 schur= 0	 lambda= 18475967627504.675781	 levenbergIter= 4
iteration= 64	 chi2= 661.080707	 time= 0.0658315	 cumTime= 3.25813	 edges= 7092	 schur= 0	 lambda= 788307952106866.125000	 levenbergIter= 4
iteration= 65	 chi2= 661.080707	 time= 0.122538	 cumTime= 3.38067	 edges= 7092	 schur= 0	 lambda= 28401787194893448701701766774784.000000	 levenbergIter= 10
I0114 14:47:09.966687 425119 optimization.cc:255] RTK 误差：
I0114 14:47:09.966703 425119 optimization.cc:256] RTK 平移误差：数量: 1143, 均值: 0.054377, 中位数: 0.002057, 0.1分位: 0.000644, 0.9分位: 0.008998, 0.95分位：0.311381, 最大值: 1.703702, 阈值: 1.000000
I0114 14:47:09.966771 425119 optimization.cc:257] lidar 误差：数量: 5700, 均值: 0.009403, 中位数: 0.000008, 0.1分位: 0.000000, 0.9分位: 0.033417, 0.95分位：0.055681, 最大值: 0.286353, 阈值: 0.000000
I0114 14:47:09.967931 425119 optimization.cc:258] loop 误差：数量: 249, 均值: 2.421480, 中位数: 1.883298, 0.1分位: 0.652742, 0.9分位: 4.821146, 0.95分位：6.357756, 最大值: 16.565907, 阈值: 0.000000
I0114 14:47:09.968003 425119 optimization.cc:278] gnss outlier: 25/1143
I0114 14:47:09.968009 425119 optimization.cc:283] loop outlier: 21/249
iteration= 0	 chi2= 524.481274	 time= 0.0534861	 cumTime= 0.0534861	 edges= 7046	 schur= 0	 lambda= 14.659429	 levenbergIter= 1
iteration= 1	 chi2= 524.396160	 time= 0.0416237	 cumTime= 0.0951098	 edges= 7046	 schur= 0	 lambda= 4.886476	 levenbergIter= 1
iteration= 2	 chi2= 524.311806	 time= 0.0407585	 cumTime= 0.135868	 edges= 7046	 schur= 0	 lambda= 1.628825	 levenbergIter= 1
iteration= 3	 chi2= 524.201346	 time= 0.0401856	 cumTime= 0.176054	 edges= 7046	 schur= 0	 lambda= 0.542942	 levenbergIter= 1
iteration= 4	 chi2= 524.070422	 time= 0.0399105	 cumTime= 0.215964	 edges= 7046	 schur= 0	 lambda= 0.180981	 levenbergIter= 1
iteration= 5	 chi2= 523.976912	 time= 0.0406078	 cumTime= 0.256572	 edges= 7046	 schur= 0	 lambda= 0.060327	 levenbergIter= 1
iteration= 6	 chi2= 523.943490	 time= 0.0401059	 cumTime= 0.296678	 edges= 7046	 schur= 0	 lambda= 0.020109	 levenbergIter= 1
iteration= 7	 chi2= 523.931502	 time= 0.0400695	 cumTime= 0.336748	 edges= 7046	 schur= 0	 lambda= 0.006703	 levenbergIter= 1
iteration= 8	 chi2= 523.921460	 time= 0.0401983	 cumTime= 0.376946	 edges= 7046	 schur= 0	 lambda= 0.002234	 levenbergIter= 1
iteration= 9	 chi2= 523.911708	 time= 0.039986	 cumTime= 0.416932	 edges= 7046	 schur= 0	 lambda= 0.001490	 levenbergIter= 1
iteration= 10	 chi2= 523.909097	 time= 0.0400748	 cumTime= 0.457007	 edges= 7046	 schur= 0	 lambda= 0.000993	 levenbergIter= 1
iteration= 11	 chi2= 523.908841	 time= 0.0401001	 cumTime= 0.497107	 edges= 7046	 schur= 0	 lambda= 0.000662	 levenbergIter= 1
iteration= 12	 chi2= 523.908475	 time= 0.0402014	 cumTime= 0.537308	 edges= 7046	 schur= 0	 lambda= 0.000441	 levenbergIter= 1
iteration= 13	 chi2= 523.908462	 time= 0.0558095	 cumTime= 0.593118	 edges= 7046	 schur= 0	 lambda= 0.002354	 levenbergIter= 3
iteration= 14	 chi2= 523.908461	 time= 0.0712249	 cumTime= 0.664343	 edges= 7046	 schur= 0	 lambda= 1.606902	 levenbergIter= 5
iteration= 15	 chi2= 523.908459	 time= 0.0398896	 cumTime= 0.704232	 edges= 7046	 schur= 0	 lambda= 1.071268	 levenbergIter= 1
iteration= 16	 chi2= 523.908457	 time= 0.0400018	 cumTime= 0.744234	 edges= 7046	 schur= 0	 lambda= 0.714179	 levenbergIter= 1
iteration= 17	 chi2= 523.908456	 time= 0.0398928	 cumTime= 0.784127	 edges= 7046	 schur= 0	 lambda= 0.476119	 levenbergIter= 1
iteration= 18	 chi2= 523.908456	 time= 0.055664	 cumTime= 0.839791	 edges= 7046	 schur= 0	 lambda= 2.539302	 levenbergIter= 3
iteration= 19	 chi2= 523.908456	 time= 0.0401968	 cumTime= 0.879988	 edges= 7046	 schur= 0	 lambda= 1.692868	 levenbergIter= 1
iteration= 20	 chi2= 523.908456	 time= 0.0634764	 cumTime= 0.943464	 edges= 7046	 schur= 0	 lambda= 72.229036	 levenbergIter= 4
iteration= 21	 chi2= 523.908456	 time= 0.0401159	 cumTime= 0.98358	 edges= 7046	 schur= 0	 lambda= 48.152691	 levenbergIter= 1
iteration= 22	 chi2= 523.908456	 time= 0.040062	 cumTime= 1.02364	 edges= 7046	 schur= 0	 lambda= 32.101794	 levenbergIter= 1
iteration= 23	 chi2= 523.908456	 time= 0.0399811	 cumTime= 1.06362	 edges= 7046	 schur= 0	 lambda= 21.401196	 levenbergIter= 1
iteration= 24	 chi2= 523.908455	 time= 0.0634087	 cumTime= 1.12703	 edges= 7046	 schur= 0	 lambda= 913.117694	 levenbergIter= 4
iteration= 25	 chi2= 523.908455	 time= 0.0399583	 cumTime= 1.16699	 edges= 7046	 schur= 0	 lambda= 608.745129	 levenbergIter= 1
iteration= 26	 chi2= 523.908455	 time= 0.0401255	 cumTime= 1.20712	 edges= 7046	 schur= 0	 lambda= 405.830086	 levenbergIter= 1
iteration= 27	 chi2= 523.908455	 time= 0.0477113	 cumTime= 1.25483	 edges= 7046	 schur= 0	 lambda= 541.106782	 levenbergIter= 2
iteration= 28	 chi2= 523.908455	 time= 0.0399891	 cumTime= 1.29482	 edges= 7046	 schur= 0	 lambda= 360.737854	 levenbergIter= 1
iteration= 29	 chi2= 523.908455	 time= 0.0400652	 cumTime= 1.33488	 edges= 7046	 schur= 0	 lambda= 240.491903	 levenbergIter= 1
iteration= 30	 chi2= 523.908455	 time= 0.0556648	 cumTime= 1.39055	 edges= 7046	 schur= 0	 lambda= 1282.623482	 levenbergIter= 3
iteration= 31	 chi2= 523.908455	 time= 0.0399304	 cumTime= 1.43048	 edges= 7046	 schur= 0	 lambda= 855.082322	 levenbergIter= 1
iteration= 32	 chi2= 523.908455	 time= 0.048053	 cumTime= 1.47853	 edges= 7046	 schur= 0	 lambda= 1140.109762	 levenbergIter= 2
iteration= 33	 chi2= 523.908455	 time= 0.0399793	 cumTime= 1.51851	 edges= 7046	 schur= 0	 lambda= 760.073175	 levenbergIter= 1
iteration= 34	 chi2= 523.908455	 time= 0.0557636	 cumTime= 1.57427	 edges= 7046	 schur= 0	 lambda= 4053.723599	 levenbergIter= 3
iteration= 35	 chi2= 523.908455	 time= 0.0401256	 cumTime= 1.6144	 edges= 7046	 schur= 0	 lambda= 2702.482399	 levenbergIter= 1
iteration= 36	 chi2= 523.908455	 time= 0.0399855	 cumTime= 1.65438	 edges= 7046	 schur= 0	 lambda= 1801.654933	 levenbergIter= 1
iteration= 37	 chi2= 523.908455	 time= 0.0477881	 cumTime= 1.70217	 edges= 7046	 schur= 0	 lambda= 2402.206577	 levenbergIter= 2
iteration= 38	 chi2= 523.908455	 time= 0.0558878	 cumTime= 1.75806	 edges= 7046	 schur= 0	 lambda= 12811.768410	 levenbergIter= 3
iteration= 39	 chi2= 523.908455	 time= 0.0398665	 cumTime= 1.79793	 edges= 7046	 schur= 0	 lambda= 8541.178940	 levenbergIter= 1
iteration= 40	 chi2= 523.908455	 time= 0.0477839	 cumTime= 1.84571	 edges= 7046	 schur= 0	 lambda= 11388.238587	 levenbergIter= 2
iteration= 41	 chi2= 523.908455	 time= 0.0398962	 cumTime= 1.88561	 edges= 7046	 schur= 0	 lambda= 7592.159058	 levenbergIter= 1
iteration= 42	 chi2= 523.908455	 time= 0.0478328	 cumTime= 1.93344	 edges= 7046	 schur= 0	 lambda= 10122.878744	 levenbergIter= 2
iteration= 43	 chi2= 523.908455	 time= 0.0476778	 cumTime= 1.98112	 edges= 7046	 schur= 0	 lambda= 13497.171659	 levenbergIter= 2
iteration= 44	 chi2= 523.908455	 time= 0.0478666	 cumTime= 2.02898	 edges= 7046	 schur= 0	 lambda= 17996.228878	 levenbergIter= 2
iteration= 45	 chi2= 523.908455	 time= 0.0478792	 cumTime= 2.07686	 edges= 7046	 schur= 0	 lambda= 23994.971837	 levenbergIter= 2
iteration= 46	 chi2= 523.908455	 time= 0.048001	 cumTime= 2.12486	 edges= 7046	 schur= 0	 lambda= 31993.295783	 levenbergIter= 2
iteration= 47	 chi2= 523.908455	 time= 0.0478022	 cumTime= 2.17267	 edges= 7046	 schur= 0	 lambda= 42657.727711	 levenbergIter= 2
iteration= 48	 chi2= 523.908455	 time= 0.0476888	 cumTime= 2.22035	 edges= 7046	 schur= 0	 lambda= 56876.970281	 levenbergIter= 2
iteration= 49	 chi2= 523.908455	 time= 0.0480185	 cumTime= 2.26837	 edges= 7046	 schur= 0	 lambda= 75835.960375	 levenbergIter= 2
iteration= 50	 chi2= 523.908455	 time= 0.0478004	 cumTime= 2.31617	 edges= 7046	 schur= 0	 lambda= 101114.613833	 levenbergIter= 2
iteration= 51	 chi2= 523.908455	 time= 0.0479061	 cumTime= 2.36408	 edges= 7046	 schur= 0	 lambda= 134819.485111	 levenbergIter= 2
iteration= 52	 chi2= 523.908455	 time= 0.0477509	 cumTime= 2.41183	 edges= 7046	 schur= 0	 lambda= 179759.313481	 levenbergIter= 2
iteration= 53	 chi2= 523.908455	 time= 0.0398897	 cumTime= 2.45172	 edges= 7046	 schur= 0	 lambda= 119839.542321	 levenbergIter= 1
iteration= 54	 chi2= 523.908455	 time= 0.0477834	 cumTime= 2.4995	 edges= 7046	 schur= 0	 lambda= 159786.056428	 levenbergIter= 2
iteration= 55	 chi2= 523.908455	 time= 0.0478571	 cumTime= 2.54736	 edges= 7046	 schur= 0	 lambda= 213048.075237	 levenbergIter= 2
iteration= 56	 chi2= 523.908455	 time= 0.0477106	 cumTime= 2.59507	 edges= 7046	 schur= 0	 lambda= 284064.100316	 levenbergIter= 2
iteration= 57	 chi2= 523.908455	 time= 0.047834	 cumTime= 2.6429	 edges= 7046	 schur= 0	 lambda= 378752.133755	 levenbergIter= 2
iteration= 58	 chi2= 523.908455	 time= 0.0400486	 cumTime= 2.68295	 edges= 7046	 schur= 0	 lambda= 252501.422503	 levenbergIter= 1
iteration= 59	 chi2= 523.908455	 time= 0.0558823	 cumTime= 2.73884	 edges= 7046	 schur= 0	 lambda= 1346674.253352	 levenbergIter= 3
iteration= 60	 chi2= 523.908455	 time= 0.0399954	 cumTime= 2.77883	 edges= 7046	 schur= 0	 lambda= 897782.835568	 levenbergIter= 1
iteration= 61	 chi2= 523.908455	 time= 0.103178	 cumTime= 2.88201	 edges= 7046	 schur= 0	 lambda= 31587925341583663104.000000	 levenbergIter= 9
I0114 14:47:12.922147 425119 optimization.cc:255] RTK 误差：
I0114 14:47:12.922160 425119 optimization.cc:256] RTK 平移误差：数量: 1118, 均值: 0.024533, 中位数: 0.001735, 0.1分位: 0.000555, 0.9分位: 0.005800, 0.95分位：0.185730, 最大值: 0.970918, 阈值: 1.000000
I0114 14:47:12.922228 425119 optimization.cc:257] lidar 误差：数量: 5700, 均值: 0.009159, 中位数: 0.000007, 0.1分位: 0.000000, 0.9分位: 0.032959, 0.95分位：0.055079, 最大值: 0.265268, 阈值: 0.000000
I0114 14:47:12.923386 425119 optimization.cc:258] loop 误差：数量: 228, 均值: 1.948570, 中位数: 1.779741, 0.1分位: 0.619406, 0.9分位: 3.609942, 0.95分位：4.147956, 最大值: 5.030160, 阈值: 0.000000
I0114 14:47:12.963697 425119 optimization.cc:306] med error: 0.496892
I0114 14:47:12.973387 425119 optimization.cc:96] done

wu@WP:~/slam_in_autonomous_driving/data/ch9$ g2o_viewer before.g2o

g2o_viewer after.g2o 结果类似，基本没有区别。

4 回环检测

系统是离线运行的，可以一次性把所有待检查的区域检查完，不必像实时系统那样需要等待前端的结果。执行匹配时，可以充分利用并行机制，加速回环检测的过程。回环检测结果以候选回环对的形式描述：

/**
 * 回环检测候选帧
 */
struct LoopCandidate {
    LoopCandidate() {}
    LoopCandidate(IdType id1, IdType id2, SE3 Tij) : idx1_(id1), idx2_(id2), Tij_(Tij) {}

    IdType idx1_ = 0;
    IdType idx2_ = 0;
    SE3 Tij_;
    double ndt_score_ = 0.0;
};

4.1 回环检测流程

使用基于位置（欧氏距离）的回环检测，回环检测流程如下：

① 遍历第一阶段优化轨迹中的关键帧。

对每两个在空间上相隔较近，但时间上存在一定距离的关键帧，进行一次回环检测。我们称这样一对关键帧为检查点。为了防止检查点数量太大，我们设置一个 ID 间隔，即每隔多少个关键帧取一次检查。实验当中这个间隔取 5。

具体实现为：

对每个关键帧 kf_first，遍历其之后的关键帧 kf_second，检查是否满足回环条件（距离小于 30 米）。若满足回环条件，则将其添加到候选回环对中。
在整个过程中，使用 check_first 和 check_second 跟踪之前已添加为候选对的关键帧对，如果正在检查的关键帧 kf_first 离 check_first 的距离太近（ID 间隔小于 5），则跳过此关键帧进行下一个 kf_first。
当 kf_first 满足距离条件，进而判断 kf_second 是否同时满足与 check_second （ID 间隔小于 5）以及 kf_first （ID 间隔小于 100）的距离条件，如果都满足则使用第一阶段优化位姿来计算并判定 kf_first 和 kf_second 之间的平移距离是否满足回环条件（距离小于 30 米），满足则添加到回环候选对当中。

void LoopClosure::DetectLoopCandidates() {
    KFPtr check_first = nullptr;
    KFPtr check_second = nullptr;

    LOG(INFO) << "detecting loop candidates from pose in stage 1";

    // 本质上是两重循环
    for (auto iter_first = keyframes_.begin(); iter_first != keyframes_.end(); ++iter_first) {
        auto kf_first = iter_first->second;

        if (check_first != nullptr && abs(int(kf_first->id_) - int(check_first->id_)) <= skip_id_) {
            // 两个关键帧之前ID太近
            continue;
        }

        for (auto iter_second = iter_first; iter_second != keyframes_.end(); ++iter_second) {
            auto kf_second = iter_second->second;

            if (check_second != nullptr && abs(int(kf_second->id_) - int(check_second->id_)) <= skip_id_) {
                // 两个关键帧之前ID太近
                continue;
            }

            if (abs(int(kf_first->id_) - int(kf_second->id_)) < min_id_interval_) {
                /// 在同一条轨迹中，如果间隔太近，就不考虑回环
                continue;
            }

            Vec3d dt = kf_first->opti_pose_1_.translation() - kf_second->opti_pose_1_.translation();
            double t2d = dt.head<2>().norm();  // x-y distance
            double range_th = min_distance_;

            if (t2d < range_th) {
                LoopCandidate c(kf_first->id_, kf_second->id_,
                                kf_first->opti_pose_1_.inverse() * kf_second->opti_pose_1_);
                loop_candiates_.emplace_back(c);
                check_first = kf_first;
                check_second = kf_second;
            }
        }
    }
    LOG(INFO) << "detected candidates: " << loop_candiates_.size();
}

② 并发计算候选回环对是否成立

对于每个候选回环对，我们使用 scan to map 的配准方式，对候选回环对中第一个关键帧 kf1 建立子地图（前后各 40 个关键帧，每隔 4 个关键帧选取一帧，共计 20 帧点云拼接，建立在世界坐标系下），对第二个关键帧 kf2 只取其一帧点云。将点云与子地图进行多分辨率 NDT 配准，得到 kf2 世界坐标系下的位姿。

与里程计方法不同，在回环检测配准过程中，我们经常要面对初值位姿估计很差的情况，希望算法不太依赖于给定的位姿初值。因此使用由粗至精的配准过程，即多分辨率 NDT 配准（体素分辨率 10.0, 5.0, 4.0, 3.0）。

最后判断配准后的候选回环对中的 ndt_score_ 是否满足阈值（4.5），满足则视为成功的候选回环对。

void LoopClosure::ComputeLoopCandidates() {
    // 执行计算
    std::for_each(std::execution::par_unseq, loop_candiates_.begin(), loop_candiates_.end(),
                  [this](LoopCandidate& c) { ComputeForCandidate(c); });
    // 保存成功的候选
    std::vector<LoopCandidate> succ_candidates;
    for (const auto& lc : loop_candiates_) {
        if (lc.ndt_score_ > ndt_score_th_) {
            succ_candidates.emplace_back(lc);
        }
    }
    LOG(INFO) << "success: " << succ_candidates.size() << "/" << loop_candiates_.size();

    // 将两个容器的内容互换
    loop_candiates_.swap(succ_candidates);
}

void LoopClosure::ComputeForCandidate(sad::LoopCandidate& c) {
    LOG(INFO) << "aligning " << c.idx1_ << " with " << c.idx2_;
    const int submap_idx_range = 40;
    KFPtr kf1 = keyframes_.at(c.idx1_), kf2 = keyframes_.at(c.idx2_);

    auto build_submap = [this](int given_id, bool build_in_world) -> CloudPtr {
        CloudPtr submap(new PointCloudType);
        for (int idx = -submap_idx_range; idx < submap_idx_range; idx += 4) {
            int id = idx + given_id;
            if (id < 0) {
                continue;
            }
            auto iter = keyframes_.find(id);
            if (iter == keyframes_.end()) {
                continue;
            }

            auto kf = iter->second;
            CloudPtr cloud(new PointCloudType);
            pcl::io::loadPCDFile("/home/wu/slam_in_autonomous_driving/data/ch9/" + std::to_string(id) + ".pcd", *cloud);
            sad::RemoveGround(cloud, 0.1);

            if (cloud->empty()) {
                continue;
            }

            // 转到世界系下
            SE3 Twb = kf->opti_pose_1_;

            if (!build_in_world) {
                // 转到 given_id 所在的关键帧坐标系下
                Twb = keyframes_.at(given_id)->opti_pose_1_.inverse() * Twb;
            }

            CloudPtr cloud_trans(new PointCloudType);
            pcl::transformPointCloud(*cloud, *cloud_trans, Twb.matrix());

            *submap += *cloud_trans;
        }
        return submap;
    };

    auto submap_kf1 = build_submap(kf1->id_, true);

    CloudPtr submap_kf2(new PointCloudType);
    pcl::io::loadPCDFile("/home/wu/slam_in_autonomous_driving/data/ch9/" + std::to_string(kf2->id_) + ".pcd", *submap_kf2);

    if (submap_kf1->empty() || submap_kf2->empty()) {
        c.ndt_score_ = 0;
        return;
    }

    pcl::NormalDistributionsTransform<PointType, PointType> ndt;

    ndt.setTransformationEpsilon(0.05);
    ndt.setStepSize(0.7);
    ndt.setMaximumIterations(40);

    // T_wj
    Mat4f Tw2 = kf2->opti_pose_1_.matrix().cast<float>();

    /// 不同分辨率下的匹配
    // 多分辨率 NDT 配准
    CloudPtr output(new PointCloudType);
    std::vector<double> res{10.0, 5.0, 4.0, 3.0};
    for (auto& r : res) {
        // 设置/更改体素网格的分辨率。
        ndt.setResolution(r);
        // 降采样尺寸和体素分辨率成比例变化，比例因子为 0.1
        auto rough_map1 = VoxelCloud(submap_kf1, r * 0.1);
        auto rough_map2 = VoxelCloud(submap_kf2, r * 0.1);
        ndt.setInputTarget(rough_map1);
        ndt.setInputSource(rough_map2);

        ndt.align(*output, Tw2);
        Tw2 = ndt.getFinalTransformation();
    }

    Mat4d T = Tw2.cast<double>();
    Quatd q(T.block<3, 3>(0, 0));
    q.normalize();
    Vec3d t = T.block<3, 1>(0, 3);
    // T_ij = T_wi^-1 * T_wj
    c.Tij_ = kf1->opti_pose_1_.inverse() * SE3(q, t);
    // 返回 变换概率（Transformation Probability), 是一个 非负浮点数，值越大表示匹配效果越好
    c.ndt_score_ = ndt.getTransformationProbability();
    // std::cout << "Transformation Probability: " << c.ndt_score_ << std::endl;
}

③ 记录所有成功经过筛选的回环候选对，并在第二阶段优化时读取回环检测的结果

void LoopClosure::SaveResults() {
    auto save_SE3 = [](std::ostream& f, SE3 pose) {
        auto q = pose.so3().unit_quaternion();
        Vec3d t = pose.translation();
        f << t[0] << " " << t[1] << " " << t[2] << " " << q.x() << " " << q.y() << " " << q.z() << " " << q.w() << " ";
    };

    std::ofstream fout("/home/wu/slam_in_autonomous_driving/data/ch9/loops.txt");
    for (const auto& lc : loop_candiates_) {
        fout << lc.idx1_ << " " << lc.idx2_ << " " << lc.ndt_score_ << " ";
        save_SE3(fout, lc.Tij_);
        fout << std::endl;
    }
}

4.2 回环检测结果

回环检测数据会被存储在 data/ch9/keyframes.txt 。

存储顺序为：idx1_、idx2_、ndt_score_、Tij_

wu@WP:~/slam_in_autonomous_driving/bin$ ./run_loopclosure

I0114 14:39:34.600237 424832 keyframe.cc:78] Loaded kfs: 1143
I0114 14:39:34.600332 424832 loopclosure.cc:26] keyframes: 1143
I0114 14:39:34.600500 424832 loopclosure.cc:47] detecting loop candidates from pose in stage 1
I0114 14:39:34.605368 424832 loopclosure.cc:84] detected candidates: 272
I0114 14:39:34.605371 424832 loopclosure.cc:109] aligning 0 with 100
I0114 14:39:34.660082 424832 loopclosure.cc:109] aligning 0 with 106
I0114 14:39:34.715945 424832 loopclosure.cc:109] aligning 246 with 631
I0114 14:39:34.838471 424832 loopclosure.cc:109] aligning 246 with 637
I0114 14:39:34.961339 424832 loopclosure.cc:109] aligning 252 with 612
I0114 14:39:35.083011 424832 loopclosure.cc:109] aligning 252 with 618
I0114 14:39:35.214619 424832 loopclosure.cc:109] aligning 252 with 624
I0114 14:39:35.336724 424832 loopclosure.cc:109] aligning 252 with 630
I0114 14:39:35.461663 424832 loopclosure.cc:109] aligning 252 with 636
I0114 14:39:35.698182 424832 loopclosure.cc:109] aligning 252 with 642
I0114 14:39:35.822700 424832 loopclosure.cc:109] aligning 252 with 648
I0114 14:39:35.944046 424832 loopclosure.cc:109] aligning 258 with 602
I0114 14:39:36.076938 424832 loopclosure.cc:109] aligning 258 with 608
I0114 14:39:36.218856 424832 loopclosure.cc:109] aligning 258 with 614
I0114 14:39:36.361122 424832 loopclosure.cc:109] aligning 258 with 620
I0114 14:39:36.492717 424832 loopclosure.cc:109] aligning 258 with 626
I0114 14:39:36.618754 424832 loopclosure.cc:109] aligning 258 with 632
I0114 14:39:36.735234 424832 loopclosure.cc:109] aligning 258 with 638
I0114 14:39:36.852849 424832 loopclosure.cc:109] aligning 258 with 644
I0114 14:39:36.974691 424832 loopclosure.cc:109] aligning 258 with 650
I0114 14:39:37.102720 424832 loopclosure.cc:109] aligning 264 with 591
I0114 14:39:37.239533 424832 loopclosure.cc:109] aligning 264 with 597
I0114 14:39:37.377104 424832 loopclosure.cc:109] aligning 264 with 603
I0114 14:39:37.514230 424832 loopclosure.cc:109] aligning 264 with 609
I0114 14:39:37.659909 424832 loopclosure.cc:109] aligning 264 with 615
I0114 14:39:37.802589 424832 loopclosure.cc:109] aligning 264 with 621
I0114 14:39:37.935138 424832 loopclosure.cc:109] aligning 264 with 627
I0114 14:39:38.060462 424832 loopclosure.cc:109] aligning 264 with 633
I0114 14:39:38.178555 424832 loopclosure.cc:109] aligning 264 with 639
I0114 14:39:38.302886 424832 loopclosure.cc:109] aligning 264 with 645
I0114 14:39:38.424055 424832 loopclosure.cc:109] aligning 264 with 651
I0114 14:39:38.555510 424832 loopclosure.cc:109] aligning 270 with 577
I0114 14:39:38.683847 424832 loopclosure.cc:109] aligning 270 with 583
I0114 14:39:38.814306 424832 loopclosure.cc:109] aligning 270 with 589
I0114 14:39:38.945605 424832 loopclosure.cc:109] aligning 270 with 595
I0114 14:39:39.081390 424832 loopclosure.cc:109] aligning 270 with 601
I0114 14:39:39.217231 424832 loopclosure.cc:109] aligning 270 with 607
I0114 14:39:39.361466 424832 loopclosure.cc:109] aligning 270 with 613
I0114 14:39:39.482360 424832 loopclosure.cc:109] aligning 270 with 619
I0114 14:39:39.612998 424832 loopclosure.cc:109] aligning 270 with 625
I0114 14:39:39.735734 424832 loopclosure.cc:109] aligning 270 with 631
I0114 14:39:39.855314 424832 loopclosure.cc:109] aligning 270 with 637
I0114 14:39:39.976063 424832 loopclosure.cc:109] aligning 270 with 643
I0114 14:39:40.100976 424832 loopclosure.cc:109] aligning 270 with 649
I0114 14:39:40.233157 424832 loopclosure.cc:109] aligning 276 with 570
I0114 14:39:40.358598 424832 loopclosure.cc:109] aligning 276 with 576
I0114 14:39:40.482266 424832 loopclosure.cc:109] aligning 276 with 582
I0114 14:39:40.599781 424832 loopclosure.cc:109] aligning 276 with 588
I0114 14:39:40.732409 424832 loopclosure.cc:109] aligning 276 with 594
I0114 14:39:40.862918 424832 loopclosure.cc:109] aligning 276 with 600
I0114 14:39:41.003029 424832 loopclosure.cc:109] aligning 276 with 606
I0114 14:39:41.145427 424832 loopclosure.cc:109] aligning 276 with 612
I0114 14:39:41.264133 424832 loopclosure.cc:109] aligning 276 with 618
I0114 14:39:41.391021 424832 loopclosure.cc:109] aligning 276 with 624
I0114 14:39:41.510094 424832 loopclosure.cc:109] aligning 276 with 630
I0114 14:39:41.628989 424832 loopclosure.cc:109] aligning 276 with 636
I0114 14:39:41.747018 424832 loopclosure.cc:109] aligning 276 with 642
I0114 14:39:41.868511 424832 loopclosure.cc:109] aligning 276 with 648
I0114 14:39:41.986932 424832 loopclosure.cc:109] aligning 282 with 565
I0114 14:39:42.131405 424832 loopclosure.cc:109] aligning 282 with 571
I0114 14:39:42.264037 424832 loopclosure.cc:109] aligning 282 with 577
I0114 14:39:42.397466 424832 loopclosure.cc:109] aligning 282 with 583
I0114 14:39:42.533788 424832 loopclosure.cc:109] aligning 282 with 589
I0114 14:39:42.670135 424832 loopclosure.cc:109] aligning 282 with 595
I0114 14:39:42.810570 424832 loopclosure.cc:109] aligning 282 with 601
I0114 14:39:42.951463 424832 loopclosure.cc:109] aligning 282 with 607
I0114 14:39:43.101276 424832 loopclosure.cc:109] aligning 282 with 613
I0114 14:39:43.226984 424832 loopclosure.cc:109] aligning 282 with 619
I0114 14:39:43.364259 424832 loopclosure.cc:109] aligning 282 with 625
I0114 14:39:43.491612 424832 loopclosure.cc:109] aligning 282 with 631
I0114 14:39:43.615964 424832 loopclosure.cc:109] aligning 282 with 637
I0114 14:39:43.742473 424832 loopclosure.cc:109] aligning 288 with 559
I0114 14:39:43.881448 424832 loopclosure.cc:109] aligning 288 with 565
I0114 14:39:44.026831 424832 loopclosure.cc:109] aligning 288 with 571
I0114 14:39:44.159776 424832 loopclosure.cc:109] aligning 288 with 577
I0114 14:39:44.294018 424832 loopclosure.cc:109] aligning 288 with 583
I0114 14:39:44.437618 424832 loopclosure.cc:109] aligning 288 with 589
I0114 14:39:44.573611 424832 loopclosure.cc:109] aligning 288 with 595
I0114 14:39:44.710968 424832 loopclosure.cc:109] aligning 288 with 601
I0114 14:39:44.857462 424832 loopclosure.cc:109] aligning 288 with 607
I0114 14:39:45.006702 424832 loopclosure.cc:109] aligning 288 with 613
I0114 14:39:45.133204 424832 loopclosure.cc:109] aligning 288 with 619
I0114 14:39:45.275009 424832 loopclosure.cc:109] aligning 288 with 625
I0114 14:39:45.406067 424832 loopclosure.cc:109] aligning 288 with 631
I0114 14:39:45.531435 424832 loopclosure.cc:109] aligning 294 with 555
I0114 14:39:45.834162 424832 loopclosure.cc:109] aligning 294 with 561
I0114 14:39:45.985754 424832 loopclosure.cc:109] aligning 294 with 567
I0114 14:39:46.139230 424832 loopclosure.cc:109] aligning 294 with 573
I0114 14:39:46.285375 424832 loopclosure.cc:109] aligning 294 with 579
I0114 14:39:46.429277 424832 loopclosure.cc:109] aligning 294 with 585
I0114 14:39:46.570621 424832 loopclosure.cc:109] aligning 294 with 591
I0114 14:39:46.722086 424832 loopclosure.cc:109] aligning 294 with 597
I0114 14:39:46.869537 424832 loopclosure.cc:109] aligning 294 with 603
I0114 14:39:47.013934 424832 loopclosure.cc:109] aligning 294 with 609
I0114 14:39:47.166580 424832 loopclosure.cc:109] aligning 294 with 615
I0114 14:39:47.318617 424832 loopclosure.cc:109] aligning 294 with 621
I0114 14:39:47.460216 424832 loopclosure.cc:109] aligning 294 with 627
I0114 14:39:47.594961 424832 loopclosure.cc:109] aligning 300 with 551
I0114 14:39:47.746563 424832 loopclosure.cc:109] aligning 300 with 557
I0114 14:39:47.896881 424832 loopclosure.cc:109] aligning 300 with 563
I0114 14:39:48.044682 424832 loopclosure.cc:109] aligning 300 with 569
I0114 14:39:48.190032 424832 loopclosure.cc:109] aligning 300 with 575
I0114 14:39:48.334643 424832 loopclosure.cc:109] aligning 300 with 581
I0114 14:39:48.486544 424832 loopclosure.cc:109] aligning 300 with 587
I0114 14:39:48.637301 424832 loopclosure.cc:109] aligning 300 with 593
I0114 14:39:48.790048 424832 loopclosure.cc:109] aligning 300 with 599
I0114 14:39:48.942795 424832 loopclosure.cc:109] aligning 300 with 605
I0114 14:39:49.072114 424832 loopclosure.cc:109] aligning 300 with 611
I0114 14:39:49.230252 424832 loopclosure.cc:109] aligning 300 with 617
I0114 14:39:49.377197 424832 loopclosure.cc:109] aligning 300 with 623
I0114 14:39:49.514369 424832 loopclosure.cc:109] aligning 306 with 547
I0114 14:39:49.676714 424832 loopclosure.cc:109] aligning 306 with 553
I0114 14:39:49.829732 424832 loopclosure.cc:109] aligning 306 with 559
I0114 14:39:49.986097 424832 loopclosure.cc:109] aligning 306 with 565
I0114 14:39:50.150573 424832 loopclosure.cc:109] aligning 306 with 571
I0114 14:39:50.298872 424832 loopclosure.cc:109] aligning 306 with 577
I0114 14:39:50.448585 424832 loopclosure.cc:109] aligning 306 with 583
I0114 14:39:50.597843 424832 loopclosure.cc:109] aligning 306 with 589
I0114 14:39:50.747596 424832 loopclosure.cc:109] aligning 306 with 595
I0114 14:39:50.901561 424832 loopclosure.cc:109] aligning 306 with 601
I0114 14:39:51.055693 424832 loopclosure.cc:109] aligning 306 with 607
I0114 14:39:51.217823 424832 loopclosure.cc:109] aligning 306 with 613
I0114 14:39:51.356518 424832 loopclosure.cc:109] aligning 306 with 619
I0114 14:39:51.505082 424832 loopclosure.cc:109] aligning 312 with 541
I0114 14:39:51.665200 424832 loopclosure.cc:109] aligning 312 with 547
I0114 14:39:51.839165 424832 loopclosure.cc:109] aligning 312 with 553
I0114 14:39:51.997599 424832 loopclosure.cc:109] aligning 312 with 559
I0114 14:39:52.156019 424832 loopclosure.cc:109] aligning 312 with 565
I0114 14:39:52.320459 424832 loopclosure.cc:109] aligning 312 with 571
I0114 14:39:52.471503 424832 loopclosure.cc:109] aligning 312 with 577
I0114 14:39:52.624399 424832 loopclosure.cc:109] aligning 312 with 583
I0114 14:39:52.786643 424832 loopclosure.cc:109] aligning 312 with 589
I0114 14:39:52.938331 424832 loopclosure.cc:109] aligning 312 with 595
I0114 14:39:53.094488 424832 loopclosure.cc:109] aligning 312 with 601
I0114 14:39:53.264699 424832 loopclosure.cc:109] aligning 312 with 607
I0114 14:39:53.432278 424832 loopclosure.cc:109] aligning 312 with 613
I0114 14:39:53.575907 424832 loopclosure.cc:109] aligning 312 with 619
I0114 14:39:53.734344 424832 loopclosure.cc:109] aligning 318 with 536
I0114 14:39:53.909785 424832 loopclosure.cc:109] aligning 318 with 542
I0114 14:39:54.069643 424832 loopclosure.cc:109] aligning 318 with 548
I0114 14:39:54.280661 424832 loopclosure.cc:109] aligning 318 with 554
I0114 14:39:54.440966 424832 loopclosure.cc:109] aligning 318 with 560
I0114 14:39:54.928949 424832 loopclosure.cc:109] aligning 318 with 566
I0114 14:39:55.095808 424832 loopclosure.cc:109] aligning 318 with 572
I0114 14:39:55.260769 424832 loopclosure.cc:109] aligning 318 with 578
I0114 14:39:55.419515 424832 loopclosure.cc:109] aligning 318 with 584
I0114 14:39:55.568085 424832 loopclosure.cc:109] aligning 318 with 590
I0114 14:39:55.734709 424832 loopclosure.cc:109] aligning 318 with 596
I0114 14:39:55.890156 424832 loopclosure.cc:109] aligning 318 with 602
I0114 14:39:56.050088 424832 loopclosure.cc:109] aligning 318 with 608
I0114 14:39:56.216840 424832 loopclosure.cc:109] aligning 324 with 530
I0114 14:39:56.392796 424832 loopclosure.cc:109] aligning 324 with 536
I0114 14:39:56.572223 424832 loopclosure.cc:109] aligning 324 with 542
I0114 14:39:56.737180 424832 loopclosure.cc:109] aligning 324 with 548
I0114 14:39:56.915028 424832 loopclosure.cc:109] aligning 324 with 554
I0114 14:39:57.080109 424832 loopclosure.cc:109] aligning 324 with 560
I0114 14:39:57.252213 424832 loopclosure.cc:109] aligning 324 with 566
I0114 14:39:57.423735 424832 loopclosure.cc:109] aligning 324 with 572
I0114 14:39:57.590920 424832 loopclosure.cc:109] aligning 324 with 578
I0114 14:39:57.753762 424832 loopclosure.cc:109] aligning 324 with 584
I0114 14:39:57.914364 424832 loopclosure.cc:109] aligning 324 with 590
I0114 14:39:58.072743 424832 loopclosure.cc:109] aligning 324 with 596
I0114 14:39:58.231325 424832 loopclosure.cc:109] aligning 324 with 602
I0114 14:39:58.398078 424832 loopclosure.cc:109] aligning 330 with 524
I0114 14:39:58.583384 424832 loopclosure.cc:109] aligning 330 with 530
I0114 14:39:58.772176 424832 loopclosure.cc:109] aligning 330 with 536
I0114 14:39:58.960115 424832 loopclosure.cc:109] aligning 330 with 542
I0114 14:39:59.135846 424832 loopclosure.cc:109] aligning 330 with 548
I0114 14:39:59.323920 424832 loopclosure.cc:109] aligning 330 with 554
I0114 14:39:59.499577 424832 loopclosure.cc:109] aligning 330 with 560
I0114 14:39:59.693114 424832 loopclosure.cc:109] aligning 330 with 566
I0114 14:39:59.876761 424832 loopclosure.cc:109] aligning 330 with 572
I0114 14:40:00.057803 424832 loopclosure.cc:109] aligning 330 with 578
I0114 14:40:00.229849 424832 loopclosure.cc:109] aligning 336 with 519
I0114 14:40:00.416292 424832 loopclosure.cc:109] aligning 336 with 525
I0114 14:40:00.634541 424832 loopclosure.cc:109] aligning 336 with 531
I0114 14:40:00.830394 424832 loopclosure.cc:109] aligning 336 with 537
I0114 14:40:01.022815 424832 loopclosure.cc:109] aligning 336 with 543
I0114 14:40:01.239847 424832 loopclosure.cc:109] aligning 336 with 549
I0114 14:40:01.431026 424832 loopclosure.cc:109] aligning 336 with 555
I0114 14:40:01.624220 424832 loopclosure.cc:109] aligning 336 with 561
I0114 14:40:01.808413 424832 loopclosure.cc:109] aligning 336 with 567
I0114 14:40:01.998997 424832 loopclosure.cc:109] aligning 336 with 573
I0114 14:40:02.180449 424832 loopclosure.cc:109] aligning 342 with 513
I0114 14:40:02.370496 424832 loopclosure.cc:109] aligning 342 with 519
I0114 14:40:02.559326 424832 loopclosure.cc:109] aligning 342 with 525
I0114 14:40:02.761710 424832 loopclosure.cc:109] aligning 342 with 531
I0114 14:40:02.960893 424832 loopclosure.cc:109] aligning 342 with 537
I0114 14:40:03.152431 424832 loopclosure.cc:109] aligning 342 with 543
I0114 14:40:03.376098 424832 loopclosure.cc:109] aligning 342 with 549
I0114 14:40:03.569427 424832 loopclosure.cc:109] aligning 342 with 555
I0114 14:40:03.761706 424832 loopclosure.cc:109] aligning 342 with 561
I0114 14:40:03.948815 424832 loopclosure.cc:109] aligning 342 with 567
I0114 14:40:04.140497 424832 loopclosure.cc:109] aligning 348 with 507
I0114 14:40:04.349716 424832 loopclosure.cc:109] aligning 348 with 513
I0114 14:40:04.546634 424832 loopclosure.cc:109] aligning 348 with 519
I0114 14:40:04.740049 424832 loopclosure.cc:109] aligning 348 with 525
I0114 14:40:04.959317 424832 loopclosure.cc:109] aligning 348 with 531
I0114 14:40:05.162758 424832 loopclosure.cc:109] aligning 348 with 537
I0114 14:40:05.361279 424832 loopclosure.cc:109] aligning 348 with 543
I0114 14:40:05.591269 424832 loopclosure.cc:109] aligning 348 with 549
I0114 14:40:05.794329 424832 loopclosure.cc:109] aligning 348 with 555
I0114 14:40:05.995422 424832 loopclosure.cc:109] aligning 348 with 561
I0114 14:40:06.189824 424832 loopclosure.cc:109] aligning 354 with 501
I0114 14:40:06.387213 424832 loopclosure.cc:109] aligning 354 with 507
I0114 14:40:06.746068 424832 loopclosure.cc:109] aligning 354 with 513
I0114 14:40:06.939594 424832 loopclosure.cc:109] aligning 354 with 519
I0114 14:40:07.133322 424832 loopclosure.cc:109] aligning 354 with 525
I0114 14:40:07.339231 424832 loopclosure.cc:109] aligning 354 with 531
I0114 14:40:07.544878 424832 loopclosure.cc:109] aligning 354 with 537
I0114 14:40:07.736769 424832 loopclosure.cc:109] aligning 354 with 543
I0114 14:40:07.930209 424832 loopclosure.cc:109] aligning 354 with 549
I0114 14:40:08.125648 424832 loopclosure.cc:109] aligning 354 with 555
I0114 14:40:08.320142 424832 loopclosure.cc:109] aligning 360 with 495
I0114 14:40:08.540534 424832 loopclosure.cc:109] aligning 360 with 501
I0114 14:40:08.779071 424832 loopclosure.cc:109] aligning 360 with 507
I0114 14:40:08.993053 424832 loopclosure.cc:109] aligning 360 with 513
I0114 14:40:09.191763 424832 loopclosure.cc:109] aligning 360 with 519
I0114 14:40:09.401374 424832 loopclosure.cc:109] aligning 360 with 525
I0114 14:40:09.623682 424832 loopclosure.cc:109] aligning 360 with 531
I0114 14:40:09.826388 424832 loopclosure.cc:109] aligning 360 with 537
I0114 14:40:10.029390 424832 loopclosure.cc:109] aligning 360 with 543
I0114 14:40:10.236105 424832 loopclosure.cc:109] aligning 360 with 549
I0114 14:40:10.442286 424832 loopclosure.cc:109] aligning 366 with 489
I0114 14:40:10.632905 424832 loopclosure.cc:109] aligning 366 with 495
I0114 14:40:10.840906 424832 loopclosure.cc:109] aligning 366 with 501
I0114 14:40:11.037856 424832 loopclosure.cc:109] aligning 366 with 507
I0114 14:40:11.260270 424832 loopclosure.cc:109] aligning 366 with 513
I0114 14:40:11.465464 424832 loopclosure.cc:109] aligning 366 with 519
I0114 14:40:11.659193 424832 loopclosure.cc:109] aligning 366 with 525
I0114 14:40:11.875249 424832 loopclosure.cc:109] aligning 366 with 531
I0114 14:40:12.082633 424832 loopclosure.cc:109] aligning 366 with 537
I0114 14:40:12.279668 424832 loopclosure.cc:109] aligning 366 with 543
I0114 14:40:12.495205 424832 loopclosure.cc:109] aligning 372 with 483
I0114 14:40:12.722119 424832 loopclosure.cc:109] aligning 372 with 489
I0114 14:40:12.916532 424832 loopclosure.cc:109] aligning 372 with 495
I0114 14:40:13.117549 424832 loopclosure.cc:109] aligning 372 with 501
I0114 14:40:13.347931 424832 loopclosure.cc:109] aligning 372 with 507
I0114 14:40:13.552963 424832 loopclosure.cc:109] aligning 372 with 513
I0114 14:40:13.744216 424832 loopclosure.cc:109] aligning 372 with 519
I0114 14:40:13.935374 424832 loopclosure.cc:109] aligning 372 with 525
I0114 14:40:14.144537 424832 loopclosure.cc:109] aligning 372 with 531
I0114 14:40:14.345687 424832 loopclosure.cc:109] aligning 372 with 537
I0114 14:40:14.540819 424832 loopclosure.cc:109] aligning 378 with 478
I0114 14:40:14.721980 424832 loopclosure.cc:109] aligning 378 with 484
I0114 14:40:14.918220 424832 loopclosure.cc:109] aligning 378 with 490
I0114 14:40:15.098886 424832 loopclosure.cc:109] aligning 378 with 496
I0114 14:40:15.302568 424832 loopclosure.cc:109] aligning 378 with 502
I0114 14:40:15.520671 424832 loopclosure.cc:109] aligning 378 with 508
I0114 14:40:15.730944 424832 loopclosure.cc:109] aligning 378 with 514
I0114 14:40:15.933339 424832 loopclosure.cc:109] aligning 378 with 520
I0114 14:40:16.116988 424832 loopclosure.cc:109] aligning 378 with 526
I0114 14:40:16.304324 424832 loopclosure.cc:109] aligning 378 with 532
I0114 14:40:16.513626 424832 loopclosure.cc:109] aligning 384 with 484
I0114 14:40:16.713029 424832 loopclosure.cc:109] aligning 384 with 490
I0114 14:40:16.898360 424832 loopclosure.cc:109] aligning 384 with 496
I0114 14:40:17.177525 424832 loopclosure.cc:109] aligning 384 with 502
I0114 14:40:17.511034 424832 loopclosure.cc:109] aligning 384 with 508
I0114 14:40:17.715927 424832 loopclosure.cc:109] aligning 384 with 514
I0114 14:40:18.106099 424832 loopclosure.cc:109] aligning 384 with 520
I0114 14:40:18.292825 424832 loopclosure.cc:109] aligning 384 with 526
I0114 14:40:18.485371 424832 loopclosure.cc:109] aligning 390 with 490
I0114 14:40:18.665769 424832 loopclosure.cc:109] aligning 390 with 496
I0114 14:40:18.902027 424832 loopclosure.cc:109] aligning 390 with 502
I0114 14:40:19.115675 424832 loopclosure.cc:109] aligning 390 with 508
I0114 14:40:19.313446 424832 loopclosure.cc:109] aligning 390 with 514
I0114 14:40:19.656185 424832 loopclosure.cc:109] aligning 390 with 520
I0114 14:40:19.840093 424832 loopclosure.cc:109] aligning 396 with 496
I0114 14:40:20.085132 424832 loopclosure.cc:109] aligning 396 with 502
I0114 14:40:20.293792 424832 loopclosure.cc:109] aligning 396 with 508
I0114 14:40:20.501314 424832 loopclosure.cc:109] aligning 396 with 514
I0114 14:40:20.699945 424832 loopclosure.cc:109] aligning 402 with 502
I0114 14:40:20.910954 424832 loopclosure.cc:102] success: 249/272

5 轨迹可视化

将 lidar_pose_、rtk_pose_、opti_pose_1_、opti_pose_2_ 轨迹可视化：

wu@WP:~/slam_in_autonomous_driving/scripts$ python3 all_path.py /home/wu/slam_in_autonomous_driving/data/ch9/keyframes.txt 

all_path.py:10: MatplotlibDeprecationWarning: Axes3D(fig) adding itself to the figure is deprecated since 3.4. Pass the keyword argument auto_add_to_figure=False and use fig.add_axes(ax) to suppress this warning. The default value of auto_add_to_figure will change to False in mpl3.5 and True values will no longer work in 3.6.  This is consistent with other Axes classes.
  ax = Axes3D(fig

2D 轨迹：

3D 轨迹：

6 地图导出

根据 pose_source 参数选择位姿来源，可选择 lidar/rtk/opti1/opti2。原理是根据每个关键帧的位姿，将关键帧点云转到世界坐标系上，再拼接起来。

/**
 * 将keyframes.txt中的地图和点云合并为一个pcd
 */

int main(int argc, char** argv) {
    google::ParseCommandLineFlags(&argc, &argv, true);
    google::InitGoogleLogging(argv[0]);
    FLAGS_stderrthreshold = google::INFO;
    FLAGS_colorlogtostderr = true;

    using namespace sad;
    std::map<IdType, KFPtr> keyframes;
    if (!LoadKeyFrames("/home/wu/slam_in_autonomous_driving/data/ch9/keyframes.txt", keyframes)) {
        LOG(ERROR) << "failed to load keyframes.txt";
        return -1;
    }

    if (keyframes.empty()) {
        LOG(INFO) << "keyframes are empty";
        return 0;
    }

    // dump kf cloud and merge
    LOG(INFO) << "merging";
    CloudPtr global_cloud(new PointCloudType);

    pcl::VoxelGrid<PointType> voxel_grid_filter;
    float resolution = FLAGS_voxel_size;
    voxel_grid_filter.setLeafSize(resolution, resolution, resolution);

    int cnt = 0;
    for (auto& kfp : keyframes) {
        auto kf = kfp.second;
        SE3 pose;
        if (FLAGS_pose_source == "rtk") {
            pose = kf->rtk_pose_;
        } else if (FLAGS_pose_source == "lidar") {
            pose = kf->lidar_pose_;
        } else if (FLAGS_pose_source == "opti1") {
            pose = kf->opti_pose_1_;
        } else if (FLAGS_pose_source == "opti2") {
            pose = kf->opti_pose_2_;
        }

        kf->LoadScan("/home/wu/slam_in_autonomous_driving/data/ch9/");

        CloudPtr cloud_trans(new PointCloudType);
        pcl::transformPointCloud(*kf->cloud_, *cloud_trans, pose.matrix());

        // voxel size
        CloudPtr kf_cloud_voxeled(new PointCloudType);
        voxel_grid_filter.setInputCloud(cloud_trans);
        voxel_grid_filter.filter(*kf_cloud_voxeled);

        *global_cloud += *kf_cloud_voxeled;
        kf->cloud_ = nullptr;

        LOG(INFO) << "merging " << cnt << " in " << keyframes.size() << ", pts: " << kf_cloud_voxeled->size()
                  << " global pts: " << global_cloud->size();
        cnt++;
    }

    if (!global_cloud->empty()) {
        sad::SaveCloudToFile(FLAGS_dump_to + "/map.pcd", *global_cloud);
    }

    LOG(INFO) << "done.";
    return 0;
}

运行结果：

wu@WP:~/slam_in_autonomous_driving/bin$ ./dump_map --pose_source=opti2

I0114 15:08:02.963590 425971 keyframe.cc:78] Loaded kfs: 1143
I0114 15:08:02.963681 425971 dump_map.cc:42] merging
I0114 15:08:02.967100 425971 dump_map.cc:76] merging 0 in 1143, pts: 1758 global pts: 1758
I0114 15:08:02.970384 425971 dump_map.cc:76] merging 1 in 1143, pts: 1771 global pts: 3529
I0114 15:08:02.973193 425971 dump_map.cc:76] merging 2 in 1143, pts: 1524 global pts: 5053
//* 省略 *//
I0114 15:08:09.278196 425971 dump_map.cc:76] merging 1140 in 1143, pts: 2632 global pts: 3317012
I0114 15:08:09.282899 425971 dump_map.cc:76] merging 1141 in 1143, pts: 2334 global pts: 3319346
I0114 15:08:09.287400 425971 dump_map.cc:76] merging 1142 in 1143, pts: 2246 global pts: 3321592
I0114 15:08:12.711279 425971 dump_map.cc:85] done.

7 地图分块

在多数应用中，我们希望控制实时点云的载入规模，例如只加载自身周围 200 米范围内的点云，其他范围的点云则视情况卸载，这样可以控制实时系统的计算量。因此需要对大地图进行分块处理（这里按照 100m 进行切分）。

点云的切分实际上是根据每个点的坐标计算其所在的网格，然后把它投到对应的网格中去。如果我们考虑更周密一些的话，也可以把并行化、点云分批读写等行为考虑进来。

地图分块的流程是：遍历每一个关键帧。对于当前关键帧，遍历其每一个点，对每个点的 $x,y$ 坐标进行处理，判断其所属的网格并将其插入到对应的子地图块中。

这里对每个点的 $x,y$ 坐标进行如下处理：

            // floor()：向下取整。-50 是为了使包含原 (0,0) 的区域不被分开,形成以 (0,0) 为中心的块
            int gx = floor((pt.x - 50.0) / 100);
            int gy = floor((pt.y - 50.0) / 100);

int main(int argc, char** argv) {
    google::InitGoogleLogging(argv[0]);
    FLAGS_stderrthreshold = google::INFO;
    FLAGS_colorlogtostderr = true;
    google::ParseCommandLineFlags(&argc, &argv, true);

    using namespace sad;

    std::map<IdType, KFPtr> keyframes;
    if (!LoadKeyFrames("/home/wu/slam_in_autonomous_driving/data/ch9/keyframes.txt", keyframes)) {
        LOG(ERROR) << "failed to load keyframes";
        return 0;
    }

    std::map<Vec2i, CloudPtr, less_vec<2>> map_data;  // 以网格ID为索引的地图数据
    pcl::VoxelGrid<PointType> voxel_grid_filter;
    float resolution = FLAGS_voxel_size;
    voxel_grid_filter.setLeafSize(resolution, resolution, resolution);

    // 逻辑和dump map差不多，但每个点个查找它的网格ID，没有的话会创建
    for (auto& kfp : keyframes) {
        auto kf = kfp.second;
        kf->LoadScan("/home/wu/slam_in_autonomous_driving/data/ch9/");

        CloudPtr cloud_trans(new PointCloudType);
        pcl::transformPointCloud(*kf->cloud_, *cloud_trans, kf->opti_pose_2_.matrix());

        // voxel size
        CloudPtr kf_cloud_voxeled(new PointCloudType);
        voxel_grid_filter.setInputCloud(cloud_trans);
        voxel_grid_filter.filter(*kf_cloud_voxeled);

        LOG(INFO) << "building kf " << kf->id_ << " in " << keyframes.size();

        // add to grid
        for (const auto& pt : kf_cloud_voxeled->points) {
            // floor()：向下取整。-50 是为了使包含原 (0,0) 的区域不被分开,形成以 (0,0) 为中心的块
            int gx = floor((pt.x - 50.0) / 100);
            int gy = floor((pt.y - 50.0) / 100);
            Vec2i key(gx, gy);
            auto iter = map_data.find(key);
            if (iter == map_data.end()) {
                // create point cloud
                CloudPtr cloud(new PointCloudType);
                // 最后插入的是 pt，(gx, gy)只是为了分块。
                cloud->points.emplace_back(pt);
                cloud->is_dense = false;
                cloud->height = 1;
                map_data.emplace(key, cloud);
            } else {
                iter->second->points.emplace_back(pt);
            }
        }
    }

    // 存储点云和索引文件
    LOG(INFO) << "saving maps, grids: " << map_data.size();
    std::system("mkdir -p /home/wu/slam_in_autonomous_driving/data/ch9/map_data/");
    std::system("rm -rf /home/wu/slam_in_autonomous_driving/data/ch9/map_data/*");  // 清理一下文件夹
    std::ofstream fout("/home/wu/slam_in_autonomous_driving/data/ch9/map_data/map_index.txt");
    for (auto& dp : map_data) {
        fout << dp.first[0] << " " << dp.first[1] << std::endl;
        dp.second->width = dp.second->size();
        sad::VoxelGrid(dp.second, 0.1);

        sad::SaveCloudToFile(
            "/home/wu/slam_in_autonomous_driving/data/ch9/map_data/" + std::to_string(dp.first[0]) + "_" + std::to_string(dp.first[1]) + ".pcd",
            *dp.second);
    }
    fout.close();

    LOG(INFO) << "done.";
    return 0;
}

运行结果：

wu@WP:~/slam_in_autonomous_driving/bin$ ./split_map 

I0114 15:20:58.483939 426447 keyframe.cc:78] Loaded kfs: 1143
I0114 15:20:58.487437 426447 split_map.cc:52] building kf 0 in 1143
I0114 15:20:58.490739 426447 split_map.cc:52] building kf 1 in 1143
I0114 15:20:58.493566 426447 split_map.cc:52] building kf 2 in 1143
//* 省略 *//
I0114 15:21:04.844982 426447 split_map.cc:52] building kf 1140 in 1143
I0114 15:21:04.849773 426447 split_map.cc:52] building kf 1141 in 1143
I0114 15:21:04.854301 426447 split_map.cc:52] building kf 1142 in 1143
I0114 15:21:04.854329 426447 split_map.cc:76] saving maps, grids: 24
I0114 15:21:06.976038 426447 split_map.cc:91] done.

8 本章小结

本章主要向读者演示了一个完整的点云地图构建、优化、切分导出的程序，可以看到，整个建图过程是比较流程化、自动化的。这些地图可以帮助我们进行激光高精定位，让车辆和机器人在没有 RTK 的环境下获取高精度位姿。

int main(int argc, char** argv) {
    google::InitGoogleLogging(argv[0]);
    FLAGS_stderrthreshold = google::INFO;
    FLAGS_colorlogtostderr = true;
    google::ParseCommandLineFlags(&argc, &argv, true);

    LOG(INFO) << "testing frontend";
    sad::Frontend frontend(FLAGS_config_yaml);
    if (!frontend.Init()) {
        LOG(ERROR) << "failed to init frontend.";
        return -1;
    }

    frontend.Run();

    sad::Optimization opti(FLAGS_config_yaml);
    if (!opti.Init(1)) {
        LOG(ERROR) << "failed to init opti1.";
        return -1;
    }
    opti.Run();

    sad::LoopClosure lc(FLAGS_config_yaml);
    if (!lc.Init()) {
        LOG(ERROR) << "failed to init loop closure.";
        return -1;
    }
    lc.Run();

    sad::Optimization opti2(FLAGS_config_yaml);
    if (!opti2.Init(2)) {
        LOG(ERROR) << "failed to init opti2.";
        return -1;
    }
    opti2.Run();

    LOG(INFO) << "done.";
    return 0;
}