深蓝学院-多传感器融合定位学习第二章

第一次课程作业

1. 跑通提供的工程框架
2. 使用基于NDT的匹配，之后使用evo评估得到的轨迹精度
3. 实现ICP-SVD点云匹配方法，并使用evo评估得到的轨迹精度

1.跑通提供的工程框架
编译环境

catkin_make
source ./devel/setup.bash

启动front_end.launch

roslaunch lidar_localization hello_kitti.launch 

打开终端，cd到bag所在目录，输入指令

rosbag play kitti_2011_10_03_drive_0027_synced.bag

2.使用基于NDT的匹配，之后使用evo评估两种算法得到的轨迹精度
在代码框架/lidar_localization/config/front_end/config.yaml修改registration_method为NDT

# 匹配
# TODO: implement your custom registration method and add it here
registration_method: NDT   # 选择点云匹配方法，目前支持：ICP, ICP_SVD, NDT, SICP

# 局部地图
key_frame_distance: 2.0 # 关键帧距离
local_frame_num: 20
local_map_filter: voxel_filter # 选择滑窗地图点云滤波方法，目前支持：voxel_filter

# rviz显示
display_filter: voxel_filter # rviz 实时显示点云时滤波方法，目前支持：voxel_filter

# 当前帧
frame_filter: voxel_filter # 选择当前帧点云滤波方法，目前支持：voxel_filter

## 滤波相关参数
voxel_filter:
    local_map:
        leaf_size: [0.6, 0.6, 0.6]
    frame:
        leaf_size: [1.3, 1.3, 1.3]
    display:
        leaf_size: [0.5, 0.5, 0.5]

# 各配置选项对应参数
## 匹配相关参数
ICP:
    max_corr_dist : 1.2
    trans_eps : 0.01
    euc_fitness_eps : 0.36
    max_iter : 30
ICP_SVD:
    max_corr_dist : 1.2
    trans_eps : 0.01
    euc_fitness_eps : 0.36
    max_iter : 10
NDT:
    res : 1.0
    step_size : 0.1
    trans_eps : 0.01
    max_iter : 30
SICP:
    p : 1.0
    mu : 10.0
    alpha : 1.2
    max_mu : 1e5
    max_icp : 100
    max_outer : 100
    max_inner : 1
    stop : 1e-5

然后编译环境运行节点

roslaunch lidar_localization front_end.launch 
rosbag play kitti_2011_10_03_drive_0027_synced.bag

使用evo工具进行评估

evo_rpe kitti ground_truth.txt laser_odom.txt  -r  trans_part --delta 100 --plot --plot_mode xyz

3.实现ICP-SVD点云匹配方法，并使用evo计算出指标
ScanMatch 函数改写

bool ICPSVDRegistration::ScanMatch(
    const CloudData::CLOUD_PTR& input_source, 
    const Eigen::Matrix4f& predict_pose, 
    CloudData::CLOUD_PTR& result_cloud_ptr,
    Eigen::Matrix4f& result_pose
) {
    input_source_ = input_source;

    // pre-process input source:
    CloudData::CLOUD_PTR transformed_input_source(new CloudData::CLOUD());
    pcl::transformPointCloud(*input_source_, *transformed_input_source, predict_pose);

    // init estimation:
    transformation_.setIdentity();
    
    //
    // TODO: first option -- implement all computing logic on your own
    //
    // do estimation:
    int curr_iter = 0;
    while (curr_iter < max_iter_) {
        // TODO: apply current estimation:
            CloudData::CLOUD_PTR curr_input_source(new CloudData::CLOUD());
            pcl::transformPointCloud(*transformed_input_source, *curr_input_source, transformation_);
        // TODO: get correspondence:
            std::vector<Eigen::Vector3f> xs;
            std::vector<Eigen::Vector3f> ys;
        // TODO: do not have enough correspondence -- break:
            if (GetCorrespondence(curr_input_source, xs, ys) < 3)
            break;
        // TODO: update current transform:
            Eigen::Matrix4f delta_transformation;
            GetTransform(xs, ys, delta_transformation);
                // TODO: whether the transformation update is significant:
            if( ! IsSignificant(delta_transformation,trans_eps_))
                break;

            
             // TODO: update transformation:
            transformation_= delta_transformation*  transformation_;
            
            ++curr_iter;
                
      
    }

    // set output:
    result_pose = transformation_ * predict_pose;
    //归一化
    Eigen::Quaternionf qr(result_pose.block<3,3>(0,0));
    qr.normalize();
    Eigen::Vector3f t =result_pose.block<3,1>(0,3);
    result_pose.setIdentity();
    result_pose.block<3,3>(0,0) = qr.toRotationMatrix();
    result_pose.block<3,1>(0,3) = t;
    pcl::transformPointCloud(*input_source_, *result_cloud_ptr, result_pose);
    
    return true;
}

