多传感器融合定位项目

Dong_zs

已于 2023-07-08 19:58:00 修改

阅读量250

点赞数 1

文章标签：学习

于 2023-07-08 19:42:29 首次发布

本文链接：https://blog.csdn.net/Dong_zs/article/details/131612061

版权

激光点云定位 PCL之NDT, ICP

该任务中的代码框架放在了我的github中，名为ICP_SVD（点击这里查看）
任务用于研究经典的点云匹配算法NDT以及ICP，数据来源为KITTI数据集。GNSS信号作为真值对该轨迹质量进行评估。
使用pcl自带的NDT以及ICP求解器，下面为求解器代码部分
NDT：

NDTRegistration::NDTRegistration(const YAML::Node& node)
    :ndt_ptr_(new pcl::NormalDistributionsTransform<CloudData::POINT, CloudData::POINT>()) {
    
    float res = node["res"].as<float>();
    float step_size = node["step_size"].as<float>();
    float trans_eps = node["trans_eps"].as<float>();
    int max_iter = node["max_iter"].as<int>();

    SetRegistrationParam(res, step_size, trans_eps, max_iter);
}

NDTRegistration::NDTRegistration(float res, float step_size, float trans_eps, int max_iter)
    :ndt_ptr_(new pcl::NormalDistributionsTransform<CloudData::POINT, CloudData::POINT>()) {

    SetRegistrationParam(res, step_size, trans_eps, max_iter);
}

bool NDTRegistration::SetRegistrationParam(float res, float step_size, float trans_eps, int max_iter) {
    ndt_ptr_->setResolution(res);
    ndt_ptr_->setStepSize(step_size);
    ndt_ptr_->setTransformationEpsilon(trans_eps);
    ndt_ptr_->setMaximumIterations(max_iter);

    LOG(INFO) << "NDT params:" << std::endl
              << "res: " << res << ", "
              << "step_size: " << step_size << ", "
              << "trans_eps: " << trans_eps << ", "
              << "max_iter: " << max_iter 
              << std::endl << std::endl;

    return true;
}

bool NDTRegistration::SetInputTarget(const CloudData::CLOUD_PTR& input_target) {
    ndt_ptr_->setInputTarget(input_target);

    return true;
}

bool NDTRegistration::ScanMatch(const CloudData::CLOUD_PTR& input_source, 
                                const Eigen::Matrix4f& predict_pose, 
                                CloudData::CLOUD_PTR& result_cloud_ptr,
                                Eigen::Matrix4f& result_pose) {
    ndt_ptr_->setInputSource(input_source);
    ndt_ptr_->align(*result_cloud_ptr, predict_pose);
    result_pose = ndt_ptr_->getFinalTransformation();

    return true;
}

ICP：

namespace lidar_localization {

ICPRegistration::ICPRegistration(
    const YAML::Node& node
) : icp_ptr_(new pcl::IterativeClosestPoint<CloudData::POINT, CloudData::POINT>()) {
    
    float max_corr_dist = node["max_corr_dist"].as<float>();
    float trans_eps = node["trans_eps"].as<float>();
    float euc_fitness_eps = node["euc_fitness_eps"].as<float>();
    int max_iter = node["max_iter"].as<int>();

    SetRegistrationParam(max_corr_dist, trans_eps, euc_fitness_eps, max_iter);
}

ICPRegistration::ICPRegistration(
    float max_corr_dist, 
    float trans_eps, 
    float euc_fitness_eps, 
    int max_iter
) : icp_ptr_(new pcl::IterativeClosestPoint<CloudData::POINT, CloudData::POINT>()) {

    SetRegistrationParam(max_corr_dist, trans_eps, euc_fitness_eps, max_iter);
}

bool ICPRegistration::SetRegistrationParam(
    float max_corr_dist, 
    float trans_eps, 
    float euc_fitness_eps, 
    int max_iter
) {
    icp_ptr_->setMaxCorrespondenceDistance(max_corr_dist);
    icp_ptr_->setTransformationEpsilon(trans_eps);
    icp_ptr_->setEuclideanFitnessEpsilon(euc_fitness_eps);
    icp_ptr_->setMaximumIterations(max_iter);

    LOG(INFO) << "ICP params:" << std::endl
              << "max_corr_dist: " << max_corr_dist << ", "
              << "trans_eps: " << trans_eps << ", "
              << "euc_fitness_eps: " << euc_fitness_eps << ", "
              << "max_iter: " << max_iter 
              << std::endl << std::endl;

    return true;
}

bool ICPRegistration::SetInputTarget(const CloudData::CLOUD_PTR& input_target) {
    icp_ptr_->setInputTarget(input_target);

    return true;
}

bool ICPRegistration::ScanMatch(const CloudData::CLOUD_PTR& input_source, 
                                const Eigen::Matrix4f& predict_pose, 
                                CloudData::CLOUD_PTR& result_cloud_ptr,
                                Eigen::Matrix4f& result_pose) {
    icp_ptr_->setInputSource(input_source);
    icp_ptr_->align(*result_cloud_ptr, predict_pose);
    result_pose = icp_ptr_->getFinalTransformation();

    return true;
}
}

