scanRegistration理论部分概述
这部分主要工作实际就是点云的预处理，其中包括点云降采样（也就是减少点云数量，减少计算量），还有就算点云的分类，曲率最大的点（作为匹配点），曲率次大的点（作为曲率大点的待匹配点），曲率最小的点（作为匹配点），曲率次小的点（作为曲率最小点的匹配点）
scanRegistration过程代码详解
#include <cmath>
#include <vector>
#include <string>
#include "aloam_velodyne/common.h"
#include "aloam_velodyne/tic_toc.h"
#include <nav_msgs/Odometry.h>
#include <opencv2/imgproc.hpp>
#include "opencv2/imgcodecs/legacy/constants_c.h"
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/PointCloud2.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_broadcaster.h>
using std::atan2;
using std::cos;
using std::sin;
const double scanPeriod = 0.1;
const int systemDelay = 0;
int systemInitCount = 0;
bool systemInited = false;
int N_SCANS = 0;
float cloudCurvature[400000];
int cloudSortInd[400000];
int cloudNeighborPicked[400000];
int cloudLabel[400000];
//用于比较两个点的曲率
bool comp (int i,int j) { return (cloudCurvature[i]<cloudCurvature[j]); }
ros::Publisher pubLaserCloud;
ros::Publisher pubCornerPointsSharp;
ros::Publisher pubCornerPointsLessSharp;
ros::Publisher pubSurfPointsFlat;
ros::Publisher pubSurfPointsLessFlat;
ros::Publisher pubRemovePoints;
std::vector<ros::Publisher> pubEachScan;
bool PUB_EACH_LINE = false;
double MINIMUM_RANGE = 0.1;
//丢弃一定距离内的点
template <typename PointT>
void removeClosedPointCloud(const pcl::PointCloud<PointT> &cloud_in,
pcl::PointCloud<PointT> &cloud_out, float thres)
{
if (&cloud_in != &cloud_out)
{
cloud_out.header = cloud_in.header;
cloud_out.points.resize(cloud_in.points.size());
}
size_t j = 0;
for (size_t i = 0; i < cloud_in.points.size(); ++i)
{
if (cloud_in.points[i].x * cloud_in.points[i].x + cloud_in.points[i].y * cloud_in.points[i].y + cloud_in.points[i].z * cloud_in.points[i].z < thres * thres)
continue;
cloud_out.points[j] = cloud_in.points[i];
j++;
}
if (j != cloud_in.points.size())
{
cloud_out.points.resize(j);
}
cloud_out.height = 1;
cloud_out.width = static_cast<uint32_t>(j);
cloud_out.is_dense = true;
}
//对接受的激光数据进行处理，存储点云信息
//存储顺序是先按照垂直方向存储，然后在按照转角方向进行存储直到旋转一周完成
void laserCloudHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudMsg)
{
//如果系统没有初始化
if (!systemInited)
{
systemInitCount++;
//初始化的次数大于等待时间 进行初始化
if (systemInitCount >= systemDelay)
{
systemInited = true;
}
else
return;
}
//计时器
TicToc t_whole;
//计算曲率前预处理计时
TicToc t_prepare;
//记录每个scan有曲率的点的开始和结束索引
std::vector<int> scanStartInd(N_SCANS, 0);
std::vector<int> scanEndInd(N_SCANS, 0);
//定义一个pcl形式的输入点云
pcl::PointCloud<pcl::PointXYZ> laserCloudIn;
//把ros包的点云信息转化为pcl形式
pcl::fromROSMsg(*laserCloudMsg, laserCloudIn);
std::vector<int> indices;
//预处理
//去除无效点
//这个函数目的是去除过远点，第一个参数是输入，第二个参数是输出，
//第三个参数是列表保存输出的点在输入里的位置
//输出里的第i个点，是输入里的第indices[i]个点，就是
//cloud_out.points[i] = cloud_in.points[indices[i]]
pcl::removeNaNFromPointCloud(laserCloudIn, laserCloudIn, indices);
//去除距离过近的点
removeClosedPointCloud(laserCloudIn, laserCloudIn, MINIMUM_RANGE);
//定义点云数量的大小
int cloudSize = laserCloudIn.points.size();
//计算点云的起始角和终止角
//atan2的输出为-pi到pi(PS:atan输出为-pi/2到pi/2)
//lidar scan开始点的旋转角,atan2范围[-pi,+pi],计算旋转角时取负号是因为velodyne是顺时针旋转
float startOri = -atan2(laserCloudIn.points[0].y, laserCloudIn.points[0].x);
//lidar scan结束点的旋转角，加2*pi使点云旋转周期为2*pi
float endOri = -atan2(laserCloudIn.points[cloudSize - 1].y,
laserCloudIn.points[cloudSize - 1].x) +
2 * M_PI;
//保证角度是合理的
//用一个周期来调整终止角在合理范围
//激光间距收束到1pi到3pi
if (endOri - startOri > 3 * M_PI)
{
endOri -= 2 * M_PI;
}
else if (endOri - startOri < M_PI)
{
endOri += 2 * M_PI;
}
//printf("end Ori %f\n", endOri);
//雷达扫描线是否过半
bool halfPassed = false;
//记录总点数
int count = cloudSize;
PointType point;
//按线数保存的点云集合
std::vector<pcl::PointCloud<PointType>> laserCloudScans(N_SCANS);
//遍历所有点，提取旋转角，id，根据旋转角确定时间
for (int i = 0; i < cloudSize; i++)
{
//获得点坐标
point.x = laserCloudIn.points[i].x;
point.y = laserCloudIn.points[i].y;
point.z = laserCloudIn.points[i].z;
//计算点的仰角 为了编排垂直方向的序号即线号
float angle = atan(point.z / sqrt(point.x * point.x + point.y * point.y)) * 180 / M_PI;
int scanID = 0;
//如果雷达线数为16 32 64线一样的
if (N_SCANS == 16)
{
//定义该点的序号
scanID = int((angle + 15) / 2 + 0.5);
//如果序号大于15，或小于 0 那么舍弃这个点
if (scanID > (N_SCANS - 1) || scanID < 0)
{
count--;
continue;
}
}
else if (N_SCANS == 32)
{
scanID = int((angle + 92.0/3.0) * 3.0 / 4.0);
if (scanID > (N_SCANS - 1) || scanID < 0)
{
count--;
continue;
}
}
else if (N_SCANS == 64)
{
if (angle >= -8.83)
scanID = int((2 - angle) * 3.0 + 0.5);
else
scanID = N_SCANS / 2 + int((-8.83 - angle) * 2.0 + 0.5);
// use [0 50] > 50 remove outlies
if (angle > 2 || angle < -24.33 || scanID > 50 || scanID < 0)
{
count--;
continue;
}
}
else
{
printf("wrong scan number\n");
ROS_BREAK();
}
//printf("angle %f scanID %d \n", angle, scanID);
//计算旋转角
float ori = -atan2(point.y, point.x);
if (!halfPassed)
{
//
if (ori < startOri - M_PI / 2)
{
ori += 2 * M_PI;
}
else if (ori > startOri + M_PI * 3 / 2)
{
ori -= 2 * M_PI;
}
if (ori - startOri > M_PI)
{
halfPassed = true;
}
}
else
{
ori += 2 * M_PI;
if (ori < endOri - M_PI * 3 / 2)
{
ori += 2 * M_PI;
}
else if (ori > endOri + M_PI / 2)
{
ori -= 2 * M_PI;
}
}
//const double scanPeriod = 0.1;
//计算旋转角占总角度的比值 即点云中的相对时间
float relTime = (ori - startOri) / (endOri - startOri);
//scanID为线号， 小数点后面为相对时间
point.intensity = scanID + scanPeriod * relTime;
//按线把点存好
laserCloudScans[scanID].push_back(point);
}
cloudSize = count;
printf("points size %d \n", cloudSize);
//把所有线保存在laserCloud一个数据集合里，
//把每条线的第五个和倒数第五个位置反馈给scanStartInd和scanEndInd
pcl::PointCloud<PointType>::Ptr laserCloud(new pcl::PointCloud<PointType>());
//把点云按照线的顺序排列并保存
for (int i = 0; i < N_SCANS; i++)
{
//每组从第五个点云开始计算
scanStartInd[i] = laserCloud->size() + 5;
//点云按照线程顺序存储好
*laserCloud += laserCloudScans[i];
//每组的后五个不进行计算
scanEndInd[i] = laserCloud->size() - 6;
}
//预处理部分完成，记录一下时间
printf("prepare time %f \n", t_prepare.toc());
// 曲率公式
//由于计算中我们只是为了比较曲率大小，所以只计算了前后五个点与当前点的差，然后求其平方代替曲率计算
for (int i = 5; i < cloudSize - 5; i++)
{
使用每个点的前后五个点计算曲率，因此前五个与最后五个点跳过
float diffX = laserCloud->points[i - 5].x + laserCloud->points[i - 4].x + laserCloud->points[i - 3].x + laserCloud->points[i - 2].x + laserCloud->points[i - 1].x - 10 * laserCloud->points[i].x + laserCloud->points[i + 1].x + laserCloud->points[i + 2].x + laserCloud->points[i + 3].x + laserCloud->points[i + 4].x + laserCloud->points[i + 5].x;
float diffY = laserCloud->points[i - 5].y + laserCloud->points[i - 4].y + laserCloud->points[i - 3].y + laserCloud->points[i - 2].y + laserCloud->points[i - 1].y - 10 * laserCloud->points[i].y + laserCloud->points[i + 1].y + laserCloud->points[i + 2].y + laserCloud->points[i + 3].y + laserCloud->points[i + 4].y + laserCloud->points[i + 5].y;
float diffZ = laserCloud->points[i - 5].z + laserCloud->points[i - 4].z + laserCloud->points[i - 3].z + laserCloud->points[i - 2].z + laserCloud->points[i - 1].z - 10 * laserCloud->points[i].z + laserCloud->points[i + 1].z + laserCloud->points[i + 2].z + laserCloud->points[i + 3].z + laserCloud->points[i + 4].z + laserCloud->points[i + 5].z;
//计算曲率
cloudCurvature[i] = diffX * diffX + diffY * diffY + diffZ * diffZ;
//记录曲率索引
cloudSortInd[i] = i;
//判断能否作为i特征点
cloudNeighborPicked[i] = 0;
//记录曲率程度 大 2 小 1 平面 -1
cloudLabel[i] = 0;
}
//计时
TicToc t_pts;
//曲率很大的特征点
pcl::PointCloud<PointType> cornerPointsSharp;
//曲率稍微大的特征点
pcl::PointCloud<PointType> cornerPointsLessSharp;
//曲率很小的特征点
pcl::PointCloud<PointType> surfPointsFlat;
//曲率稍微小的特征点
pcl::PointCloud<PointType> surfPointsLessFlat;
float t_q_sort = 0;
//遍历所有线程
for (int i = 0; i < N_SCANS; i++)
{
//跳过终止角 - 起始角 < 0 的点
if( scanEndInd[i] - scanStartInd[i] < 6)
continue;
//
pcl::PointCloud<PointType>::Ptr surfPointsLessFlatScan(new pcl::PointCloud<PointType>);
//对每个扫描进行6等分
for (int j = 0; j < 6; j++)
{
//每次的提取序号范围
//起始序号
int sp = scanStartInd[i] + (scanEndInd[i] - scanStartInd[i]) * j / 6;
//结束序号 -1是避免提取到相同的点
int ep = scanStartInd[i] + (scanEndInd[i] - scanStartInd[i]) * (j + 1) / 6 - 1;
TicToc t_tmp;
// 对cloudSortInd进行排序 依据的是序号cloudSortInd[i]的点的曲率从小到大
//sort（数组的起始位置，数组的结束位置， 比较方法）
std::sort (cloudSortInd + sp, cloudSortInd + ep + 1, comp);
t_q_sort += t_tmp.toc();
//记录曲率大点的数目
int largestPickedNum = 0;
//挑选各个分段曲率很大和比较大的点
for (int k = ep; k >= sp; k--)
{
//曲率最大点的序号
int ind = cloudSortInd[k];
//筛选曲率大于0.1的点
if (cloudNeighborPicked[ind] == 0 &&
cloudCurvature[ind] > 0.1)
{
largestPickedNum++;
//当最大点的个数小于等于2的时候进行保存
if (largestPickedNum <= 2)
{
cloudLabel[ind] = 2;
cornerPointsSharp.push_back(laserCloud->points[ind]);
cornerPointsLessSharp.push_back(laserCloud->points[ind]);
}
else if (largestPickedNum <= 20)
{
cloudLabel[ind] = 1;
cornerPointsLessSharp.push_back(laserCloud->points[ind]);
}
else
{
break;
}
//筛选过的点标志 为 1 不进行下次的计算
cloudNeighborPicked[ind] = 1;
for (int l = 1; l <= 5; l++)
{
// 考虑该特征点序号前后5个点，如果有两两之间距离过于接近的点
// 则不考虑将其前一个作为下一个特征点的候选点 这是防止特征点聚集，使得特征点在每个方向上尽量分布均匀
//提取该点后五个点的xyz方向的导数
float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l - 1].x;
float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l - 1].y;
float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l - 1].z;
if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05)
{
break;
}
//计算过的点标志 1
cloudNeighborPicked[ind + l] = 1;
}
for (int l = -1; l >= -5; l--)
{
//提取该点前五个点的xyz方向的导数
float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l + 1].x;
float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l + 1].y;
float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l + 1].z;
if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05)
{
break;
}
//计算过的点标志 1
cloudNeighborPicked[ind + l] = 1;
}
}
}
//存储曲率小的数目
int smallestPickedNum = 0;
//率选曲率娇小的点，同理
for (int k = sp; k <= ep; k++)
{
int ind = cloudSortInd[k];
if (cloudNeighborPicked[ind] == 0 &&
cloudCurvature[ind] < 0.1)
{
cloudLabel[ind] = -1;
surfPointsFlat.push_back(laserCloud->points[ind]);
smallestPickedNum++;
if (smallestPickedNum >= 4)
{
break;
}
cloudNeighborPicked[ind] = 1;
for (int l = 1; l <= 5; l++)
{
float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l - 1].x;
float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l - 1].y;
float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l - 1].z;
if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05)
{
break;
}
cloudNeighborPicked[ind + l] = 1;
}
for (int l = -1; l >= -5; l--)
{
float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l + 1].x;
float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l + 1].y;
float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l + 1].z;
if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05)
{
break;
}
cloudNeighborPicked[ind + l] = 1;
}
}
}
//将剩余的点（包括之前被排除的点以及平面点 ）全部归入平面点中less flat类别中
for (int k = sp; k <= ep; k++)
{
if (cloudLabel[k] <= 0)
{
surfPointsLessFlatScan->push_back(laserCloud->points[k]);
}
}
}
//由于less flat点最多，对每个分段less flat的点进行体素栅格滤波
pcl::PointCloud<PointType> surfPointsLessFlatScanDS;
pcl::VoxelGrid<PointType> downSizeFilter;
downSizeFilter.setInputCloud(surfPointsLessFlatScan);
downSizeFilter.setLeafSize(0.2, 0.2, 0.2);
downSizeFilter.filter(surfPointsLessFlatScanDS);
//less flat点汇总
surfPointsLessFlat += surfPointsLessFlatScanDS;
}
//打印个时间
//降采样和分类的时间时间
printf("sort q time %f \n", t_q_sort);
printf("seperate points time %f \n", t_pts.toc());
//发布点云信息
sensor_msgs::PointCloud2 laserCloudOutMsg;
pcl::toROSMsg(*laserCloud, laserCloudOutMsg);
laserCloudOutMsg.header.stamp = laserCloudMsg->header.stamp;
laserCloudOutMsg.header.frame_id = "camera_init";
pubLaserCloud.publish(laserCloudOutMsg);
//发布曲率大的点云信息
sensor_msgs::PointCloud2 cornerPointsSharpMsg;
pcl::toROSMsg(cornerPointsSharp, cornerPointsSharpMsg);
cornerPointsSharpMsg.header.stamp = laserCloudMsg->header.stamp;
cornerPointsSharpMsg.header.frame_id = "camera_init";
pubCornerPointsSharp.publish(cornerPointsSharpMsg);
//发布曲率较大的点云信息
sensor_msgs::PointCloud2 cornerPointsLessSharpMsg;
pcl::toROSMsg(cornerPointsLessSharp, cornerPointsLessSharpMsg);
cornerPointsLessSharpMsg.header.stamp = laserCloudMsg->header.stamp;
cornerPointsLessSharpMsg.header.frame_id = "camera_init";
pubCornerPointsLessSharp.publish(cornerPointsLessSharpMsg);
//发布曲率小的点云信息
sensor_msgs::PointCloud2 surfPointsFlat2;
pcl::toROSMsg(surfPointsFlat, surfPointsFlat2);
surfPointsFlat2.header.stamp = laserCloudMsg->header.stamp;
surfPointsFlat2.header.frame_id = "camera_init";
pubSurfPointsFlat.publish(surfPointsFlat2);
//发布曲率较小的点云信息
sensor_msgs::PointCloud2 surfPointsLessFlat2;
pcl::toROSMsg(surfPointsLessFlat, surfPointsLessFlat2);
surfPointsLessFlat2.header.stamp = laserCloudMsg->header.stamp;
surfPointsLessFlat2.header.frame_id = "camera_init";
pubSurfPointsLessFlat.publish(surfPointsLessFlat2);
// pub each scam
//发布激光数据时间辍和序号
if(PUB_EACH_LINE)
{
for(int i = 0; i< N_SCANS; i++)
{
sensor_msgs::PointCloud2 scanMsg;
pcl::toROSMsg(laserCloudScans[i], scanMsg);
scanMsg.header.stamp = laserCloudMsg->header.stamp;
scanMsg.header.frame_id = "camera_init";
pubEachScan[i].publish(scanMsg);
}
}
//总用时
printf("scan registration time %f ms *************\n", t_whole.toc());
///匹配时间过长发出警告
if(t_whole.toc() > 100)
ROS_WARN("scan registration process over 100ms");
}
int main(int argc, char **argv)
{
//节点初始化
ros::init(argc, argv, "scanRegistration");
//定义节点句柄
ros::NodeHandle nh;
//bool类型 定义参数（名称，线数，16线）
nh.param<int>("scan_line", N_SCANS, 16);
//定义最小范围 0.1
nh.param<double>("minimum_range", MINIMUM_RANGE, 0.1);
printf("scan line number %d \n", N_SCANS);
//如果雷达的线数不对结束运行
if(N_SCANS != 16 && N_SCANS != 32 && N_SCANS != 64)
{
printf("only support velodyne with 16, 32 or 64 scan line!");
return 0;
}
//订阅激光数据 队列 100 ，回调函数
ros::Subscriber subLaserCloud = nh.subscribe<sensor_msgs::PointCloud2>("velodyne_points", 100, laserCloudHandler);
//发布点云信息 将原始点云滤波，模型化后重组成的点云
pubLaserCloud = nh.advertise<sensor_msgs::PointCloud2>("velodyne_cloud_2", 100);
//发布角点，降采样角点，面点，降采样面点
//发布大曲率特征点信息
pubCornerPointsSharp = nh.advertise<sensor_msgs::PointCloud2>("laser_cloud_sharp", 100);
//发布小曲率，包括大曲率信息
pubCornerPointsLessSharp = nh.advertise<sensor_msgs::PointCloud2>("laser_cloud_less_sharp", 100);
//发布面信息
pubSurfPointsFlat = nh.advertise<sensor_msgs::PointCloud2>("laser_cloud_flat", 100);
//发布除了角点之外全部点的信息
pubSurfPointsLessFlat = nh.advertise<sensor_msgs::PointCloud2>("laser_cloud_less_flat", 100);
//发布去除点云的信息
pubRemovePoints = nh.advertise<sensor_msgs::PointCloud2>("laser_remove_points", 100);
//检测每条发布的激光数据，并注明id存好
if(PUB_EACH_LINE)
{
for(int i = 0; i < N_SCANS; i++)
{
ros::Publisher tmp = nh.advertise<sensor_msgs::PointCloud2>("/laser_scanid_" + std::to_string(i), 100);
pubEachScan.push_back(tmp);
}
}
//循环
ros::spin();
return 0;
}
1689
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
