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

一. 及格部分

代码跑通

直接编译，跑通：
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位置开始初始化。。。

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

1. GNSS为什么和实际Scan Context得到的pose的 t 差别这么大？要经过怎样的预处理吗？
2. 这个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] 深蓝学院多传感器融合课程 - 第四章

---

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