Scanregister.cpp
对点云进行预处理和得到特征点(角点/平面)集合。
从main函数开始:
Main函数主要实现2个功能：订阅2个节点和发布6个节点。通过回调函数的处理，将处理后的点云重新发出去。
两个回调函数imuHandler(高频)和laserCloudHandler(低频)，每获得一团激光点云，只知道它的初始时刻，根据点云的相对方位角计算出每个点云的相对时间(点云分组，16层，每层6个区)，在imu数据中获取到对应的时间戳和速度，加速度信息，并转换到sweep k 的初始imu坐标系下。
int main(int argc, char** argv)
{
ros::init(argc, argv, "scanRegistration");
/*
* NodeHandle 是节点同ROS系统交流的主要接口
* NodeHandle 在构造的时候会完整地初始化本节点
* NodeHandle 析构的时候会关闭此节点
*/
ros::NodeHandle nh;
/*
* 参数1：话题名称
* 参数2：信息队列长度
* 参数3：回调函数，每当一个信息到来的时候，这个函数会被调用
* 返回一个ros::Subscriber类的对象，当此对象的所有引用都被销毁是，本节点将不再是该话题的订阅者
*/
// 订阅了velodyne_points和imu/data
ros::Subscriber subLaserCloud = nh.subscribe<sensor_msgs::PointCloud2>
("/velodyne_points", 2, laserCloudHandler);
ros::Subscriber subImu = nh.subscribe<sensor_msgs::Imu> ("/imu/data", 50, imuHandler);
/*
* 我们通过advertise() 函数指定我们如何在给定的topic上发布信息
* 它会触发对ROS master的调用，master会记录话题发布者和订阅者
* 在advertise()函数执行之后，master会通知每一个订阅此话题的节点
* 两节点间由此可以建立直接的联系
* advertise()会返回一个Publisher对象，使用这个对象的publish方法我们就可以在此话题上发布信息
* 当返回的Publisher对象的所有引用都被销毁的时候，本节点将不再是该话题的发布者
* 此函数是一个带模板的函数，需要传入具体的类型进行实例化
* 传入的类型就是要发布的信息的类型，在这里是String
* 第一个参数是话题名称
* 第二个参数是信息队列的长度，相当于信息的一个缓冲区
* 在我们发布信息的速度大于处理信息的速度时
* 信息会被缓存在先进先出的信息队列里
*/
// 发布了6个话题：velodyne_cloud_2、laser_cloud_sharp、laser_cloud_flat、laser_cloud_less_flat、laser_cloud_less_sharp、imu_trans
pubLaserCloud = nh.advertise<sensor_msgs::PointCloud2>
("/velodyne_cloud_2", 2);
pubCornerPointsSharp = nh.advertise<sensor_msgs::PointCloud2>
("/laser_cloud_sharp", 2);
pubCornerPointsLessSharp = nh.advertise<sensor_msgs::PointCloud2>
("/laser_cloud_less_sharp", 2);
pubSurfPointsFlat = nh.advertise<sensor_msgs::PointCloud2>
("/laser_cloud_flat", 2);
pubSurfPointsLessFlat = nh.advertise<sensor_msgs::PointCloud2>
("/laser_cloud_less_flat", 2);
pubImuTrans = nh.advertise<sensor_msgs::PointCloud2> ("/imu_trans", 5);
/**
* 它可以保证你指定的回调函数会被调用
* 程序执行到spin()后就不调用其他语句了
*/
ros::spin();
return 0;
}imuHandler
回调函数imuHandler，实现imu信息的解析。
imu数组是一个长度为200的循环写入的数组。
1. 减去重力对imu的影响
2. 解析出当前时刻的imu时间戳，角度以及各个轴的加速度
3. 将加速度转换到世界坐标系轴下
4. 进行航距推算，S1=S0+V0*t+0.5*a0*t^2(当前imu点和下一个时刻的imu点的时间内，假定为匀速运动推算出当前时刻的位置)
5. 推算当前时刻的速度信息，V1=V0+a*t
void imuHandler(const sensor_msgs::Imu::ConstPtr& imuIn)
{
double roll, pitch, yaw;
tf::Quaternion orientation;
tf::quaternionMsgToTF(imuIn->orientation, orientation);
tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);
// 减去重力加速度的影响
float accX = imuIn->linear_acceleration.y - sin(roll) * cos(pitch) * 9.81;
float accY = imuIn->linear_acceleration.z - cos(roll) * cos(pitch) * 9.81;
float accZ = imuIn->linear_acceleration.x + sin(pitch) * 9.81;
// 表明数组是一个长度为imuQueLength的循环数组
imuPointerLast = (imuPointerLast + 1) % imuQueLength;
imuTime[imuPointerLast] = imuIn->header.stamp.toSec();
imuRoll[imuPointerLast] = roll;
imuPitch[imuPointerLast] = pitch;
imuYaw[imuPointerLast] = yaw;
imuAccX[imuPointerLast] = accX;
imuAccY[imuPointerLast] = accY;
imuAccZ[imuPointerLast] = accZ;
AccumulateIMUShift();
}
void AccumulateIMUShift()
{
float roll = imuRoll[imuPointerLast];
float pitch = imuPitch[imuPointerLast];
float yaw = imuYaw[imuPointerLast];
float accX = imuAccX[imuPointerLast];
float accY = imuAccY[imuPointerLast];
float accZ = imuAccZ[imuPointerLast];
// 转换到世界坐标系下
float x1 = cos(roll) * accX - sin(roll) * accY;
float y1 = sin(roll) * accX + cos(roll) * accY;
float z1 = accZ;
float x2 = x1;
float y2 = cos(pitch) * y1 - sin(pitch) * z1;
float z2 = sin(pitch) * y1 + cos(pitch) * z1;
accX = cos(yaw) * x2 + sin(yaw) * z2;
accY = y2;
accZ = -sin(yaw) * x2 + cos(yaw) * z2;
// imuPointerBack 为 imuPointerLast-1， 这样处理是为了防止内存溢出
int imuPointerBack = (imuPointerLast + imuQueLength - 1) % imuQueLength;
double timeDiff = imuTime[imuPointerLast] - imuTime[imuPointerBack];
if (timeDiff < scanPeriod) {
// 推算当前时刻的位置信息
imuShiftX[imuPointerLast] = imuShiftX[imuPointerBack] + imuVeloX[imuPointerBack] * timeDiff
+ accX * timeDiff * timeDiff / 2;
imuShiftY[imuPointerLast] = imuShiftY[imuPointerBack] + imuVeloY[imuPointerBack] * timeDiff
+ accY * timeDiff * timeDiff / 2;
imuShiftZ[imuPointerLast] = imuShiftZ[imuPointerBack] + imuVeloZ[imuPointerBack] * timeDiff
+ accZ * timeDiff * timeDiff / 2;
// 推算当前时刻的速度信息
imuVeloX[imuPointerLast] = imuVeloX[imuPointerBack] + accX * timeDiff;
imuVeloY[imuPointerLast] = imuVeloY[imuPointerBack] + accY * timeDiff;
imuVeloZ[imuPointerLast] = imuVeloZ[imuPointerBack] + accZ * timeDiff;
}
}
laserCloudHandler
- 点云分组——将点云点按照时间顺序，压入16个数组中，插入imu的数据；
- 特征提取——提取点云中的特征角点/特征平面。这部分按照论文中的描述来处理：
void laserCloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg)
{
if (!systemInited) {
systemInitCount++;
if (systemInitCount >= systemDelay) {
// systemDelay 有延时作用，保证有imu数据后在调用laserCloudHandle
systemInited = true;
}
return;
}
std::vector<int> scanStartInd(N_SCANS, 0);
std::vector<int> scanEndInd(N_SCANS, 0);
// Lidar的时间戳
double timeScanCur = laserCloudMsg->header.stamp.toSec();
pcl::PointCloud<pcl::PointXYZ> laserCloudIn;
// fromROSmsg(input,cloud1) 转为为模板点云laserCloudIn
pcl::fromROSMsg(*laserCloudMsg, laserCloudIn);
std::vector<int> indices;
pcl::removeNaNFromPointCloud(laserCloudIn, laserCloudIn, indices);
int cloudSize = laserCloudIn.points.size();
//计算点云的起始角度/终止角度
float startOri = -atan2(laserCloudIn.points[0].y, laserCloudIn.points[0].x);
float endOri = -atan2(laserCloudIn.points[cloudSize - 1].y,
laserCloudIn.points[cloudSize - 1].x) + 2 * M_PI;
if (endOri - startOri > 3 * M_PI) {
endOri -= 2 * M_PI;
} else if (endOri - startOri < M_PI) {
endOri += 2 * M_PI;
}
bool halfPassed = false;
int count = cloudSize;
PointType point;
std::vector<pcl::PointCloud<PointType> > laserCloudScans(N_SCANS);
/*——————————————————————————————— 第一部分 点云分类 —————————————————————————————————*/
// 这个处理是深坑！
for (int i = 0; i < cloudSize; i++) {
point.x = laserCloudIn.points[i].y;
point.y = laserCloudIn.points[i].z;
point.z = laserCloudIn.points[i].x;
float angle = atan(point.y / sqrt(point.x * point.x + point.z * point.z)) * 180 / M_PI;
int scanID;
int roundedAngle = int(angle + (angle<0.0?-0.5:+0.5));
if (roundedAngle > 0){
scanID = roundedAngle;
}
else {
// 角度大于0，由小到大划入偶数线0-16；角度小于0，由大到小划入奇数线15-1
scanID = roundedAngle + (N_SCANS - 1);
}
if (scanID > (N_SCANS - 1) || scanID < 0 ){
// 不在16线附近的点作为杂点进行剔除
count--;
continue;
}
float ori = -atan2(point.x, point.z);
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;
}
}
//scanPeriod=0.1，是因为lidar工作周期是10HZ，意味着转一圈是0.1秒；intensity是一个整数+小数，小数不会超过0.1，完成了按照时间排序的需求
float relTime = (ori - startOri) / (endOri - startOri);
point.intensity = scanID + scanPeriod * relTime;
// imuPointerLast 是当前点，变量只在imu中改变，设为t时刻
// 对每一个cloud point处理
if (imuPointerLast >= 0) {
float pointTime = relTime * scanPeriod;
while (imuPointerFront != imuPointerLast) {
// (timeScanCur + pointTime)设为ti时刻；imuPointerFront 是 ti后一个时刻
if (timeScanCur + pointTime < imuTime[imuPointerFront]) {
break;
}
imuPointerFront = (imuPointerFront + 1) % imuQueLength;
}
if (timeScanCur + pointTime > imuTime[imuPointerFront]) {
// 这个的意思是 imuPointerFront=imuPointerLast时候
imuRollCur = imuRoll[imuPointerFront];
imuPitchCur = imuPitch[imuPointerFront];
imuYawCur = imuYaw[imuPointerFront];
imuVeloXCur = imuVeloX[imuPointerFront];
imuVeloYCur = imuVeloY[imuPointerFront];
imuVeloZCur = imuVeloZ[imuPointerFront];
imuShiftXCur = imuShiftX[imuPointerFront];
imuShiftYCur = imuShiftY[imuPointerFront];
imuShiftZCur = imuShiftZ[imuPointerFront];
} else {
// imuPointerBack = imuPointerFront - 1 插值求解出当前点对应的imu角度，位移和速度
int imuPointerBack = (imuPointerFront + imuQueLength - 1) % imuQueLength;
float ratioFront = (timeScanCur + pointTime - imuTime[imuPointerBack])
/ (imuTime[imuPointerFront] - imuTime[imuPointerBack]);
float ratioBack = (imuTime[imuPointerFront] - timeScanCur - pointTime)
/ (imuTime[imuPointerFront] - imuTime[imuPointerBack]);
imuRollCur = imuRoll[imuPointerFront] * ratioFront + imuRoll[imuPointerBack] * ratioBack;
imuPitchCur = imuPitch[imuPointerFront] * ratioFront + imuPitch[imuPointerBack] * ratioBack;
if (imuYaw[imuPointerFront] - imuYaw[imuPointerBack] > M_PI) {
imuYawCur = imuYaw[imuPointerFront] * ratioFront + (imuYaw[imuPointerBack] + 2 * M_PI) * ratioBack;
} else if (imuYaw[imuPointerFront] - imuYaw[imuPointerBack] < -M_PI) {
imuYawCur = imuYaw[imuPointerFront] * ratioFront + (imuYaw[imuPointerBack] - 2 * M_PI) * ratioBack;
} else {
imuYawCur = imuYaw[imuPointerFront] * ratioFront + imuYaw[imuPointerBack] * ratioBack;
}
imuVeloXCur = imuVeloX[imuPointerFront] * ratioFront + imuVeloX[imuPointerBack] * ratioBack;
imuVeloYCur = imuVeloY[imuPointerFront] * ratioFront + imuVeloY[imuPointerBack] * ratioBack;
imuVeloZCur = imuVeloZ[imuPointerFront] * ratioFront + imuVeloZ[imuPointerBack] * ratioBack;
imuShiftXCur = imuShiftX[imuPointerFront] * ratioFront + imuShiftX[imuPointerBack] * ratioBack;
imuShiftYCur = imuShiftY[imuPointerFront] * ratioFront + imuShiftY[imuPointerBack] * ratioBack;
imuShiftZCur = imuShiftZ[imuPointerFront] * ratioFront + imuShiftZ[imuPointerBack] * ratioBack;
}
if (i == 0) {
imuRollStart = imuRollCur;
imuPitchStart = imuPitchCur;
imuYawStart = imuYawCur;
imuVeloXStart = imuVeloXCur;
imuVeloYStart = imuVeloYCur;
imuVeloZStart = imuVeloZCur;
imuShiftXStart = imuShiftXCur;
imuShiftYStart = imuShiftYCur;
imuShiftZStart = imuShiftZCur;
} else {
// 这一部分不理解具体原理，只能pass了
// 计算Lidar的位移的
ShiftToStartIMU(pointTime);
// 将Lidar运动速度转到IMU起始坐标系下
VeloToStartIMU();
// 将点坐标转到起始IMU坐标系下
TransformToStartIMU(&point);
}
}
// 将点按照每一层线，分类压入16个数组中
laserCloudScans[scanID].push_back(point);
}
/*——————————————————————————————— 第二部分 提取特征点 —————————————————————————————————*/
cloudSize = count;
pcl::PointCloud<PointType>::Ptr laserCloud(new pcl::PointCloud<PointType>());
for (int i = 0; i < N_SCANS; i++) {
*laserCloud += laserCloudScans[i];
}
int scanCount = -1;
/*————————————————————— 记录点云的曲率 & 记录每一层曲率数组的起始和终止 ————————————————————————*/
for (int i = 5; i < cloudSize - 5; i++) {
// 对所有的激光点一个一个求出在该点前后5个点(10点)的偏差，作为cloudCurvature点云数据的曲率
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->fpoints[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;
cloudNeighborPicked[i] = 0;
cloudLabel[i] = 0;
if (int(laserCloud->points[i].intensity) != scanCount) {
scanCount = int(laserCloud->points[i].intensity);
// 记录每一层起始点和终止点的位置——[4]&[end-4]。为什么这么存储呢？因为后续需要根据这个起始/终止来操作点云曲率，在求曲率的过程中已经去除了前5个点和后5个点
if (scanCount > 0 && scanCount < N_SCANS) {
scanStartInd[scanCount] = i + 5;
scanEndInd[scanCount - 1] = i - 5;
}
}
}
scanStartInd[0] = 5;
scanEndInd.back() = cloudSize - 5;
for (int i = 5; i < cloudSize - 6; i++) {
float diffX = laserCloud->points[i + 1].x - laserCloud->points[i].x;
float diffY = laserCloud->points[i + 1].y - laserCloud->points[i].y;
float diffZ = laserCloud->points[i + 1].z - laserCloud->points[i].z;
float diff = diffX * diffX + diffY * diffY + diffZ * diffZ;
if (diff > 0.1) {
float depth1 = sqrt(laserCloud->points[i].x * laserCloud->points[i].x +
laserCloud->points[i].y * laserCloud->points[i].y +
laserCloud->points[i].z * laserCloud->points[i].z);
float depth2 = sqrt(laserCloud->points[i + 1].x * laserCloud->points[i + 1].x +
laserCloud->points[i + 1].y * laserCloud->points[i + 1].y +
laserCloud->points[i + 1].z * laserCloud->points[i + 1].z);
/*— 针对论文的(b)情况，两向量夹角小于某阈值b时（夹角小就可能存在遮挡），将其一侧的临近6个点设为不可标记为特征点的点 —*/
/*— 然后构建了一个等腰三角形的底向量，根据等腰三角形性质，判断X[i]向量与X[i+1]的夹角小于5.732度(threshold=0.1) —*/
/*— depth1>depth2 X[i+1]距离更近，远侧点标记不特征；depth1<depth2 X[i]距离更近，远侧点标记不特征 —*/
if (depth1 > depth2) {
diffX = laserCloud->points[i + 1].x - laserCloud->points[i].x * depth2 / depth1;
diffY = laserCloud->points[i + 1].y - laserCloud->points[i].y * depth2 / depth1;
diffZ = laserCloud->points[i + 1].z - laserCloud->points[i].z * depth2 / depth1;
if (sqrt(diffX * diffX + diffY * diffY + diffZ * diffZ) / depth2 < 0.1) {
cloudNeighborPicked[i - 5] = 1;
cloudNeighborPicked[i - 4] = 1;
cloudNeighborPicked[i - 3] = 1;
cloudNeighborPicked[i - 2] = 1;
cloudNeighborPicked[i - 1] = 1;
cloudNeighborPicked[i] = 1;
}
} else {
diffX = laserCloud->points[i + 1].x * depth1 / depth2 - laserCloud->points[i].x;
diffY = laserCloud->points[i + 1].y * depth1 / depth2 - laserCloud->points[i].y;
diffZ = laserCloud->points[i + 1].z * depth1 / depth2 - laserCloud->points[i].z;
if (sqrt(diffX * diffX + diffY * diffY + diffZ * diffZ) / depth1 < 0.1) {
cloudNeighborPicked[i + 1] = 1;
cloudNeighborPicked[i + 2] = 1;
cloudNeighborPicked[i + 3] = 1;
cloudNeighborPicked[i + 4] = 1;
cloudNeighborPicked[i + 5] = 1;
cloudNeighborPicked[i + 6] = 1;
}
}
}
/*— 针对论文的(a)情况，当某点及其后点间的距离平方大于某阈值a（说明这两点有一定距离） ———*/
/*— 若某点到其前后两点的距离均大于c倍的该点深度，则该点判定为不可标记特征点的点 ———————*/
/*—（入射角越小，点间距越大，即激光发射方向与投射到的平面越近似水平） ———————————————*/
float diffX2 = laserCloud->points[i].x - laserCloud->points[i - 1].x;
float diffY2 = laserCloud->points[i].y - laserCloud->points[i - 1].y;
float diffZ2 = laserCloud->points[i].z - laserCloud->points[i - 1].z;
float diff2 = diffX2 * diffX2 + diffY2 * diffY2 + diffZ2 * diffZ2;
float dis = laserCloud->points[i].x * laserCloud->points[i].x
+ laserCloud->points[i].y * laserCloud->points[i].y
+ laserCloud->points[i].z * laserCloud->points[i].z;
if (diff > 0.0002 * dis && diff2 > 0.0002 * dis) {
cloudNeighborPicked[i] = 1;
}
}
/*————————————————————— 记录点云的特征角点 ————————————————————————*/
pcl::PointCloud<PointType> cornerPointsSharp;
pcl::PointCloud<PointType> cornerPointsLessSharp;
pcl::PointCloud<PointType> surfPointsFlat;
pcl::PointCloud<PointType> surfPointsLessFlat;
for (int i = 0; i < N_SCANS; i++) {
/*—— 对于每一层激光点(总16层)，将每层区域分成6份，起始位置为sp，终止位置为ep。——————*/
/*—— 有两个循环，作用是对cloudCurvature从小到大进行排序，cloudSortedInd是它的索引数组 ————*/
pcl::PointCloud<PointType>::Ptr surfPointsLessFlatScan(new pcl::PointCloud<PointType>);
for (int j = 0; j < 6; j++) {
int sp = (scanStartInd[i] * (6 - j) + scanEndInd[i] * j) / 6;
int ep = (scanStartInd[i] * (5 - j) + scanEndInd[i] * (j + 1)) / 6 - 1;
for (int k = sp + 1; k <= ep; k++) {
for (int l = k; l >= sp + 1; l--) {
if (cloudCurvature[cloudSortInd[l]] < cloudCurvature[cloudSortInd[l - 1]]) {
int temp = cloudSortInd[l - 1];
cloudSortInd[l - 1] = cloudSortInd[l];
cloudSortInd[l] = temp;
}
}
}
/*—— 筛选特征角点 Corner: label=2; LessCorner: label=1 ————*/
int largestPickedNum = 0;
for (int k = ep; k >= sp; k--) {
int ind = cloudSortInd[k];
if (cloudNeighborPicked[ind] == 0 &&
cloudCurvature[ind] > 0.1) {
largestPickedNum++;
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;
}
// 遍历该曲率点后，将该点标记，并将该曲率点附近的前后5个点标记不被选取为特征点
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;
}
}
}
/*—— 筛选特征平面点 Flat: label=-1 普通点和Flat点降采样形成LessFlat: label=0 ————*/
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;
}
}
}
// surfPointsLessFlat为降采样后的flat点，采样前包含太多label=0的点
for (int k = sp; k <= ep; k++) {
if (cloudLabel[k] <= 0) {
surfPointsLessFlatScan->push_back(laserCloud->points[k]);
}
}
}
pcl::PointCloud<PointType> surfPointsLessFlatScanDS;
pcl::VoxelGrid<PointType> downSizeFilter;
downSizeFilter.setInputCloud(surfPointsLessFlatScan);
downSizeFilter.setLeafSize(0.2, 0.2, 0.2);
downSizeFilter.filter(surfPointsLessFlatScanDS);
surfPointsLessFlat += surfPointsLessFlatScanDS;
}
/*———— 发布信息 ——————*/
sensor_msgs::PointCloud2 laserCloudOutMsg;
pcl::toROSMsg(*laserCloud, laserCloudOutMsg);
laserCloudOutMsg.header.stamp = laserCloudMsg->header.stamp;
laserCloudOutMsg.header.frame_id = "/camera";
pubLaserCloud.publish(laserCloudOutMsg);
sensor_msgs::PointCloud2 cornerPointsSharpMsg;
pcl::toROSMsg(cornerPointsSharp, cornerPointsSharpMsg);
cornerPointsSharpMsg.header.stamp = laserCloudMsg->header.stamp;
cornerPointsSharpMsg.header.frame_id = "/camera";
pubCornerPointsSharp.publish(cornerPointsSharpMsg);
sensor_msgs::PointCloud2 cornerPointsLessSharpMsg;
pcl::toROSMsg(cornerPointsLessSharp, cornerPointsLessSharpMsg);
cornerPointsLessSharpMsg.header.stamp = laserCloudMsg->header.stamp;
cornerPointsLessSharpMsg.header.frame_id = "/camera";
pubCornerPointsLessSharp.publish(cornerPointsLessSharpMsg);
sensor_msgs::PointCloud2 surfPointsFlat2;
pcl::toROSMsg(surfPointsFlat, surfPointsFlat2);
surfPointsFlat2.header.stamp = laserCloudMsg->header.stamp;
surfPointsFlat2.header.frame_id = "/camera";
pubSurfPointsFlat.publish(surfPointsFlat2);
sensor_msgs::PointCloud2 surfPointsLessFlat2;
pcl::toROSMsg(surfPointsLessFlat, surfPointsLessFlat2);
surfPointsLessFlat2.header.stamp = laserCloudMsg->header.stamp;
surfPointsLessFlat2.header.frame_id = "/camera";
pubSurfPointsLessFlat.publish(surfPointsLessFlat2);
/*—————— imu数据发布 ————————*/
pcl::PointCloud<pcl::PointXYZ> imuTrans(4, 1);
imuTrans.points[0].x = imuPitchStart;
imuTrans.points[0].y = imuYawStart;
imuTrans.points[0].z = imuRollStart;
// imuPitchCur是imuPitchEnd 因此此时指针已指向末尾
imuTrans.points[1].x = imuPitchCur;
imuTrans.points[1].y = imuYawCur;
imuTrans.points[1].z = imuRollCur;
imuTrans.points[2].x = imuShiftFromStartXCur;
imuTrans.points[2].y = imuShiftFromStartYCur;
imuTrans.points[2].z = imuShiftFromStartZCur;
imuTrans.points[3].x = imuVeloFromStartXCur;
imuTrans.points[3].y = imuVeloFromStartYCur;
imuTrans.points[3].z = imuVeloFromStartZCur;
sensor_msgs::PointCloud2 imuTransMsg;
pcl::toROSMsg(imuTrans, imuTransMsg);
imuTransMsg.header.stamp = laserCloudMsg->header.stamp;
imuTransMsg.header.frame_id = "/camera";
pubImuTrans.publish(imuTransMsg);
}