得到了如下的结果图（经典的形状））
NDT的结果图误差，但仍能完整的跑完全程。
NDT匹配的定位结果图

而运用ICP作为里程计时刚跑没多久定位结果就直接飞了，如下图所示
PCL匹配的定位结果图

在roslaunch的终端中在某一时刻会提示出现不正常的四元数。分析定位结果误差逐渐增大的原因可能与累积误差不断增大有关。
在这里插入图片描述

为了研究ICP的svd求解方法，同时解决上述问题，我们手写ICP_SVD方法。
代码中对输入的点云以及目标点云进行k近邻搜索，确定相距最近的点云对。然后运用点云对构建H矩阵，对H进行奇异值分解，右奇异值与左奇异值的转置相乘 $R=VU^{T}$ 就是最后R的数值，而t则是与R相关的量
$t=u_{x} -Ru_{y}$
具体原因详见这里
核心部分如下所示：

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;
    std::vector<Eigen::Vector3f> xs,ys;
    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;
        size_t num_c=GetCorrespondence(curr_input_source, xs, ys);

        // TODO: do not have enough correspondence -- break:
        if(num_c<10) break;

        // TODO: update current transform:
        Eigen::Matrix4f transformation_delta;
        GetTransform(xs, ys, transformation_delta);

        // TODO: whether the transformation update is significant:
        if(!IsSignificant(transformation_delta,trans_eps_)) break;

        // TODO: update transformation:
        transformation_=transformation_delta * transformation_;

        ++curr_iter;
    }

    // set output:
    result_pose = transformation_ * predict_pose;
    Eigen::Vector3f t=result_pose.block(0,3,3,1);
    Eigen::Matrix3f R= result_pose.block(0,0,3,3);
    Eigen::Quaternionf q(R);
    q.normalize();
    Eigen::Matrix3f R_nor=q.toRotationMatrix();
    result_pose.setZero();
    result_pose.block(0,0,3,3)=R_nor;
    result_pose.block(0,3,3,1)=t;
    result_pose(3, 3)=1;
    pcl::transformPointCloud(*input_source_, *result_cloud_ptr, result_pose);
    
    return true;
}

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++){
        int K = 1;          //设置邻域点个数 K
        std::vector<int>pointIdxNKNSearch;//存储查询点近邻索引
        std::vector<float>pointNKNSquaredDistance;//存储近邻点对应平方距离
        if(input_target_kdtree_->nearestKSearch(input_source->points[i], K, pointIdxNKNSearch, pointNKNSquaredDistance)){
            if(pointNKNSquaredDistance[0]>MAX_CORR_DIST_SQR)
               continue;
            Eigen::Vector3f x(input_target_->points[pointIdxNKNSearch[0]].x,
                              input_target_->points[pointIdxNKNSearch[0]].y,
                              input_target_->points[pointIdxNKNSearch[0]].z);
            Eigen::Vector3f y(input_source->points[i].x,
                              input_source->points[i].y,
                              input_source->points[i].z);
            xs.push_back(x);
            ys.push_back(y);
            num_corr++;
        };
    }
    
    return num_corr;
}

void ICPSVDRegistration::GetTransform(
    const std::vector<Eigen::Vector3f> &xs,
    const std::vector<Eigen::Vector3f> &ys,
    Eigen::Matrix4f &transformation_
) {
    const size_t N = xs.size();

    // TODO: find centroids of mu_x and mu_y:
    Eigen::Vector3f mu_x=Eigen::Vector3f::Zero();
    Eigen::Vector3f mu_y=Eigen::Vector3f::Zero();
    for(size_t i=0;i<N;i++){
        mu_x+=xs[i];
        mu_y+=ys[i];
    }
    mu_x= mu_x/N;
    mu_y= mu_y/N;
    // TODO: build H:
    Eigen::Matrix3f H=Eigen::Matrix3f::Zero(3,3);
    for(size_t i=0;i<N;i++){
        H+=(ys[i]-mu_y)*(xs[i]-mu_x).transpose();
    }
    // TODO: solve R:
    Eigen::JacobiSVD<Eigen::Matrix3f> svd(H, Eigen::ComputeFullU | Eigen::ComputeFullV);
    Eigen::Matrix3f U = svd.matrixU();
    Eigen::Matrix3f V = svd.matrixV();
    Eigen::Matrix3f R = V*U.transpose();
    // TODO: solve t:
    Eigen::Vector3f t ;
    t = mu_x-R*mu_y;
    // TODO: set output:
    transformation_.setZero();
    transformation_.block(0,0,3,3)=R;
    transformation_.block(0,3,3,1)=t;
    transformation_(3, 3)=1;
    // transformation_.block(3,3)=1;
}

