固态激光雷达zvision_lidar lio-sam与fast-lio2对比

 机械式激光雷达，可以根据ring提取边和面特征，固态式激光雷达实现特征检查需要的工作比较多，可以将降采样的都做为面特征进行处理。lio-sam添加固态雷达代码：

一、imageProjection.cpp

(sensor == SensorType::ZVISION) {
            pcl::moveFromROSMsg(currentCloudMsg, *tmpOusterCloudIn);
            laserCloudIn->points.resize(tmpOusterCloudIn->size());
            laserCloudIn->is_dense = tmpOusterCloudIn->is_dense;
            for (size_t i = 0; i < tmpOusterCloudIn->size(); i++) {
                auto &src = tmpOusterCloudIn->points[i];
                auto &dst = laserCloudIn->points[i];
                dst.x = src.x;
                dst.y = src.y;
                dst.z = src.z;
                dst.intensity = src.intensity;
                //dst.ring = src.ring;
                dst.time = src.t * 1e-9f; //ms
            }
        } 

 取消ring检测

if (ringFlag == 0 && sensor != SensorType::ZVISION) {
            ringFlag = -1;
            for (int i = 0; i < (int)currentCloudMsg.fields.size(); ++i) {
                if (currentCloudMsg.fields[i].name == "ring") {
                    ringFlag = 1;
                    break;
                }
            }
            if (ringFlag == -1) {
                ROS_ERROR("Point cloud ring channel not available, please configure your point cloud data!");
                ros::shutdown();
            }
        }

 取消去畸变

void projectPointCloud() {
        int cloudSize = laserCloudIn->points.size();
        fullCloud->clear();
        // range image projection
        for (int i = 0; i < cloudSize; ++i) {
            PointType thisPoint;
            thisPoint.x = laserCloudIn->points[i].x;
            thisPoint.y = laserCloudIn->points[i].y;
            thisPoint.z = laserCloudIn->points[i].z;
            thisPoint.intensity = laserCloudIn->points[i].intensity;

            float range = pointDistance(thisPoint);
            if (range < lidarMinRange || range > lidarMaxRange)
                continue;
            if (thisPoint.z < lidarLowRange) //去除地面点
                continue;

            if (sensor != SensorType::ZVISION) {
                int rowIdn = laserCloudIn->points[i].ring;
                if (rowIdn < 0 || rowIdn >= N_SCAN)
                    continue;

                if (rowIdn % downsampleRate != 0)
                    continue;

                int columnIdn = -1;
                if (sensor == SensorType::VELODYNE || sensor == SensorType::OUSTER) {
                    float horizonAngle = atan2(thisPoint.x, thisPoint.y) * 180 / M_PI;
                    static float ang_res_x = 360.0 / float(Horizon_SCAN);
                    columnIdn = -round((horizonAngle - 90.0) / ang_res_x) + Horizon_SCAN / 2;
                    if (columnIdn >= Horizon_SCAN)
                        columnIdn -= Horizon_SCAN;
                } else if (sensor == SensorType::LIVOX) {
                    columnIdn = columnIdnCountVec[rowIdn];
                    columnIdnCountVec[rowIdn] += 1;
                }

                if (columnIdn < 0 || columnIdn >= Horizon_SCAN)
                    continue;

                if (rangeMat.at<float>(rowIdn, columnIdn) != FLT_MAX)
                    continue;

                thisPoint = deskewPoint(&thisPoint, laserCloudIn->points[i].time); //去畸变

                rangeMat.at<float>(rowIdn, columnIdn) = range;

                int index = columnIdn + rowIdn * Horizon_SCAN;
            } else {
                if (i % downsampleRate != 0)
                    continue;
            }
            fullCloud->push_back(thisPoint);
        }
    }

 将所有的点广播出去

void publishClouds() {
        cloudInfo.header = cloudHeader;
        if (sensor != SensorType::ZVISION)
            cloudInfo.cloud_deskewed = publishCloud(pubExtractedCloud, extractedCloud, cloudHeader.stamp, lidarFrame);
        else
            cloudInfo.cloud_deskewed = publishCloud(pubExtractedCloud, fullCloud, cloudHeader.stamp, lidarFrame);
        pubLaserCloudInfo.publish(cloudInfo);
    }

 二、mapOptmization.cpp

订阅所有图片

if (sensor != SensorType::ZVISION)
            subCloud = nh.subscribe<lio_sam::cloud_info>("lio_sam/feature/cloud_info", 1, &mapOptimization::laserCloudInfoHandler, this, ros::TransportHints().tcpNoDelay());
        else
            subCloud = nh.subscribe<lio_sam::cloud_info>("lio_sam/deskew/cloud_info", 1, &mapOptimization::laserCloudInfoHandler, this, ros::TransportHints().tcpNoDelay());

 将点云放入面特征