GetCorrespondence函数

size_t ICPSVDRegistration::GetCorrespondence(
    const CloudData::CLOUD_PTR &input_source, 
    std::vector<Eigen::Vector3f> &xs,
    std::vector<Eigen::Vector3f> &ys
) {
    const float MAX_CORR_DIST_SQR = max_corr_dist_ * max_corr_dist_;

    size_t num_corr = 0;

    // TODO: set up point correspondence
    for (size_t i = 0; i < input_source->points.size(); ++i) {
        std::vector<int> corr_ind;
        std::vector<float> corr_sq_dis;
        input_target_kdtree_->nearestKSearch(
            input_source->at(i), 
            1, 
            corr_ind, corr_sq_dis
        ); 
       if (corr_sq_dis.at(0) > MAX_CORR_DIST_SQR)
            continue;
        
        // add correspondence:
        Eigen::Vector3f x(
            input_target_->at(corr_ind.at(0)).x,
            input_target_->at(corr_ind.at(0)).y,
            input_target_->at(corr_ind.at(0)).z
        );
        Eigen::Vector3f y(
            input_source->at(i).x,
            input_source->at(i).y,
            input_source->at(i).z
        );

        xs.push_back(x);
        ys.push_back(y);

        ++num_corr;
    }
    return num_corr;
}

GetTransform函数

void ICPSVDRegistration::GetTransform(
    const std::vector<Eigen::Vector3f> &xs,
    const std::vector<Eigen::Vector3f> &ys,
    Eigen::Matrix4f &transformation_
) {
    const size_t N = xs.size();
   Eigen::Vector3f mu_x(0,0,0);
    Eigen::Vector3f mu_y(0,0,0);
   
   Eigen::MatrixXf A(3,N),B(3,N);
   for(auto i =0; i<N;i++)
   {
         A.block<3,1>(0,i) = xs[i];
         B.block<3,1>(0,i) = ys[i];
   }

   mu_x = A.rowwise().mean();
   mu_y = B.rowwise().mean();

   A.colwise()   -=  mu_x;
   B.colwise()   -=  mu_y;
   Eigen::Matrix3f H,R;
   Eigen::Vector3f t;
   H = B*A.transpose();
   LOG(INFO) << "H:"<<H<< std::endl;
   Eigen::JacobiSVD<Eigen::MatrixXf>svd(H,Eigen::ComputeFullU | Eigen::ComputeFullV);
   R=svd.matrixV() * svd.matrixU().transpose();
   t=mu_x -R*mu_y;
   transformation_.setZero();
   transformation_.block<3,3>(0,0)=R;
   transformation_.block<3,1>(0,3)=t;
   transformation_(3,3)=1; 
}

运行节点

roslaunch lidar_localization front_end.launch 
rosbag play kitti_2011_10_03_drive_0027_synced.bag

在slam-data文件下运行命令

evo_rpe kitti ground_truth.txt laser_odom.txt  -r  trans_part --delta 100 --plot --plot_mode xyz

evo_ape kitti ground_truth.txt laser_odom.txt -r full --plot --plot_mode xyz