深蓝学院-多传感器融合定位-第4章作业

一. 及格部分
- 代码跑通
二. 良好部分
三. 优秀部分
- Scan Context
- GNSS牛刀小试
- GNSS再来一遍

一. 及格部分

代码跑通

直接编译，跑通：
catkin config --install && catkin build

二. 良好部分

从原点开始初始化定位, 这个在代码中已经实现了，直接跑就行。

可是跑一下就会飞，估计是因为原始的Rotation也没有（就一个Identity Matrix啥都没…），匹配不上。

不过只要跑就可以良好，所以咱们在第三题解决这个初始化问题。

三. 优秀部分

Scan Context

(为了理解这个SC方法，我写了一篇博客¹来分析它的原理)
简单来说，Scan Context会直接通过与全局的Scan Context Matrix进行Two-phase search来找到最有可能re-localization的位置的pose（包括R, t）。在咱们作业里，直接一句代码就可以调用：

bool MatchingFlow::UpdateMatching() {if (!matching_ptr_->HasInited()) {matching_ptr_->SetScanContextPose(current_cloud_data_); }return matching_ptr_->Update(current_cloud_data_, laser_odometry_);
}

Fig. 0s初始化

Fig. 100s初始化

（鉴于老师说不能用Scan Context，我就没继续尝试200s 300s了，但估计效果都可以)

只是这里有个问题，Odom Trajectory是右上角绿色那佗，是正常的。

小红薯: 但是, GNSS轨迹就还是以(0,0)为起点在走，而不是100s的位置开始走，所以左边黄色那根轨迹，平移了好远！
小红薯: 这个我就不知道怎么设置GNSS的轨迹也重新按100s 200s的点为起点了…
大师兄: 其实，这个就是咱们下一题GNSS要解决的问题！

GNSS牛刀小试

先尝试直接一句话来调用GNSS，我想，既然Scan Context直接找到当前pose就行，那我直接一句话把当前gnss赋过去就行了呗：

matching_ptr_->SetGNSSPose(current_gnss_data_.pose);

但是这样貌似不行，因为这个current_gnss_data.pose还需要减去原点的坐标值才是真正的相对坐标。于是我print出0s初始化时的current_gnss_data.pose，根据其中t的数值，把代码改成了:

Eigen::Matrix4f init_pose = current_gnss_data_.pose;
init_pose(0,3) -= 0.681433;
init_pose(1,3) -= 0.552625;
init_pose(2,3) -= 0.791128;
std::cout << "init pose: " << std::endl;
std::cout << init_pose << std::endl;
matching_ptr_->SetGNSSPose(init_pose);

减去了0s的点，结果还是不如人意，还是“旋转，跳跃，我闭上眼～”：
怎么回事呢？于是我和成功的ScanContext版本比较了下200s时候print出来的pose，发现一个问题：

大家能够发现，上面GNSS matrix的 R 和下面SC matrix的 R 差不多，但是这个 t 怎么差了这么多！！！！怪不得始终不管怎么减去0s原点的初始值，它还是在0s原点附近开始初始化而不是200s位置开始初始化。。。

因而，我定位了待解决问题：

GNSS为什么和实际Scan Context得到的pose的 t 差别这么大？要经过怎样的预处理吗？

这个100s的pose减去0s的pose的方法，是不是能够正确得到相对坐标呢？

GNSS再来一遍

大师兄：上面两个问题困扰了老纳许久～
大师兄：终于，经过@何大仙的指点，发现了问题的关键！

问题的关键就在于：GNSS是经纬度，而这里的pose是将第一个GNSS作为原点，转换后的xyz坐标！

不论是100s, 200s, 300s的GNSS经纬度过来，都会被先转换成xyz坐标。而且第一个传过来的经纬度，转换后会接近于零点坐标(0,0,0)，难怪不管哪个时刻进行初始化current_gnss.pose的 t 都趋近于0（如上图结果所示）！！！

那么现在要做的，就是把0s时刻的第一帧GNSS经纬度永远作为原点，而不是把每次rosbag 播放后传入的第一帧经纬度作为原点！

首先调用SetGNSSPose():