void laserCloudInfoHandler(const lio_sam::cloud_infoConstPtr &msgIn) {
        // extract time stamp
        timeLaserInfoStamp = msgIn->header.stamp;
        timeLaserInfoCur = msgIn->header.stamp.toSec();

        // extract info and feature cloud
        cloudInfo = *msgIn;
        if (sensor != SensorType::ZVISION) {
            pcl::fromROSMsg(msgIn->cloud_corner, *laserCloudCornerLast);
            pcl::fromROSMsg(msgIn->cloud_surface, *laserCloudSurfLast);
        } else {
            pcl::fromROSMsg(msgIn->cloud_deskewed, *laserCloudSurfLast);
        }

 修改特征处理函数

void scan2MapOptimization() {
        if (cloudKeyPoses3D->points.empty())
            return;
        if (sensor != SensorType::ZVISION) {
            if (laserCloudSurfLastDSNum > surfFeatureMinValidNum && laserCloudCornerLastDSNum > edgeFeatureMinValidNum) {
                //kdtreeCornerFromMap->setInputCloud(laserCloudCornerFromMapDS);
                kdtreeSurfFromMap->setInputCloud(laserCloudSurfFromMapDS);

                for (int iterCount = 0; iterCount < 30; iterCount++) {
                    laserCloudOri->clear();
                    coeffSel->clear();

                    //cornerOptimization();
                    surfOptimization();

                    combineOptimizationCoeffs();

                    if (LMOptimization(iterCount) == true)
                        break;
                }

                transformUpdate();
            } else {
                ROS_WARN("Not enough features! Only %d edge and %d planar features available.", laserCloudCornerLastDSNum, laserCloudSurfLastDSNum);
            }
        } else {
            if (laserCloudSurfLastDSNum > surfFeatureMinValidNum) {
                kdtreeSurfFromMap->setInputCloud(laserCloudSurfFromMapDS);

                for (int iterCount = 0; iterCount < 30; iterCount++) {
                    laserCloudOri->clear();
                    coeffSel->clear();

                    cornerOptimization();
                    surfOptimization();

                    combineOptimizationCoeffs();

                    if (LMOptimization(iterCount) == true)
                        break;
                }

                transformUpdate();
            } else {
                ROS_WARN("Not enough features!  %d planar features available.", laserCloudSurfLastDSNum);
            }
        }
    }

 cmakelist.txt

取消特征检测函数

# Feature Association 取消特征检测
if(0)
  add_executable(${PROJECT_NAME}_featureExtraction src/featureExtraction.cpp)
  add_dependencies(${PROJECT_NAME}_featureExtraction ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_generate_messages_cpp)
  target_link_libraries(${PROJECT_NAME}_featureExtraction ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES})
endif()

 修改launch不需要运行featureExtration

    <!--<node pkg="$(arg project)" type="$(arg project)_featureExtraction"   name="$(arg project)_featureExtraction"    output="screen"     respawn="true"/>-->

 lio-sam将结果反馈到Imu预积分中，如果特征匹配较差会发散，导致imu预积分也发散。

 fast-lio2

在预处理函数中添加zvison_lidar处理函数

void Preprocess::zvision_handler(const sensor_msgs::PointCloud2::ConstPtr &msg) {
    pl_surf.clear();
    pl_corn.clear();
    pl_full.clear();
    // ROS_WARN("size of aaaaaa");
    PointCloudXYZI pl_orig;
    pcl::fromROSMsg(*msg, pl_orig);
    int plsize = pl_orig.points.size();
    // ROS_WARN("size of bbbbbb");
    if (plsize == 0) return;
    pl_surf.reserve(plsize);
    // ROS_WARN("size of cccc%d", plsize);
    for (int i = 0; i < plsize; i++) {
        PointType added_pt;

        added_pt.x = pl_orig.points[i].x;
        added_pt.y = pl_orig.points[i].y;
        added_pt.z = pl_orig.points[i].z;
        added_pt.intensity = pl_orig.points[i].intensity;
        added_pt.curvature = pl_orig.points[i].normal_x; // curvature unit: ms

        /**** 较近距离，以及较远距离点云的去除  ***/
        // ROS_WARN("size of ddd%f", top_height);
        if (added_pt.z > top_height) {
            continue;
        }

        double distance_p = added_pt.x * added_pt.x + added_pt.y * added_pt.y;
        // ROS_WARN("size of eee%f", max_blind);
        if (distance_p > (max_blind)) {
            continue;
        }
        //ROS_WARN("size of fff%f", blind);
        if (distance_p < (blind)) {
            continue;
        }

        all_point_num += 1;
        /**** 点云降采样  地面点采用更大的降采样倍率 且距离近的地面点用更大的降采样倍率 ***/
        //ROS_WARN("size of gggg%f", ground_height);
        if (added_pt.z < ground_height) {
            //ROS_WARN("size of hhhh");
            ground_point_num += 1;
            if ((i / 8) % ground_filter_num == 0) {
                pl_surf.points.push_back(added_pt);
            }

        } else {
            // ROS_WARN("size of iiiii");
            if ((i / 8) % point_filter_num == 0) { //  8个点为一个扫描周期
                pl_surf.points.push_back(added_pt);
            }
        }
    }
    //std::cout << "aaaaaaaaaaaaaaaa" << pl_surf.points.size() << std::endl;
    //ROS_WARN("size of %d", pl_surf.points.size());
}