bool ICPSVDRegistration::IsSignificant(
    const Eigen::Matrix4f &transformation,
    const float trans_eps
) {
    // a. translation magnitude -- norm:
    float translation_magnitude = transformation.block<3, 1>(0, 3).norm();
    // b. rotation magnitude -- angle:
    float rotation_magnitude = fabs(
        acos(
            (transformation.block<3, 3>(0, 0).trace() - 1.0f) / 2.0f
        )
    );

    return (
        (translation_magnitude > trans_eps) || 
        (rotation_magnitude > trans_eps)
    );
}

注意上述代码中如下这一部分，考虑到T变换矩阵不断的右乘或者左乘可能会引起误差的提高，我们将已经计算完成的3*4的result_pose的R部分进行了归一化处理，归一化后的旋转效果并不受影响，只消除了尺度，这也抑制了旋转带来的误差的积累。

// set output:
    result_pose = transformation_ * predict_pose;
    Eigen::Vector3f t=result_pose.block(0,3,3,1);
    Eigen::Matrix3f R= result_pose.block(0,0,3,3);
    Eigen::Quaternionf q(R);
    q.normalize();
    Eigen::Matrix3f R_nor=q.toRotationMatrix();
    result_pose.setZero();
    result_pose.block(0,0,3,3)=R_nor;
    result_pose.block(0,3,3,1)=t;
    result_pose(3, 3)=1;
    pcl::transformPointCloud(*input_source_, *result_cloud_ptr, result_pose);

得到的轨迹图如下在这里插入图片描述
我们上述三种方法记录的轨迹点进行过评估，用到了evo；
终端进入到trajectory的目录，键入如下命令：（ground_truth.txt，laser_odom.txt为文件名）

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

图像如下：


ndt_raw	ndt_map


icp_raw	icp_map


icp_svd_raw	icp_svd_map

终于到了最重要的部分，数据对比；可以看到对位姿的旋转部分进行归一化处理之后，轨迹指标得到了全方位的提升，ICP_SVD的RPE优势尤其明显，大部分指标都小于1m。APE方面由于z轴方向误差的增大，ICP_SVD与NDT结果相近，但相较于ICP仍有不小的提升。

	RPE
	max	mean	median	min	rmse	sse	std
ICP	26.743375	4.859037	2.511602	0.376490	8.439967	854.796490	6.900928
NDT	9.80749	1.015497	0.827607	0.072813	1.710677	131.688748	1.376656
ICP_SVD	2.160243	0.887395	0.893749	0.184464	0.965237	41.925727	0.379753

	APE
	max	mean	median	min	rmse	sse	std
ICP	43.1769441	19.248585	15.665473	0.000001	23.410757	658772.357058	13.324996
NDT	34.399995	16.350463	15.550614	0.000002	17.89298	1452560.22303	7.267812
ICP_SVD	33.197926	14.996930	13.012221	0.000002	17.001314	1312551.889061	8.008543

更新中。。。

Dong_zs

关注

1
点赞
踩
0

收藏

觉得还不错? 一键收藏
1
评论
多传感器融合定位项目

注意上述代码中如下这一部分，考虑到T变换矩阵不断的右乘或者左乘可能会引起误差的提高，我们将已经计算完成的3*4的result_pose的R部分进行了归一化处理，归一化后的旋转效果并不受影响，只消除了尺度，这也抑制了旋转带来的误差的积累。可以看到对位姿的旋转部分进行归一化处理之后，轨迹指标得到了全方位的提升，ICP_SVD的RPE优势尤其明显，大部分指标都。APE方面由于z轴方向误差的增大，ICP_SVD与NDT结果相近，但相较于ICP仍有不小的提升。NDT的结果图误差，但仍能完整的跑完全程。
复制链接

扫一扫