确定网站界面,wordpress能做小程序,zhongwen网站模板,门户网站建设和运行保障招标文件目录 1 点云建图的流程 2 前端实现 2.1 前端流程 2.2 前端结果 3 后端位姿图优化与异常值剔除 3.1 两阶段优化流程 3.2 优化结果 ① 第一阶段优化结果 ② 第二阶段优化结果 4 回环检测 4.1 回环检测流程
① 遍历第一阶段优化轨迹中的关键帧。
② 并发计算候选回环对…目录 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(); // 不再需要处理RTKRemoveMapOrigin();// 再运行LIOrosbag_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_sharedKeyframe(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_sharedKeyframe(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 系统 插值部分。
假设比例 计算公式如下其中 为待插值的时刻 为起始时刻 为结束时刻 旋转部分插值四元数球面线性插值 (SLERP)确保旋转路径在旋转空间中的弧长最短。 平移部分插值平移向量线性插值 (LERP) 注意这种去畸变的方法前提是滤波器本身有效。如果滤波器失效或位姿发散去畸变算法也就随之发散了。 void Frontend::FindGPSPose(std::shared_ptrKeyframe kf) {SE3 pose;GNSSPtr match;// pose 为插值结果if (math::PoseInterpGNSSPtr(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::mapdouble, T data, const std::functionSE3(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; // s0 时为第一帧s1时为nextSE3 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 不在车辆中心时它的平移观测需要经过转换之后才能正确作用于车体的位移。 RTK 因子在 《自动驾驶与机器人中的SLAM技术》ch4基于预积分和图优化的 GINS 中有详细的介绍。
雷达里程计因子6 维的二元边称为相对运动约束边对每个关键帧连接当前关键帧和后续 5 个关键帧。误差为两帧之间的相对运动差即 。回环因子和雷达里程计因子是同一种边对所有的回环候选关键帧对添加相对运动约束边。 相对运动约束边 残差定义 残差对状态变量的雅可比矩阵使用 g2o 默认的数值方式求得的数值形式的雅可比矩阵而不是解析形式的。相对运动约束边和 《自动驾驶与机器人中的SLAM技术》ch4预积分学 中介绍的 预积分边 很像。
/*** 6 自由度的GNSS* 误差的旋转在前平移在后*/
class EdgeGNSS : public g2o::BaseUnaryEdge6, SE3, VertexPose {public:EIGEN_MAKE_ALIGNED_OPERATOR_NEW;EdgeGNSS() default;EdgeGNSS(VertexPose* v, const SE3 obs) {setVertex(0, v);setMeasurement(obs);}// 这里使用的是 bag包或者 .txt数据其中的 GNSS 数据已经被转换到车体坐标系下了是直接对 R,p 的观测void computeError() override {// 在 setVertex() 函数中设置 void setVertex(size_t i, Vertex* v) {_vertices[i]v;}// 顺序为 p_gnss_0 或者 p_gnss_1VertexPose* v (VertexPose*)_vertices[0];_error.head3() (_measurement.so3().inverse() * v-estimate().so3()).log();_error.tail3() v-estimate().translation() - _measurement.translation();};void linearizeOplus() override {VertexPose* v (VertexPose*)_vertices[0];// jacobian 6x6_jacobianOplusXi.setZero();_jacobianOplusXi.block3, 3(0, 0) (_measurement.so3().inverse() * v-estimate().so3()).jr_inv(); // dR/dR_jacobianOplusXi.block3, 3(3, 3) Mat3d::Identity(); // dp/dp}Mat6d GetHessian() {linearizeOplus();return _jacobianOplusXi.transpose() * information() * _jacobianOplusXi;}virtual bool read(std::istream in) { return true; }virtual bool write(std::ostream out) const { return true; }private:
};/*** 只有平移的GNSS* 此时需要提供RTK外参 TBG才能正确施加约束*/
class EdgeGNSSTransOnly : public g2o::BaseUnaryEdge3, 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 {// 在 setVertex() 函数中设置 void setVertex(size_t i, Vertex* v) {_vertices[i]v;}// 顺序为 p_gnss_0 或者 p_gnss_1VertexPose* v (VertexPose*)_vertices[0];// RTK 读数为 T_WG_error (v-estimate() * TBG_).translation() - _measurement;};// 可以由用户提供雅可比矩阵的解析解或者省略 linearizeOplus() 函数使用 g2o 默认的数值方式自动计算雅可比矩阵的数值解但效率较低并且可能会引入误差// void linearizeOplus() override {// VertexPose* v (VertexPose*)_vertices[0];// // jacobian 6x6// _jacobianOplusXi.setZero();// _jacobianOplusXi.block3, 3(0, 0) (_measurement.so3().inverse() * v-estimate().so3()).jr_inv(); // dR/dR// _jacobianOplusXi.block3, 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* ch9 中使用*/
class EdgeRelativeMotion : public g2o::BaseBinaryEdge6, 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_castVertexPose*(_vertices[0]);auto* v2 static_castVertexPose*(_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 matrixfor (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 两阶段优化流程 优化分为两阶段顺序为 第一阶段优化回环检测第二阶段优化。在每阶段的优化中又分别进行了两次优化。
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].asdouble();lidar_continuous_num_ yaml[lidar_continuous_num].asint();rtk_has_rot_ yaml[rtk_has_rot].asbool();rtk_pos_noise_ yaml[rtk_pos_noise].asdouble();rtk_ang_noise_ yaml[rtk_ang_noise].asdouble() * math::kDEG2RAD;rtk_height_noise_ratio_ yaml[rtk_height_noise_ratio].asdouble();std::vectordouble rtk_ext_t yaml[rtk_ext][t].asstd::vectordouble();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::LinearSolverEigenBlockSolverType::PoseMatrixType;auto* solver new g2o::OptimizationAlgorithmLevenberg(g2o::make_uniqueBlockSolverType(g2o::make_uniqueLinearSolverType()));optimizer_.setAlgorithm(solver);AddVertices();AddRTKEdges();AddLidarEdges();AddLoopEdges();
}void Optimization::AddVertices() {for (auto kfp : keyframes_) {auto kf kfp.second;// make g2o vertex for this kfauto 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.block3, 3(0, 0) info_ang;info_all.block3, 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.block3, 3(0, 0) info_pos;info_all.block3, 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.block3, 3(0, 0) info_pos;info_all.block3, 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 只处理 level0 的边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 posestd::vectordouble 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).head2().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 优化结果 ① 第一阶段优化结果
wuWP:~/slam_in_autonomous_driving/bin$ ./run_optimization --stage1I0114 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] donewuWP:~/slam_in_autonomous_driving/data/ch9$ g2o_viewer before.g2o wuWP:~/slam_in_autonomous_driving/data/ch9$ g2o_viewer after.g2o ② 第二阶段优化结果 注意完成回环检测后才进行第二阶段优化。
wuWP:~/slam_in_autonomous_driving/bin$ ./run_optimization --stage2I0114 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
wuWP:~/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.head2().norm(); // x-y distancedouble 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::vectorLoopCandidate 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::NormalDistributionsTransformPointType, PointType ndt;ndt.setTransformationEpsilon(0.05);ndt.setStepSize(0.7);ndt.setMaximumIterations(40);// T_wjMat4f Tw2 kf2-opti_pose_1_.matrix().castfloat();/// 不同分辨率下的匹配// 多分辨率 NDT 配准CloudPtr output(new PointCloudType);std::vectordouble res{10.0, 5.0, 4.0, 3.0};for (auto r : res) {// 设置/更改体素网格的分辨率。ndt.setResolution(r);// 降采样尺寸和体素分辨率成比例变化比例因子为 0.1auto 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.castdouble();Quatd q(T.block3, 3(0, 0));q.normalize();Vec3d t T.block3, 1(0, 3);// T_ij T_wi^-1 * T_wjc.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_
wuWP:~/slam_in_autonomous_driving/bin$ ./run_loopclosureI0114 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_ 轨迹可视化
wuWP:~/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_figureFalse 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::mapIdType, 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 mergeLOG(INFO) merging;CloudPtr global_cloud(new PointCloudType);pcl::VoxelGridPointType 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 sizeCloudPtr 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;
}运行结果
wuWP:~/slam_in_autonomous_driving/bin$ ./dump_map --pose_sourceopti2I0114 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 进行切分。 点云的切分实际上是根据每个点的坐标计算其所在的网格然后把它投到对应的网格中去。如果我们考虑更周密一些的话也可以把并行化、点云分批读写等行为考虑进来。 地图分块的流程是遍历每一个关键帧。对于当前关键帧遍历其每一个点对每个点的 坐标进行处理判断其所属的网格并将其插入到对应的子地图块中。 这里对每个点的 坐标进行如下处理 // 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::mapIdType, KFPtr keyframes;if (!LoadKeyFrames(/home/wu/slam_in_autonomous_driving/data/ch9/keyframes.txt, keyframes)) {LOG(ERROR) failed to load keyframes;return 0;}std::mapVec2i, CloudPtr, less_vec2 map_data; // 以网格ID为索引的地图数据pcl::VoxelGridPointType 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 sizeCloudPtr 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 gridfor (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 cloudCloudPtr 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;
}运行结果
wuWP:~/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;
} 文章转载自： http://www.morning.gcqdp.cn.gov.cn.gcqdp.cn http://www.morning.lrplh.cn.gov.cn.lrplh.cn http://www.morning.czlzn.cn.gov.cn.czlzn.cn http://www.morning.mjzcp.cn.gov.cn.mjzcp.cn http://www.morning.ndhxn.cn.gov.cn.ndhxn.cn http://www.morning.jrpmf.cn.gov.cn.jrpmf.cn http://www.morning.ckcjq.cn.gov.cn.ckcjq.cn http://www.morning.rgdcf.cn.gov.cn.rgdcf.cn http://www.morning.lsjtq.cn.gov.cn.lsjtq.cn http://www.morning.nffwl.cn.gov.cn.nffwl.cn http://www.morning.ygpdm.cn.gov.cn.ygpdm.cn http://www.morning.tgxrm.cn.gov.cn.tgxrm.cn http://www.morning.hytqt.cn.gov.cn.hytqt.cn http://www.morning.qtryb.cn.gov.cn.qtryb.cn http://www.morning.mypxm.com.gov.cn.mypxm.com http://www.morning.bslkt.cn.gov.cn.bslkt.cn http://www.morning.csznh.cn.gov.cn.csznh.cn http://www.morning.qklff.cn.gov.cn.qklff.cn http://www.morning.kaakyy.com.gov.cn.kaakyy.com http://www.morning.hytr.cn.gov.cn.hytr.cn http://www.morning.mgnrc.cn.gov.cn.mgnrc.cn http://www.morning.lkbyj.cn.gov.cn.lkbyj.cn http://www.morning.xrrbj.cn.gov.cn.xrrbj.cn http://www.morning.kfmnf.cn.gov.cn.kfmnf.cn http://www.morning.yydeq.cn.gov.cn.yydeq.cn http://www.morning.srxhd.cn.gov.cn.srxhd.cn http://www.morning.zwzlf.cn.gov.cn.zwzlf.cn http://www.morning.rqpgk.cn.gov.cn.rqpgk.cn http://www.morning.mxgpp.cn.gov.cn.mxgpp.cn http://www.morning.mcgsq.cn.gov.cn.mcgsq.cn http://www.morning.wyfpc.cn.gov.cn.wyfpc.cn http://www.morning.ykqbs.cn.gov.cn.ykqbs.cn http://www.morning.hpcpp.cn.gov.cn.hpcpp.cn http://www.morning.yfddl.cn.gov.cn.yfddl.cn http://www.morning.ljsxg.cn.gov.cn.ljsxg.cn http://www.morning.bryyb.cn.gov.cn.bryyb.cn http://www.morning.nkkr.cn.gov.cn.nkkr.cn http://www.morning.rghkg.cn.gov.cn.rghkg.cn http://www.morning.mjpgl.cn.gov.cn.mjpgl.cn http://www.morning.gywfp.cn.gov.cn.gywfp.cn http://www.morning.ghrhb.cn.gov.cn.ghrhb.cn http://www.morning.wqnc.cn.gov.cn.wqnc.cn http://www.morning.c7507.cn.gov.cn.c7507.cn http://www.morning.zlnmm.cn.gov.cn.zlnmm.cn http://www.morning.jnzfs.cn.gov.cn.jnzfs.cn http://www.morning.cfynn.cn.gov.cn.cfynn.cn http://www.morning.kfldw.cn.gov.cn.kfldw.cn http://www.morning.dndk.cn.gov.cn.dndk.cn http://www.morning.fgrcd.cn.gov.cn.fgrcd.cn http://www.morning.rfmzc.cn.gov.cn.rfmzc.cn http://www.morning.grxsc.cn.gov.cn.grxsc.cn http://www.morning.ggfdq.cn.gov.cn.ggfdq.cn http://www.morning.nllst.cn.gov.cn.nllst.cn http://www.morning.dnzyx.cn.gov.cn.dnzyx.cn http://www.morning.gmmyn.cn.gov.cn.gmmyn.cn http://www.morning.rsszk.cn.gov.cn.rsszk.cn http://www.morning.btwlp.cn.gov.cn.btwlp.cn http://www.morning.ypfw.cn.gov.cn.ypfw.cn http://www.morning.clccg.cn.gov.cn.clccg.cn http://www.morning.tpssx.cn.gov.cn.tpssx.cn http://www.morning.cwyrp.cn.gov.cn.cwyrp.cn http://www.morning.srrzb.cn.gov.cn.srrzb.cn http://www.morning.nrlsg.cn.gov.cn.nrlsg.cn http://www.morning.ncfky.cn.gov.cn.ncfky.cn http://www.morning.nrqnj.cn.gov.cn.nrqnj.cn http://www.morning.kxltf.cn.gov.cn.kxltf.cn http://www.morning.xkhhy.cn.gov.cn.xkhhy.cn http://www.morning.flqkp.cn.gov.cn.flqkp.cn http://www.morning.dbddm.cn.gov.cn.dbddm.cn http://www.morning.fgxr.cn.gov.cn.fgxr.cn http://www.morning.fmgwx.cn.gov.cn.fmgwx.cn http://www.morning.jkmjm.cn.gov.cn.jkmjm.cn http://www.morning.zqfjn.cn.gov.cn.zqfjn.cn http://www.morning.lrzst.cn.gov.cn.lrzst.cn http://www.morning.pyncx.cn.gov.cn.pyncx.cn http://www.morning.rxwfg.cn.gov.cn.rxwfg.cn http://www.morning.dqxnd.cn.gov.cn.dqxnd.cn http://www.morning.rmqmc.cn.gov.cn.rmqmc.cn http://www.morning.bftr.cn.gov.cn.bftr.cn http://www.morning.yzfrh.cn.gov.cn.yzfrh.cn