调用雷达处理函数

void Preprocess::process(const sensor_msgs::PointCloud2::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out) {
    zvision_handler(msg);
    *pcl_out = pl_surf;
}

 去掉地面删除函数

 Eigen::Matrix3f ground_cov_, all_covariance;
            Eigen::Vector4f ground_pc_mean_, xyz_centroid;
            Eigen::VectorXf ground_singular_values_;

            pcl::PointCloud<pcl::PointXYZINormal>::Ptr ground_cloud_filtered(new pcl::PointCloud<pcl::PointXYZINormal>);
            p_pre->zvision_remove_ground(feats_undistort, ground_cloud_filtered);

            /*pcl::computeMeanAndCovarianceMatrix(*ground_cloud_filtered, ground_cov_, ground_pc_mean_);
            Eigen::JacobiSVD<Eigen::MatrixXf> svd(ground_cov_, Eigen::DecompositionOptions::ComputeFullU);
            ground_singular_values_ = svd.singularValues();
            Eigen::VectorXf normal_ = (svd.matrixU().col(2));
            Eigen::Vector3f seeds_mean = ground_pc_mean_.head<3>();
            double ang_y = acos(normal_(1) / 1.0);
            double ang_x = acos(normal_(0) / 1.0);

            ground_euler_x = ang_y - M_PI / 2.0;
            ground_euler_y = M_PI / 2.0 - ang_x;

            double d_ = (normal_.transpose() * seeds_mean);
            bool is_ground_reliable = true;
            if (abs(d_ - seeds_mean(2)) > 0.03 || seeds_mean(2) > -0.50 || seeds_mean(2) < -0.55) {
                is_ground_reliable = false;
            }

            // publish_ground(pubGround, is_ground_reliable, d_, ground_euler_x, ground_euler_y);

            pcl::compute3DCentroid(*feats_undistort, xyz_centroid);
            pcl::computeCovarianceMatrix(*feats_undistort, xyz_centroid, all_covariance);

            Eigen::EigenSolver<Eigen::Matrix3f> es(all_covariance);
            Eigen::VectorXf eigenvalues = es.eigenvalues().real();
            Eigen::VectorXf sorted_eigen;
            Eigen::VectorXi index;
            sort_vec(eigenvalues, sorted_eigen, index);

            if (sorted_eigen(2) < 1000 && log10(abs(sorted_eigen(1)) / abs(sorted_eigen(2))) > eigen_degen) {
                flag_degen = true;
            }*/

地面处理函数改为

void Preprocess::zvision_remove_ground(PointCloudXYZI::Ptr &input, PointCloudXYZI::Ptr &out_put) {
    pcl::PassThrough<pcl::PointXYZINormal> passX;

    //  直通滤波去除地面
    passX.setInputCloud(input);    //这个参数得是指针，类对象不行
    passX.setFilterFieldName("x"); //设置想在哪个坐标轴上操作
    passX.setFilterLimits(-0.5, 0.5);
    passX.setFilterLimitsNegative(false); //保留（true就是删除，false就是保留而删除此区间外的）
    passX.filter(*out_put);               //输出到结果指针

    passX.setInputCloud(out_put);  //这个参数得是指针，类对象不行
    passX.setFilterFieldName("y"); //设置想在哪个坐标轴上操作
    passX.setFilterLimits(0.3, 1.3);
    passX.setFilterLimitsNegative(false); //保留（true就是删除，false就是保留而删除此区间外的）
    passX.filter(*out_put);               //输出到结果指针

    passX.setInputCloud(out_put);  //这个参数得是指针，类对象不行
    passX.setFilterFieldName("z"); //设置想在哪个坐标轴上操作
    passX.setFilterLimits(-2, -0.0);
    passX.setFilterLimitsNegative(false); //保留（true就是删除，false就是保留而删除此区间外的）
    passX.filter(*out_put);               //输出到结果指针
}

 总结：fast-lio在固态激光雷达处理上更快，鲁棒性更好。