bool MatchingFlow::UpdateMatching() {if (!matching_ptr_->HasInited()) {matching_ptr_->SetGNSSPose(current_gnss_data_.pose);        // matching_ptr_->SetScanContextPose(current_cloud_data_);}return matching_ptr_->Update(current_cloud_data_, laser_odometry_);
}

然后修改gnss_data.cpp文件，fix初始化点的经纬度：

//静态成员变量必须在类外初始化
double lidar_localization::GNSSData::origin_longitude = 0.0;
double lidar_localization::GNSSData::origin_latitude = 0.0;
double lidar_localization::GNSSData::origin_altitude = 0.0;
bool lidar_localization::GNSSData::origin_position_inited = false;
GeographicLib::LocalCartesian lidar_localization::GNSSData::geo_converter;namespace lidar_localization {void GNSSData::InitOriginPosition() {std::cout << "GNSS Data: " << std::endl;std::cout << "Latitude: " << latitude << std::endl;std::cout << "Longitude: " << longitude <<  std::endl;std::cout << "Altitude: " << altitude << std::endl << std::endl;geo_converter.Reset(48.9827, 8.39046, 116.396);origin_longitude = latitude;origin_latitude = longitude;origin_altitude = altitude;std::cout << "origin_GNSS Data: " << std::endl;std::cout << "Latitude: " << origin_longitude << std::endl;std::cout << "Longitude: " << origin_latitude <<  std::endl;std::cout << "Altitude: " << origin_altitude << std::endl;origin_position_inited = true;
}

小红薯：大师兄这里的这三个奇奇怪怪的数值是哪里来的？地里冒出来的么？
大师兄：当然不是！这里的48.9827等三个数值是print出的rosbag从0s开始run的经纬度数值。咱们以后永远把这个经纬度设置为原点。

修改之后，效果完美：

参考文献:
[1] 深蓝学院多传感器融合课程 - 第四章

Scan Context 论文详解 - 用途：Lidar SLAM 回环检测、空间描述符 ↩︎

深蓝学院-多传感器融合定位-第4章作业相关推荐

深蓝学院-多传感器融合定位-第7章作业
深蓝学院-多传感器融合定位-第7章作业 1. 及格题 2. 良好题 Parameter - verison 1 Parameter - verison 2 Parameter - verison 3 ...
深蓝学院-多传感器融合定位课程-第2章-3D激光里程计I
专栏文章: 深蓝学院-多传感器融合定位课程-第1章-概述_goldqiu的博客-CSDN博客 github保存了相关代码和学习笔记: Shenlan-Course-Multi-Sensor-Fusio ...
深蓝学院-多传感器融合定位课程-第5章-惯性导航原理及误差分析
专栏文章: 深蓝学院-多传感器融合定位课程-第1章-概述_goldqiu的博客-CSDN博客深蓝学院-多传感器融合定位课程-第2章-3D激光里程计I_goldqiu的博客-CSDN博客深蓝学院-多 ...
多传感器融合定位第四章点云地图构建及基于点云地图定位
多传感器融合定位第四章点云地图构建及基于点云地图定位代码下载 https://github.com/kahowang/sensor-fusion-for-localization-and-map ...
多传感器融合定位第六章惯性导航结算及误差模型
多传感器融合定位第六章惯性导航结算及误差模型参考博客:深蓝学院-多传感器融合定位-第6章作业代码下载:https://github.com/kahowang/sensor-fusion-for ...
看深蓝学院多传感器融合课程的笔记
贝叶斯推断和贝叶斯公式是整个卡尔曼的核心. 贝叶斯滤波比卡尔曼滤波比卡尔曼滤波更宽泛. 误差作为状态量融合工程师很多时候是调参工程师,卡尔曼滤波的Q R,需要调参掌握观测方程的推导.13章第一 ...
多传感器融合定位第八章基于滤波的融合方法进阶
多传感器融合定位第八章基于滤波的融合方法进阶参考博客:深蓝学院-多传感器融合定位-第8章作业代码下载:https://github.com/kahowang/sensor-fusion-for ...
多传感器融合定位ch1
多传感器融合定位ch1 目录多传感器融合定位ch1 1 数据集下载 2 数据集测试深蓝学院多传感器融合定位课程笔记, 采用的软硬件环境如下硬件: 华擎DeskMini X300W迷你主机软件: ...
多传感器融合定位三-3D激光里程计其三：点云畸变补偿
多传感器融合定位三-3D激光里程计其三:点云畸变补偿 1. 产生原因 2. 补偿方法 Reference: 深蓝学院-多传感器融合多传感器融合定位理论基础文章跳转: 多传感器融合定位一-3D激光里 ...
多传感器融合定位五-点云地图构建及定位
多传感器融合定位五-点云地图构建及定位 1. 回环检测 1.1 基于Scan Context 1.2 基于直方图 2. 后端优化 2.1 后端优化基本原理 2.2 李群.李代数基本知识 2.3 李群. ...

深蓝学院-多传感器融合定位-第4章作业

深蓝学院-多传感器融合定位-第4章作业

一. 及格部分

代码跑通

二. 良好部分

三. 优秀部分

Scan Context

GNSS牛刀小试

GNSS再来一遍

深蓝学院-多传感器融合定位-第4章作业相关推荐

最新文章

热门文章