机械式激光雷达，可以根据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在固态激光雷达处理上更快，鲁棒性更好。
