ALOAM试跑及程序注释
ALOAM是秦通对LOAM的一个简化版本，没有IMU的信息，算是入手激光SLAM非常简单的程序了
代码：
https://github.com/HKUST-Aerial-Robotics/A-LOAM
数据：
链接: https://pan.baidu.com/s/1GaZ2eGZdfc-cluSc-bkQng 提取码: 9zsp
graph：
效果：
注释：
scanRegistration.cpp:
#include
#include
#include
#include "aloam_velodyne/common.h"
#include "aloam_velodyne/tic_toc.h"
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
using std::atan2;
using std::cos;
using std::sin;
//扫描周期, velodyne频率10Hz，周期0.1s
const double scanPeriod = 0.1;
//弃用前systemDelay帧初始数据
const int systemDelay = 0;
//systemInitCount用于计数过了多少帧
//超过systemDelay后，systemInited为true即初始化完成
int systemInitCount = 0;
bool systemInited = false;
//激光雷达线数初始化为0
int N_SCANS = 0;
//点云曲率, 400000为一帧点云中点的最大数量
float cloudCurvature[400000];
//曲率点对应的序号
int cloudSortInd[400000];
//点是否筛选过标志：0-未筛选过，1-筛选过
int cloudNeighborPicked[400000];
//点分类标号:2-代表曲率很大，1-代表曲率比较大,-1-代表曲率很小，0-曲率比较小(其中1包含了2,0包含了1,0和1构成了点云全部的点)
int cloudLabel[400000];
//两点曲率比较
bool comp (int i,int j) { return (cloudCurvature[i] pubEachScan;
//是否发布每行Scan
bool PUB_EACH_LINE = false;
//根据距离去除过远的点，距离的参数
double MINIMUM_RANGE = 0.1;
//过远点去除 使用template进行兼容
template
void removeClosedPointCloud(const pcl::PointCloud &cloud_in,
pcl::PointCloud &cloud_out, float thres)
{
//统一header(时间戳)和size
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++;
}
//有点被剔除时，size改变
if (j != cloud_in.points.size())
{
cloud_out.points.resize(j);
}
//数据行数，默认1为无组织的数据
cloud_out.height = 1;
//可以理解成点数
cloud_out.width = static_cast(j);
//点数是否有限
cloud_out.is_dense = true;
}
//订阅点云句柄
void laserCloudHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudMsg)
{
//是否剔除前systemDelay帧
if (!systemInited)
{
systemInitCount++;
if (systemInitCount >= systemDelay)
{
systemInited = true;
}
else
return;
}
//registration计时
TicToc t_whole;
//计算曲率前的预处理计时
TicToc t_prepare;
//记录每个scan有曲率的点的开始和结束索引
std::vector scanStartInd(N_SCANS, 0);
std::vector scanEndInd(N_SCANS, 0);
//命名一个pcl形式的输入点云
pcl::PointCloud laserCloudIn;
//把ros包的点云转化为pcl形式
pcl::fromROSMsg(*laserCloudMsg, laserCloudIn);
std::vector indices;
//这个函数目的是去除过远点，第一个参数是输入，第二个参数是输出，第三个参数是列表保存输出的点在输入里的位置
//输出里的第i个点，是输入里的第indices[i]个点，就是
//cloud_out.points[i] = cloud_in.points[indices[i]]
pcl::removeNaNFromPointCloud(laserCloudIn, laserCloudIn, indices);
//引用上文作者写的去除函数
removeClosedPointCloud(laserCloudIn, laserCloudIn, MINIMUM_RANGE);
//该次scan的点数
int cloudSize = laserCloudIn.points.size();
//每次扫描是一条线，看作者的数据集激光x向前，y向左，那么下面就是线一端到另一端
//atan2的输出为-pi到pi(PS:atan输出为-pi/2到pi/2)
//计算旋转角时取负号是因为velodyne是顺时针旋转
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;
//激光间距收束到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> laserCloudScans(N_SCANS);
//循环对每个点进行以下操作
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;
//求仰角atan输出为-pi/2到pi/2，实际看scanID应该每条线之间差距是2度
float angle = atan(point.z / sqrt(point.x * point.x + point.y * point.y)) * 180 / M_PI;
int scanID = 0;
//根据不同线数使用不同参数对每个点对应的第几根激光线进行判断
if (N_SCANS == 16)
{
scanID = int((angle + 15) / 2 + 0.5);
//如果判定线在16以上或是负数则忽视该点回到上面for循环
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;
}
}
//只有16,32,64线
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)
{
//确保-pi/2 < ori - startOri < 3*pi/2
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;
}
}
//确保-3*pi/2 < ori - endOri < pi/2
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;
}
}
//看看旋转多少了，记录比例relTime
float relTime = (ori - startOri) / (endOri - startOri);
//第几根线和本线进度到多少记录在point.intensity
point.intensity = scanID + scanPeriod * relTime;
//按线分类保存
laserCloudScans[scanID].push_back(point);
}
//
cloudSize = count;
printf("points size %d \n", cloudSize);
//也就是把所有线保存在laserCloud一个数据集合里，把每条线的第五个和倒数第五个位置反馈给scanStartInd和scanEndInd
pcl::PointCloud::Ptr laserCloud(new pcl::PointCloud());
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;
cloudNeighborPicked[i] = 0;
cloudLabel[i] = 0;
}
//计时用
TicToc t_pts;
//角点，降采样角点，面点，降采样面点
pcl::PointCloud cornerPointsSharp;
pcl::PointCloud cornerPointsLessSharp;
pcl::PointCloud surfPointsFlat;
pcl::PointCloud surfPointsLessFlat;
float t_q_sort = 0;
for (int i = 0; i < N_SCANS; i++)
{
//点数小于6就退出
if( scanEndInd[i] - scanStartInd[i] < 6)
continue;
pcl::PointCloud::Ptr surfPointsLessFlatScan(new pcl::PointCloud);
//将每个scan的曲率点分成6等份处理,确保周围都有点被选作特征点,或者说每两行都有
for (int j = 0; j < 6; j++)
{
//六等份起点：sp = scanStartInd + (scanEndInd - scanStartInd)*j/6
//六等份终点：ep = scanStartInd - 1 + (scanEndInd - scanStartInd)*(j+1)/6
int sp = scanStartInd[i] + (scanEndInd[i] - scanStartInd[i]) * j / 6;
int ep = scanStartInd[i] + (scanEndInd[i] - scanStartInd[i]) * (j + 1) / 6 - 1;
TicToc t_tmp;
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，并且曲率较大
if (cloudNeighborPicked[ind] == 0 &&
cloudCurvature[ind] > 0.1)
{
//则曲率大的点记数一下
largestPickedNum++;
//少于等于两个（但如果有更多的这俩cloudLabel[ind] = 2;就不更新了）
if (largestPickedNum <= 2)
{
cloudLabel[ind] = 2;
cornerPointsSharp.push_back(laserCloud->points[ind]);
cornerPointsLessSharp.push_back(laserCloud->points[ind]);
}
//保存20个角点
else if (largestPickedNum <= 20)
{
cloudLabel[ind] = 1;
cornerPointsLessSharp.push_back(laserCloud->points[ind]);
}
//多了就不要了
else
{
break;
}
//标志位一改
cloudNeighborPicked[ind] = 1;
//将曲率比较大的点的前后各5个连续距离比较近的点筛选出去，防止特征点聚集，使得特征点在每个方向上尽量分布均匀
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;
}
}
}
//挑选每个分段的曲率很小比较小的点
int smallestPickedNum = 0;
for (int k = sp; k <= ep; k++)
{
int ind = cloudSortInd[k];
//如果曲率的确比较小，并且未被筛选出
if (cloudNeighborPicked[ind] == 0 &&
cloudCurvature[ind] < 0.1)
{
//-1代表曲率很小的点
cloudLabel[ind] = -1;
surfPointsFlat.push_back(laserCloud->points[ind]);
//只选最小的四个，剩下的Label==0,就都是曲率比较小的
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 surfPointsLessFlatScanDS;
pcl::VoxelGrid 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 scan
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;
//参数，线数
nh.param("scan_line", N_SCANS, 16);
//参数，过远去除
nh.param("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;
}
//接收激光雷达信号
ros::Subscriber subLaserCloud = nh.subscribe("/velodyne_points", 100, laserCloudHandler);
//发布laserCloud，laserCloud是按线堆栈的全部点云
pubLaserCloud = nh.advertise("/velodyne_cloud_2", 100);
//发布角点，降采样角点，面点，降采样面点
pubCornerPointsSharp = nh.advertise("/laser_cloud_sharp", 100);
pubCornerPointsLessSharp = nh.advertise("/laser_cloud_less_sharp", 100);
pubSurfPointsFlat = nh.advertise("/laser_cloud_flat", 100);
pubSurfPointsLessFlat = nh.advertise("/laser_cloud_less_flat", 100);
//发布去除点
pubRemovePoints = nh.advertise("/laser_remove_points", 100);
//发布每行scan
if(PUB_EACH_LINE)
{
for(int i = 0; i < N_SCANS; i++)
{
ros::Publisher tmp = nh.advertise("/laser_scanid_" + std::to_string(i), 100);
pubEachScan.push_back(tmp);
}
}
ros::spin();
return 0;
} laser.cpp:
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include "aloam_velodyne/common.h"
#include "aloam_velodyne/tic_toc.h"
#include "lidarFactor.hpp"
#define DISTORTION 0
int corner_correspondence = 0, plane_correspondence = 0;
//扫描周期
constexpr double SCAN_PERIOD = 0.1;
//后面要进行距离比较的参数
constexpr double DISTANCE_SQ_THRESHOLD = 25;
//找点进行匹配优化时的线数距离(13线-10线>2.5就break介样用)
constexpr double NEARBY_SCAN = 2.5;
//多少Frame向mapping发送数据，实际由于主函数效果，是4帧一发
int skipFrameNum = 5;
//目的是在订阅发布，时间戳，互斥锁初始化后输出一下Initialization finished
bool systemInited = false;
//时间戳
double timeCornerPointsSharp = 0;
double timeCornerPointsLessSharp = 0;
double timeSurfPointsFlat = 0;
double timeSurfPointsLessFlat = 0;
double timeLaserCloudFullRes = 0;
//关于上一帧的KD树
pcl::KdTreeFLANN::Ptr kdtreeCornerLast(new pcl::KdTreeFLANN());
pcl::KdTreeFLANN::Ptr kdtreeSurfLast(new pcl::KdTreeFLANN());
//pcl保存形式的输入，角点，降采样角点，面点，降采样面点，上一帧角点，上一帧面点，全部点
pcl::PointCloud::Ptr cornerPointsSharp(new pcl::PointCloud());
pcl::PointCloud::Ptr cornerPointsLessSharp(new pcl::PointCloud());
pcl::PointCloud::Ptr surfPointsFlat(new pcl::PointCloud());
pcl::PointCloud::Ptr surfPointsLessFlat(new pcl::PointCloud());
pcl::PointCloud::Ptr laserCloudCornerLast(new pcl::PointCloud());
pcl::PointCloud::Ptr laserCloudSurfLast(new pcl::PointCloud());
pcl::PointCloud::Ptr laserCloudFullRes(new pcl::PointCloud());
//存储上一帧的特征点数量
int laserCloudCornerLastNum = 0;
int laserCloudSurfLastNum = 0;
// Transformation from current frame to world frame
Eigen::Quaterniond q_w_curr(1, 0, 0, 0);
Eigen::Vector3d t_w_curr(0, 0, 0);
// q_curr_last(x, y, z, w), t_curr_last
double para_q[4] = {0, 0, 0, 1};
double para_t[3] = {0, 0, 0};
//四元数Q，上帧到这帧
Eigen::Map q_last_curr(para_q);
//位移量t
Eigen::Map t_last_curr(para_t);
//定义ros格式的订阅内容
std::queue cornerSharpBuf;
std::queue cornerLessSharpBuf;
std::queue surfFlatBuf;
std::queue surfLessFlatBuf;
std::queue fullPointsBuf;
//互斥锁，让订阅信息按次序进行
std::mutex mBuf;
// undistort lidar point
//这个函数可以理解成利用变换矩阵推算一个点的位置
void TransformToStart(PointType const *const pi, PointType *const po)
{
//interpolation ratio
//用s求解某个点在本次scan中在的比例
//intensity是线号
double s;
if (DISTORTION)
s = (pi->intensity - int(pi->intensity)) / SCAN_PERIOD;
else
s = 1.0;
//s = 1;
//再根据比例求解变换矩阵的变换比例，再求出推理位姿
Eigen::Quaterniond q_point_last = Eigen::Quaterniond::Identity().slerp(s, q_last_curr);
Eigen::Vector3d t_point_last = s * t_last_curr;
Eigen::Vector3d point(pi->x, pi->y, pi->z);
Eigen::Vector3d un_point = q_point_last * point + t_point_last;
//输出一下
po->x = un_point.x();
po->y = un_point.y();
po->z = un_point.z();
po->intensity = pi->intensity;
}
// transform all lidar points to the start of the next frame
//算是求一个点一帧时间前的位置
//intensity是线号
void TransformToEnd(PointType const *const pi, PointType *const po)
{
// undistort point first
pcl::PointXYZI un_point_tmp;
TransformToStart(pi, &un_point_tmp);
Eigen::Vector3d un_point(un_point_tmp.x, un_point_tmp.y, un_point_tmp.z);
Eigen::Vector3d point_end = q_last_curr.inverse() * (un_point - t_last_curr);
po->x = point_end.x();
po->y = point_end.y();
po->z = point_end.z();
//Remove distortion time info
po->intensity = int(pi->intensity);
}
//订阅信息并且锁死，保证不乱序
void laserCloudSharpHandler(const sensor_msgs::PointCloud2ConstPtr &cornerPointsSharp2)
{
mBuf.lock();
cornerSharpBuf.push(cornerPointsSharp2);
mBuf.unlock();
}
void laserCloudLessSharpHandler(const sensor_msgs::PointCloud2ConstPtr &cornerPointsLessSharp2)
{
mBuf.lock();
cornerLessSharpBuf.push(cornerPointsLessSharp2);
mBuf.unlock();
}
void laserCloudFlatHandler(const sensor_msgs::PointCloud2ConstPtr &surfPointsFlat2)
{
mBuf.lock();
surfFlatBuf.push(surfPointsFlat2);
mBuf.unlock();
}
void laserCloudLessFlatHandler(const sensor_msgs::PointCloud2ConstPtr &surfPointsLessFlat2)
{
mBuf.lock();
surfLessFlatBuf.push(surfPointsLessFlat2);
mBuf.unlock();
}
//receive all point cloud
void laserCloudFullResHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudFullRes2)
{
mBuf.lock();
fullPointsBuf.push(laserCloudFullRes2);
mBuf.unlock();
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "laserOdometry");
ros::NodeHandle nh;
nh.param("mapping_skip_frame", skipFrameNum, 2);
printf("Mapping %d Hz \n", 10 / skipFrameNum);
ros::Subscriber subCornerPointsSharp = nh.subscribe("/laser_cloud_sharp", 100, laserCloudSharpHandler);
ros::Subscriber subCornerPointsLessSharp = nh.subscribe("/laser_cloud_less_sharp", 100, laserCloudLessSharpHandler);
ros::Subscriber subSurfPointsFlat = nh.subscribe("/laser_cloud_flat", 100, laserCloudFlatHandler);
ros::Subscriber subSurfPointsLessFlat = nh.subscribe("/laser_cloud_less_flat", 100, laserCloudLessFlatHandler);
ros::Subscriber subLaserCloudFullRes = nh.subscribe("/velodyne_cloud_2", 100, laserCloudFullResHandler);
ros::Publisher pubLaserCloudCornerLast = nh.advertise("/laser_cloud_corner_last", 100);
ros::Publisher pubLaserCloudSurfLast = nh.advertise("/laser_cloud_surf_last", 100);
ros::Publisher pubLaserCloudFullRes = nh.advertise("/velodyne_cloud_3", 100);
//pubLaserOdometry包括当前帧四元数Q和位置t
ros::Publisher pubLaserOdometry = nh.advertise("/laser_odom_to_init", 100);
//pubLaserPath包含当前帧的位置t
ros::Publisher pubLaserPath = nh.advertise("/laser_odom_path", 100);
//定义路径，用于保存帧的位置，发布于pubLaserPath
nav_msgs::Path laserPath;
//用于计算处理的帧数，每有skipFrameNum个帧处理了，就向mapping发数据
int frameCount = 0;
//设置一下ros频率
ros::Rate rate(100);
while (ros::ok())
{
//到达这里启动数据节点与ros::spin不同，到达ros::spin程序不再向下运行，只按频率进行节点，这里会继续向下
ros::spinOnce();
//如果订阅的东西应有尽有
if (!cornerSharpBuf.empty() && !cornerLessSharpBuf.empty() &&
!surfFlatBuf.empty() && !surfLessFlatBuf.empty() &&
!fullPointsBuf.empty())
{
//给时间戳
timeCornerPointsSharp = cornerSharpBuf.front()->header.stamp.toSec();
timeCornerPointsLessSharp = cornerLessSharpBuf.front()->header.stamp.toSec();
timeSurfPointsFlat = surfFlatBuf.front()->header.stamp.toSec();
timeSurfPointsLessFlat = surfLessFlatBuf.front()->header.stamp.toSec();
timeLaserCloudFullRes = fullPointsBuf.front()->header.stamp.toSec();
//时间不同就不同步报错
if (timeCornerPointsSharp != timeLaserCloudFullRes ||
timeCornerPointsLessSharp != timeLaserCloudFullRes ||
timeSurfPointsFlat != timeLaserCloudFullRes ||
timeSurfPointsLessFlat != timeLaserCloudFullRes)
{
printf("unsync messeage!");
ROS_BREAK();
}
//发布信息转换为ros格式
mBuf.lock();
cornerPointsSharp->clear();
pcl::fromROSMsg(*cornerSharpBuf.front(), *cornerPointsSharp);
cornerSharpBuf.pop();
cornerPointsLessSharp->clear();
pcl::fromROSMsg(*cornerLessSharpBuf.front(), *cornerPointsLessSharp);
cornerLessSharpBuf.pop();
surfPointsFlat->clear();
pcl::fromROSMsg(*surfFlatBuf.front(), *surfPointsFlat);
surfFlatBuf.pop();
surfPointsLessFlat->clear();
pcl::fromROSMsg(*surfLessFlatBuf.front(), *surfPointsLessFlat);
surfLessFlatBuf.pop();
laserCloudFullRes->clear();
pcl::fromROSMsg(*fullPointsBuf.front(), *laserCloudFullRes);
fullPointsBuf.pop();
mBuf.unlock();
TicToc t_whole;
// initializing输出
if (!systemInited)
{
systemInited = true;
std::cout << "Initialization finished \n";
}
else
{
//记录点数
int cornerPointsSharpNum = cornerPointsSharp->points.size();
int surfPointsFlatNum = surfPointsFlat->points.size();
TicToc t_opt;
//优化两次
for (size_t opti_counter = 0; opti_counter < 2; ++opti_counter)
{
//匹配数量
corner_correspondence = 0;
plane_correspondence = 0;
ceres::LossFunction *loss_function = new ceres::HuberLoss(0.1);
//为Eigen的表示实现四元数局部参数
//输入顺序为[w，x，y，z]
ceres::LocalParameterization *q_parameterization =
new ceres::EigenQuaternionParameterization();
ceres::Problem::Options problem_options;
ceres::Problem problem(problem_options);
problem.AddParameterBlock(para_q, 4, q_parameterization);
problem.AddParameterBlock(para_t, 3);
pcl::PointXYZI pointSel;
std::vector pointSearchInd;
std::vector pointSearchSqDis;
TicToc t_data;
// find correspondence for corner features
for (int i = 0; i < cornerPointsSharpNum; ++i)
{
//利用变换矩阵反推上一帧该点位置
TransformToStart(&(cornerPointsSharp->points[i]), &pointSel);
//使用KD-tree求解相对上一帧里点云，pointSel和他们的距离，返回一个最近点的点云线数pointSearchInd和距离pointSearchSqDis
//可以看https://zhuanlan.zhihu.com/p/112246942
kdtreeCornerLast->nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis);
//closestPointInd是离pointSel最近点A的序号（算是上一帧里第几个点的那个几）
int closestPointInd = -1, minPointInd2 = -1;
//距离小于阈值
if (pointSearchSqDis[0] < DISTANCE_SQ_THRESHOLD)
{
closestPointInd = pointSearchInd[0];
//线号
int closestPointScanID = int(laserCloudCornerLast->points[closestPointInd].intensity);
//最短距离之后更新
double minPointSqDis2 = DISTANCE_SQ_THRESHOLD;
// search in the direction of increasing scan line
//找临近线的点B，该点线数不能小于等于A线数
for (int j = closestPointInd + 1; j < (int)laserCloudCornerLast->points.size(); ++j)
{
// if in the same scan line, continue
if (int(laserCloudCornerLast->points[j].intensity) <= closestPointScanID)
continue;
// if not in nearby scans, end the loop
// 如果B点的线数过远于点A也不行，直接不再循环找
if (int(laserCloudCornerLast->points[j].intensity) > (closestPointScanID + NEARBY_SCAN))
break;
double pointSqDis = (laserCloudCornerLast->points[j].x - pointSel.x) *
(laserCloudCornerLast->points[j].x - pointSel.x) +
(laserCloudCornerLast->points[j].y - pointSel.y) *
(laserCloudCornerLast->points[j].y - pointSel.y) +
(laserCloudCornerLast->points[j].z - pointSel.z) *
(laserCloudCornerLast->points[j].z - pointSel.z);
if (pointSqDis < minPointSqDis2)
{
// find nearer point
minPointSqDis2 = pointSqDis;
minPointInd2 = j;
}
}
// search in the direction of decreasing scan line
//找临近线的点B，该点线数不能小于等于A线数
for (int j = closestPointInd - 1; j >= 0; --j)
{
// if in the same scan line, continue
if (int(laserCloudCornerLast->points[j].intensity) >= closestPointScanID)
continue;
// if not in nearby scans, end the loop
if (int(laserCloudCornerLast->points[j].intensity) < (closestPointScanID - NEARBY_SCAN))
break;
double pointSqDis = (laserCloudCornerLast->points[j].x - pointSel.x) *
(laserCloudCornerLast->points[j].x - pointSel.x) +
(laserCloudCornerLast->points[j].y - pointSel.y) *
(laserCloudCornerLast->points[j].y - pointSel.y) +
(laserCloudCornerLast->points[j].z - pointSel.z) *
(laserCloudCornerLast->points[j].z - pointSel.z);
if (pointSqDis < minPointSqDis2)
{
// find nearer point
minPointSqDis2 = pointSqDis;
minPointInd2 = j;
}
}
}
//存在比点A更近点pointSel
if (minPointInd2 >= 0) // both closestPointInd and minPointInd2 is valid
{
Eigen::Vector3d curr_point(cornerPointsSharp->points[i].x,
cornerPointsSharp->points[i].y,
cornerPointsSharp->points[i].z);
Eigen::Vector3d last_point_a(laserCloudCornerLast->points[closestPointInd].x,
laserCloudCornerLast->points[closestPointInd].y,
laserCloudCornerLast->points[closestPointInd].z);
Eigen::Vector3d last_point_b(laserCloudCornerLast->points[minPointInd2].x,
laserCloudCornerLast->points[minPointInd2].y,
laserCloudCornerLast->points[minPointInd2].z);
double s;
if (DISTORTION)
s = (cornerPointsSharp->points[i].intensity - int(cornerPointsSharp->points[i].intensity)) / SCAN_PERIOD;
else
s = 1.0;
ceres::CostFunction *cost_function = LidarEdgeFactor::Create(curr_point, last_point_a, last_point_b, s);
problem.AddResidualBlock(cost_function, loss_function, para_q, para_t);
corner_correspondence++;
}
}
// find correspondence for plane features
for (int i = 0; i < surfPointsFlatNum; ++i)
{
TransformToStart(&(surfPointsFlat->points[i]), &pointSel);
kdtreeSurfLast->nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis);
int closestPointInd = -1, minPointInd2 = -1, minPointInd3 = -1;
if (pointSearchSqDis[0] < DISTANCE_SQ_THRESHOLD)
{
closestPointInd = pointSearchInd[0];
// get closest point's scan ID
int closestPointScanID = int(laserCloudSurfLast->points[closestPointInd].intensity);
double minPointSqDis2 = DISTANCE_SQ_THRESHOLD, minPointSqDis3 = DISTANCE_SQ_THRESHOLD;
// search in the direction of increasing scan line
for (int j = closestPointInd + 1; j < (int)laserCloudSurfLast->points.size(); ++j)
{
// if not in nearby scans, end the loop
if (int(laserCloudSurfLast->points[j].intensity) > (closestPointScanID + NEARBY_SCAN))
break;
double pointSqDis = (laserCloudSurfLast->points[j].x - pointSel.x) *
(laserCloudSurfLast->points[j].x - pointSel.x) +
(laserCloudSurfLast->points[j].y - pointSel.y) *
(laserCloudSurfLast->points[j].y - pointSel.y) +
(laserCloudSurfLast->points[j].z - pointSel.z) *
(laserCloudSurfLast->points[j].z - pointSel.z);
// if in the same or lower scan line
if (int(laserCloudSurfLast->points[j].intensity) <= closestPointScanID && pointSqDis < minPointSqDis2)
{
minPointSqDis2 = pointSqDis;
minPointInd2 = j;
}
// if in the higher scan line
else if (int(laserCloudSurfLast->points[j].intensity) > closestPointScanID && pointSqDis < minPointSqDis3)
{
minPointSqDis3 = pointSqDis;
minPointInd3 = j;
}
}
// search in the direction of decreasing scan line
for (int j = closestPointInd - 1; j >= 0; --j)
{
// if not in nearby scans, end the loop
if (int(laserCloudSurfLast->points[j].intensity) < (closestPointScanID - NEARBY_SCAN))
break;
double pointSqDis = (laserCloudSurfLast->points[j].x - pointSel.x) *
(laserCloudSurfLast->points[j].x - pointSel.x) +
(laserCloudSurfLast->points[j].y - pointSel.y) *
(laserCloudSurfLast->points[j].y - pointSel.y) +
(laserCloudSurfLast->points[j].z - pointSel.z) *
(laserCloudSurfLast->points[j].z - pointSel.z);
// if in the same or higher scan line
if (int(laserCloudSurfLast->points[j].intensity) >= closestPointScanID && pointSqDis < minPointSqDis2)
{
minPointSqDis2 = pointSqDis;
minPointInd2 = j;
}
else if (int(laserCloudSurfLast->points[j].intensity) < closestPointScanID && pointSqDis < minPointSqDis3)
{
// find nearer point
minPointSqDis3 = pointSqDis;
minPointInd3 = j;
}
}
if (minPointInd2 >= 0 && minPointInd3 >= 0)
{
Eigen::Vector3d curr_point(surfPointsFlat->points[i].x,
surfPointsFlat->points[i].y,
surfPointsFlat->points[i].z);
Eigen::Vector3d last_point_a(laserCloudSurfLast->points[closestPointInd].x,
laserCloudSurfLast->points[closestPointInd].y,
laserCloudSurfLast->points[closestPointInd].z);
Eigen::Vector3d last_point_b(laserCloudSurfLast->points[minPointInd2].x,
laserCloudSurfLast->points[minPointInd2].y,
laserCloudSurfLast->points[minPointInd2].z);
Eigen::Vector3d last_point_c(laserCloudSurfLast->points[minPointInd3].x,
laserCloudSurfLast->points[minPointInd3].y,
laserCloudSurfLast->points[minPointInd3].z);
double s;
if (DISTORTION)
s = (surfPointsFlat->points[i].intensity - int(surfPointsFlat->points[i].intensity)) / SCAN_PERIOD;
else
s = 1.0;
ceres::CostFunction *cost_function = LidarPlaneFactor::Create(curr_point, last_point_a, last_point_b, last_point_c, s);
problem.AddResidualBlock(cost_function, loss_function, para_q, para_t);
plane_correspondence++;
}
}
}
//printf("coner_correspondance %d, plane_correspondence %d \n", corner_correspondence, plane_correspondence);
printf("data association time %f ms \n", t_data.toc());
//匹配过少
if ((corner_correspondence + plane_correspondence) < 10)
{
printf("less correspondence! *************************************************\n");
}
TicToc t_solver;
ceres::Solver::Options options;
options.linear_solver_type = ceres::DENSE_QR;
//迭代数
options.max_num_iterations = 4;
//进度是否发到STDOUT
options.minimizer_progress_to_stdout = false;
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);
printf("solver time %f ms \n", t_solver.toc());
}
printf("optimization twice time %f \n", t_opt.toc());
//迭代位姿
t_w_curr = t_w_curr + q_w_curr * t_last_curr;
q_w_curr = q_w_curr * q_last_curr;
}
TicToc t_pub;
// publish odometry
nav_msgs::Odometry laserOdometry;
laserOdometry.header.frame_id = "/camera_init";
laserOdometry.child_frame_id = "/laser_odom";
laserOdometry.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat);
laserOdometry.pose.pose.orientation.x = q_w_curr.x();
laserOdometry.pose.pose.orientation.y = q_w_curr.y();
laserOdometry.pose.pose.orientation.z = q_w_curr.z();
laserOdometry.pose.pose.orientation.w = q_w_curr.w();
laserOdometry.pose.pose.position.x = t_w_curr.x();
laserOdometry.pose.pose.position.y = t_w_curr.y();
laserOdometry.pose.pose.position.z = t_w_curr.z();
pubLaserOdometry.publish(laserOdometry);
geometry_msgs::PoseStamped laserPose;
laserPose.header = laserOdometry.header;
laserPose.pose = laserOdometry.pose.pose;
laserPath.header.stamp = laserOdometry.header.stamp;
laserPath.poses.push_back(laserPose);
laserPath.header.frame_id = "/camera_init";
pubLaserPath.publish(laserPath);
// transform corner features and plane features to the scan end point
//调用前面两个函数的不启动部分
if (0)
{
int cornerPointsLessSharpNum = cornerPointsLessSharp->points.size();
for (int i = 0; i < cornerPointsLessSharpNum; i++)
{
TransformToEnd(&cornerPointsLessSharp->points[i], &cornerPointsLessSharp->points[i]);
}
int surfPointsLessFlatNum = surfPointsLessFlat->points.size();
for (int i = 0; i < surfPointsLessFlatNum; i++)
{
TransformToEnd(&surfPointsLessFlat->points[i], &surfPointsLessFlat->points[i]);
}
int laserCloudFullResNum = laserCloudFullRes->points.size();
for (int i = 0; i < laserCloudFullResNum; i++)
{
TransformToEnd(&laserCloudFullRes->points[i], &laserCloudFullRes->points[i]);
}
}
//该帧变前帧
pcl::PointCloud::Ptr laserCloudTemp = cornerPointsLessSharp;
cornerPointsLessSharp = laserCloudCornerLast;
laserCloudCornerLast = laserCloudTemp;
laserCloudTemp = surfPointsLessFlat;
surfPointsLessFlat = laserCloudSurfLast;
laserCloudSurfLast = laserCloudTemp;
laserCloudCornerLastNum = laserCloudCornerLast->points.size();
laserCloudSurfLastNum = laserCloudSurfLast->points.size();
// std::cout << "the size of corner last is " << laserCloudCornerLastNum << ", and the size of surf last is " << laserCloudSurfLastNum << '\n';
//输入点云便于下次KD查询
kdtreeCornerLast->setInputCloud(laserCloudCornerLast);
kdtreeSurfLast->setInputCloud(laserCloudSurfLast);
//满足skipFrameNum帧数则发送数据
if (frameCount % skipFrameNum == 0)
{
frameCount = 0;
sensor_msgs::PointCloud2 laserCloudCornerLast2;
pcl::toROSMsg(*laserCloudCornerLast, laserCloudCornerLast2);
laserCloudCornerLast2.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat);
laserCloudCornerLast2.header.frame_id = "/camera";
pubLaserCloudCornerLast.publish(laserCloudCornerLast2);
sensor_msgs::PointCloud2 laserCloudSurfLast2;
pcl::toROSMsg(*laserCloudSurfLast, laserCloudSurfLast2);
laserCloudSurfLast2.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat);
laserCloudSurfLast2.header.frame_id = "/camera";
pubLaserCloudSurfLast.publish(laserCloudSurfLast2);
sensor_msgs::PointCloud2 laserCloudFullRes3;
pcl::toROSMsg(*laserCloudFullRes, laserCloudFullRes3);
laserCloudFullRes3.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat);
laserCloudFullRes3.header.frame_id = "/camera";
pubLaserCloudFullRes.publish(laserCloudFullRes3);
}
//输出发布用时和总用时
printf("publication time %f ms \n", t_pub.toc());
printf("whole laserOdometry time %f ms \n \n", t_whole.toc());
if(t_whole.toc() > 100)
ROS_WARN("odometry process over 100ms");
frameCount++;
}
rate.sleep();
}
return 0;
} laserMapping.cpp:
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include "lidarFactor.hpp"
#include "aloam_velodyne/common.h"
#include "aloam_velodyne/tic_toc.h"
int frameCount = 0;
//接收标志
double timeLaserCloudCornerLast = 0;
double timeLaserCloudSurfLast = 0;
double timeLaserCloudFullRes = 0;
double timeLaserOdometry = 0;
//地图有多少个包宽高深
int laserCloudCenWidth = 10;
int laserCloudCenHeight = 10;
int laserCloudCenDepth = 5;
//
const int laserCloudWidth = 21;
const int laserCloudHeight = 21;
const int laserCloudDepth = 11;
//点云方块集合最大数量
const int laserCloudNum = laserCloudWidth * laserCloudHeight * laserCloudDepth; //4851
//lidar视域范围内(FOV)的点云集索引
int laserCloudValidInd[125];
//lidar周围的点云集索引
int laserCloudSurroundInd[125];
// input: from odom
pcl::PointCloud::Ptr laserCloudCornerLast(new pcl::PointCloud());
pcl::PointCloud::Ptr laserCloudSurfLast(new pcl::PointCloud());
// ouput: all visualble cube points
pcl::PointCloud::Ptr laserCloudSurround(new pcl::PointCloud());
// surround points in map to build tree
pcl::PointCloud::Ptr laserCloudCornerFromMap(new pcl::PointCloud());
pcl::PointCloud::Ptr laserCloudSurfFromMap(new pcl::PointCloud());
//input & output: points in one frame. local --> global
pcl::PointCloud::Ptr laserCloudFullRes(new pcl::PointCloud());
// points in every cube
pcl::PointCloud::Ptr laserCloudCornerArray[laserCloudNum];
pcl::PointCloud::Ptr laserCloudSurfArray[laserCloudNum];
//kd-tree
pcl::KdTreeFLANN::Ptr kdtreeCornerFromMap(new pcl::KdTreeFLANN());
pcl::KdTreeFLANN::Ptr kdtreeSurfFromMap(new pcl::KdTreeFLANN());
double parameters[7] = {0, 0, 0, 1, 0, 0, 0};
//世界坐标系下某个点的四元数和位移
Eigen::Map q_w_curr(parameters);
Eigen::Map t_w_curr(parameters + 4);
// wmap_T_odom * odom_T_curr = wmap_T_curr;
// transformation between odom's world and map's world frame
//世界坐标系下当前里程计坐标系的四元数与位移
Eigen::Quaterniond q_wmap_wodom(1, 0, 0, 0);
Eigen::Vector3d t_wmap_wodom(0, 0, 0);
//里程计坐标系下某点的四元数和位移
Eigen::Quaterniond q_wodom_curr(1, 0, 0, 0);
Eigen::Vector3d t_wodom_curr(0, 0, 0);
//接收缓存区
std::queue cornerLastBuf;
std::queue surfLastBuf;
std::queue fullResBuf;
std::queue odometryBuf;
std::mutex mBuf;
//降采样角点和面点
pcl::VoxelGrid downSizeFilterCorner;
pcl::VoxelGrid downSizeFilterSurf;
//KD-tree使用的找到点的序号和距离
std::vector pointSearchInd;
std::vector pointSearchSqDis;
//原点和KD-tree搜索的最邻近点
PointType pointOri, pointSel;
//输出量
ros::Publisher pubLaserCloudSurround, pubLaserCloudMap, pubLaserCloudFullRes, pubOdomAftMapped, pubOdomAftMappedHighFrec, pubLaserAfterMappedPath;
//pubLaserAfterMappedPath的暂存
nav_msgs::Path laserAfterMappedPath;
// set initial guess
/*
本函数内坐标系有三个
1.雷达坐标系，雷达扫描时，某点会有一个位置point_curr
2.里程计坐标系，雷达相对于里程计有一个四元数和位移矫正 q_wodom_curr+t_wodom_curr
3.世界坐标系，里程计坐标系相对世界坐标系有一个四元数和位移矫正 q_wmap_wodom+t_wmap_wodom
so
雷达坐标系到世界坐标系有一个四元数和位移矫正 q_w_curr+t_w_curr
某点在世界坐标系下位置 point_w
*/
//求世界坐标系下某个点的四元数和位移
void transformAssociateToMap()
{
q_w_curr = q_wmap_wodom * q_wodom_curr;
t_w_curr = q_wmap_wodom * t_wodom_curr + t_wmap_wodom;
}
//求世界坐标系下当前里程计坐标系的四元数与位移
void transformUpdate()
{
q_wmap_wodom = q_w_curr * q_wodom_curr.inverse();
t_wmap_wodom = t_w_curr - q_wmap_wodom * t_wodom_curr;
}
//求某点世界坐标系下的位置
void pointAssociateToMap(PointType const *const pi, PointType *const po)
{
Eigen::Vector3d point_curr(pi->x, pi->y, pi->z);
Eigen::Vector3d point_w = q_w_curr * point_curr + t_w_curr;
po->x = point_w.x();
po->y = point_w.y();
po->z = point_w.z();
po->intensity = pi->intensity;
//po->intensity = 1.0;
}
//求雷达坐标系下的某点位置
void pointAssociateTobeMapped(PointType const *const pi, PointType *const po)
{
Eigen::Vector3d point_w(pi->x, pi->y, pi->z);
Eigen::Vector3d point_curr = q_w_curr.inverse() * (point_w - t_w_curr);
po->x = point_curr.x();
po->y = point_curr.y();
po->z = point_curr.z();
po->intensity = pi->intensity;
}
//互斥锁接收函数
void laserCloudCornerLastHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudCornerLast2)
{
mBuf.lock();
cornerLastBuf.push(laserCloudCornerLast2);
mBuf.unlock();
}
void laserCloudSurfLastHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudSurfLast2)
{
mBuf.lock();
surfLastBuf.push(laserCloudSurfLast2);
mBuf.unlock();
}
void laserCloudFullResHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudFullRes2)
{
mBuf.lock();
fullResBuf.push(laserCloudFullRes2);
mBuf.unlock();
}
//receive odomtry
void laserOdometryHandler(const nav_msgs::Odometry::ConstPtr &laserOdometry)
{
mBuf.lock();
odometryBuf.push(laserOdometry);
mBuf.unlock();
// high frequence publish
Eigen::Quaterniond q_wodom_curr;
Eigen::Vector3d t_wodom_curr;
q_wodom_curr.x() = laserOdometry->pose.pose.orientation.x;
q_wodom_curr.y() = laserOdometry->pose.pose.orientation.y;
q_wodom_curr.z() = laserOdometry->pose.pose.orientation.z;
q_wodom_curr.w() = laserOdometry->pose.pose.orientation.w;
t_wodom_curr.x() = laserOdometry->pose.pose.position.x;
t_wodom_curr.y() = laserOdometry->pose.pose.position.y;
t_wodom_curr.z() = laserOdometry->pose.pose.position.z;
Eigen::Quaterniond q_w_curr = q_wmap_wodom * q_wodom_curr;
Eigen::Vector3d t_w_curr = q_wmap_wodom * t_wodom_curr + t_wmap_wodom;
nav_msgs::Odometry odomAftMapped;
odomAftMapped.header.frame_id = "/camera_init";
odomAftMapped.child_frame_id = "/aft_mapped";
odomAftMapped.header.stamp = laserOdometry->header.stamp;
odomAftMapped.pose.pose.orientation.x = q_w_curr.x();
odomAftMapped.pose.pose.orientation.y = q_w_curr.y();
odomAftMapped.pose.pose.orientation.z = q_w_curr.z();
odomAftMapped.pose.pose.orientation.w = q_w_curr.w();
odomAftMapped.pose.pose.position.x = t_w_curr.x();
odomAftMapped.pose.pose.position.y = t_w_curr.y();
odomAftMapped.pose.pose.position.z = t_w_curr.z();
pubOdomAftMappedHighFrec.publish(odomAftMapped);
}
void process()
{
while(1)
{
//数据全部接收
//有一种可能是某一帧下没有找到某类特征点，所以该类信息会提前一个时间戳到达缓存区
while (!cornerLastBuf.empty() && !surfLastBuf.empty() &&
!fullResBuf.empty() && !odometryBuf.empty())
{
mBuf.lock();
//如果里程计信息不为空，里程计时间戳小于角特征时间戳则取出里程计数据
while (!odometryBuf.empty() && odometryBuf.front()->header.stamp.toSec() < cornerLastBuf.front()->header.stamp.toSec())
odometryBuf.pop();
//如果里程计信息为空跳出本次循环
if (odometryBuf.empty())
{
mBuf.unlock();
break;
}
//如果面特征信息不为空，面特征时间戳小于特征时间戳则取出面特征数据
while (!surfLastBuf.empty() && surfLastBuf.front()->header.stamp.toSec() < cornerLastBuf.front()->header.stamp.toSec())
surfLastBuf.pop();
//如果面特征信息为空跳出本次循环
if (surfLastBuf.empty())
{
mBuf.unlock();
break;
}
//如果全部点信息不为空，全部点云时间戳小于角特征时间戳则取出全部点云信息
while (!fullResBuf.empty() && fullResBuf.front()->header.stamp.toSec() < cornerLastBuf.front()->header.stamp.toSec())
fullResBuf.pop();
//全部点云信息为空则跳出
if (fullResBuf.empty())
{
mBuf.unlock();
break;
}
//记录时间戳
timeLaserCloudCornerLast = cornerLastBuf.front()->header.stamp.toSec();
timeLaserCloudSurfLast = surfLastBuf.front()->header.stamp.toSec();
timeLaserCloudFullRes = fullResBuf.front()->header.stamp.toSec();
timeLaserOdometry = odometryBuf.front()->header.stamp.toSec();
//再次判定时间戳是否一致
if (timeLaserCloudCornerLast != timeLaserOdometry ||
timeLaserCloudSurfLast != timeLaserOdometry ||
timeLaserCloudFullRes != timeLaserOdometry)
{
printf("time corner %f surf %f full %f odom %f \n", timeLaserCloudCornerLast, timeLaserCloudSurfLast, timeLaserCloudFullRes, timeLaserOdometry);
printf("unsync messeage!");
mBuf.unlock();
break;
}
//清空上次角特征点云，并接收新的
laserCloudCornerLast->clear();
pcl::fromROSMsg(*cornerLastBuf.front(), *laserCloudCornerLast);
cornerLastBuf.pop();
//清空上次面特征点云，并接收新的
laserCloudSurfLast->clear();
pcl::fromROSMsg(*surfLastBuf.front(), *laserCloudSurfLast);
surfLastBuf.pop();
//清空上次全部点云，并接收新的
laserCloudFullRes->clear();
pcl::fromROSMsg(*fullResBuf.front(), *laserCloudFullRes);
fullResBuf.pop();
//接收里程计坐标系下的四元数与位移
q_wodom_curr.x() = odometryBuf.front()->pose.pose.orientation.x;
q_wodom_curr.y() = odometryBuf.front()->pose.pose.orientation.y;
q_wodom_curr.z() = odometryBuf.front()->pose.pose.orientation.z;
q_wodom_curr.w() = odometryBuf.front()->pose.pose.orientation.w;
t_wodom_curr.x() = odometryBuf.front()->pose.pose.position.x;
t_wodom_curr.y() = odometryBuf.front()->pose.pose.position.y;
t_wodom_curr.z() = odometryBuf.front()->pose.pose.position.z;
odometryBuf.pop();
//角特征不为空，堆入角特征，输出目前运行实时
while(!cornerLastBuf.empty())
{
cornerLastBuf.pop();
printf("drop lidar frame in mapping for real time performance \n");
}
mBuf.unlock();
TicToc t_whole;
//根据odo_to_map和point_to_odo求point_to_map
transformAssociateToMap();
TicToc t_shift;
//由于数组下标只能为正
//将当前激光雷达（视角）的位置作为中心点，添加一个laserCloudCenWidth的偏执使center为正
int centerCubeI = int((t_w_curr.x() + 25.0) / 50.0) + laserCloudCenWidth;
int centerCubeJ = int((t_w_curr.y() + 25.0) / 50.0) + laserCloudCenHeight;
int centerCubeK = int((t_w_curr.z() + 25.0) / 50.0) + laserCloudCenDepth;
//由于int始终向0取整，所以t_w小于-25时，要修正其取整方向，使得所有取整方向一致
if (t_w_curr.x() + 25.0 < 0)
centerCubeI--;
if (t_w_curr.y() + 25.0 < 0)
centerCubeJ--;
if (t_w_curr.z() + 25.0 < 0)
centerCubeK--;
//调整之后取值范围:3 < centerCubeI < 18， 3 < centerCubeJ < 8, 3 < centerCubeK < 18
//如果处于下边界，表明地图向负方向延伸的可能性比较大，则循环移位，将数组中心点向上边界调整一个单位
while (centerCubeI < 3)
{
for (int j = 0; j < laserCloudHeight; j++)
{
for (int k = 0; k < laserCloudDepth; k++)
{
int i = laserCloudWidth - 1;
//指针赋值，保存最后一个指针位置
pcl::PointCloud::Ptr laserCloudCubeCornerPointer =
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
pcl::PointCloud::Ptr laserCloudCubeSurfPointer =
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
//循环移位，I维度上依次后移
for (; i >= 1; i--)
{
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudCornerArray[i - 1 + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudSurfArray[i - 1 + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
}
//将开始点赋值为最后一个点
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudCubeCornerPointer;
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudCubeSurfPointer;
laserCloudCubeCornerPointer->clear();
laserCloudCubeSurfPointer->clear();
}
}
centerCubeI++;
laserCloudCenWidth++;
}
//如果处于上边界，表明地图向正方向延伸的可能性比较大，则循环移位，将数组中心点向下边界调整一个单位
while (centerCubeI >= laserCloudWidth - 3)
{
for (int j = 0; j < laserCloudHeight; j++)
{
for (int k = 0; k < laserCloudDepth; k++)
{
int i = 0;
pcl::PointCloud::Ptr laserCloudCubeCornerPointer =
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
pcl::PointCloud::Ptr laserCloudCubeSurfPointer =
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
for (; i < laserCloudWidth - 1; i++)
{
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudCornerArray[i + 1 + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudSurfArray[i + 1 + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
}
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudCubeCornerPointer;
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudCubeSurfPointer;
laserCloudCubeCornerPointer->clear();
laserCloudCubeSurfPointer->clear();
}
}
centerCubeI--;
laserCloudCenWidth--;
}
while (centerCubeJ < 3)
{
for (int i = 0; i < laserCloudWidth; i++)
{
for (int k = 0; k < laserCloudDepth; k++)
{
int j = laserCloudHeight - 1;
pcl::PointCloud::Ptr laserCloudCubeCornerPointer =
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
pcl::PointCloud::Ptr laserCloudCubeSurfPointer =
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
for (; j >= 1; j--)
{
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudCornerArray[i + laserCloudWidth * (j - 1) + laserCloudWidth * laserCloudHeight * k];
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudSurfArray[i + laserCloudWidth * (j - 1) + laserCloudWidth * laserCloudHeight * k];
}
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudCubeCornerPointer;
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudCubeSurfPointer;
laserCloudCubeCornerPointer->clear();
laserCloudCubeSurfPointer->clear();
}
}
centerCubeJ++;
laserCloudCenHeight++;
}
while (centerCubeJ >= laserCloudHeight - 3)
{
for (int i = 0; i < laserCloudWidth; i++)
{
for (int k = 0; k < laserCloudDepth; k++)
{
int j = 0;
pcl::PointCloud::Ptr laserCloudCubeCornerPointer =
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
pcl::PointCloud::Ptr laserCloudCubeSurfPointer =
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
for (; j < laserCloudHeight - 1; j++)
{
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudCornerArray[i + laserCloudWidth * (j + 1) + laserCloudWidth * laserCloudHeight * k];
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudSurfArray[i + laserCloudWidth * (j + 1) + laserCloudWidth * laserCloudHeight * k];
}
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudCubeCornerPointer;
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudCubeSurfPointer;
laserCloudCubeCornerPointer->clear();
laserCloudCubeSurfPointer->clear();
}
}
centerCubeJ--;
laserCloudCenHeight--;
}
while (centerCubeK < 3)
{
for (int i = 0; i < laserCloudWidth; i++)
{
for (int j = 0; j < laserCloudHeight; j++)
{
int k = laserCloudDepth - 1;
pcl::PointCloud::Ptr laserCloudCubeCornerPointer =
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
pcl::PointCloud::Ptr laserCloudCubeSurfPointer =
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
for (; k >= 1; k--)
{
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * (k - 1)];
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * (k - 1)];
}
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudCubeCornerPointer;
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudCubeSurfPointer;
laserCloudCubeCornerPointer->clear();
laserCloudCubeSurfPointer->clear();
}
}
centerCubeK++;
laserCloudCenDepth++;
}
while (centerCubeK >= laserCloudDepth - 3)
{
for (int i = 0; i < laserCloudWidth; i++)
{
for (int j = 0; j < laserCloudHeight; j++)
{
int k = 0;
pcl::PointCloud::Ptr laserCloudCubeCornerPointer =
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
pcl::PointCloud::Ptr laserCloudCubeSurfPointer =
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
for (; k < laserCloudDepth - 1; k++)
{
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * (k + 1)];
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * (k + 1)];
}
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudCubeCornerPointer;
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudCubeSurfPointer;
laserCloudCubeCornerPointer->clear();
laserCloudCubeSurfPointer->clear();
}
}
centerCubeK--;
laserCloudCenDepth--;
}
int laserCloudValidNum = 0;
int laserCloudSurroundNum = 0;
//在每一维附近5个cube(前2个，后2个，中间1个)里进行查找（前后250米范围内，总共500米范围），三个维度总共125个cube
//在这125个cube里面进一步筛选在视域范围内的cube
for (int i = centerCubeI - 2; i <= centerCubeI + 2; i++)
{
for (int j = centerCubeJ - 2; j <= centerCubeJ + 2; j++)
{
for (int k = centerCubeK - 1; k <= centerCubeK + 1; k++)
{
if (i >= 0 && i < laserCloudWidth &&
j >= 0 && j < laserCloudHeight &&
k >= 0 && k < laserCloudDepth)
{
//记住视域范围内的cube索引，匹配用
laserCloudValidInd[laserCloudValidNum] = i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k;
laserCloudValidNum++;
//记住附近所有cube的索引，显示用
laserCloudSurroundInd[laserCloudSurroundNum] = i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k;
laserCloudSurroundNum++;
}
}
}
}
laserCloudCornerFromMap->clear();
laserCloudSurfFromMap->clear();
//构建特征点地图，查找匹配使用
for (int i = 0; i < laserCloudValidNum; i++)
{
*laserCloudCornerFromMap += *laserCloudCornerArray[laserCloudValidInd[i]];
*laserCloudSurfFromMap += *laserCloudSurfArray[laserCloudValidInd[i]];
}
int laserCloudCornerFromMapNum = laserCloudCornerFromMap->points.size();
int laserCloudSurfFromMapNum = laserCloudSurfFromMap->points.size();
//降采样角点和面点，并统计降采样之后的数量
pcl::PointCloud::Ptr laserCloudCornerStack(new pcl::PointCloud());
downSizeFilterCorner.setInputCloud(laserCloudCornerLast);
downSizeFilterCorner.filter(*laserCloudCornerStack);
int laserCloudCornerStackNum = laserCloudCornerStack->points.size();
pcl::PointCloud::Ptr laserCloudSurfStack(new pcl::PointCloud());
downSizeFilterSurf.setInputCloud(laserCloudSurfLast);
downSizeFilterSurf.filter(*laserCloudSurfStack);
int laserCloudSurfStackNum = laserCloudSurfStack->points.size();
printf("map prepare time %f ms\n", t_shift.toc());
printf("map corner num %d surf num %d \n", laserCloudCornerFromMapNum, laserCloudSurfFromMapNum);
//如果点数较多（不然会不断累计直到满足数量）
if (laserCloudCornerFromMapNum > 10 && laserCloudSurfFromMapNum > 50)
{
TicToc t_opt;
TicToc t_tree;
//构建KD-tree
kdtreeCornerFromMap->setInputCloud(laserCloudCornerFromMap);
kdtreeSurfFromMap->setInputCloud(laserCloudSurfFromMap);
printf("build tree time %f ms \n", t_tree.toc());
//优化两次，第二次在第一次得到的pose上进行
for (int iterCount = 0; iterCount < 2; iterCount++)
{
//ceres::LossFunction *loss_function = NULL;
ceres::LossFunction *loss_function = new ceres::HuberLoss(0.1);
ceres::LocalParameterization *q_parameterization =
new ceres::EigenQuaternionParameterization();
ceres::Problem::Options problem_options;
ceres::Problem problem(problem_options);
problem.AddParameterBlock(parameters, 4, q_parameterization);
problem.AddParameterBlock(parameters + 4, 3);
TicToc t_data;
int corner_num = 0;
for (int i = 0; i < laserCloudCornerStackNum; i++)
{
//对于每一个降采样后的角点
pointOri = laserCloudCornerStack->points[i];
//double sqrtDis = pointOri.x * pointOri.x + pointOri.y * pointOri.y + pointOri.z * pointOri.z;
//求出其世界坐标系的位置
pointAssociateToMap(&pointOri, &pointSel);
//并寻找最近的五个点
kdtreeCornerFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis);
//如果五个点中最远的那个还小于1米，则求解协方差矩阵
if (pointSearchSqDis[4] < 1.0)
{
std::vector nearCorners;
Eigen::Vector3d center(0, 0, 0);
for (int j = 0; j < 5; j++)
{
Eigen::Vector3d tmp(laserCloudCornerFromMap->points[pointSearchInd[j]].x,
laserCloudCornerFromMap->points[pointSearchInd[j]].y,
laserCloudCornerFromMap->points[pointSearchInd[j]].z);
center = center + tmp;
nearCorners.push_back(tmp);
}
center = center / 5.0;
//记录五个点的位置，并计算中心点
Eigen::Matrix3d covMat = Eigen::Matrix3d::Zero();
for (int j = 0; j < 5; j++)
{
Eigen::Matrix tmpZeroMean = nearCorners[j] - center;
//协方差矩阵
covMat = covMat + tmpZeroMean * tmpZeroMean.transpose();
}
Eigen::SelfAdjointEigenSolver saes(covMat);
//根据协方差矩阵的特征值判定是否真的为角特征
//可以跑一下这个程序了解一下运算https://blog.csdn.net/u012936940/article/details/79871941
Eigen::Vector3d unit_direction = saes.eigenvectors().col(2);
Eigen::Vector3d curr_point(pointOri.x, pointOri.y, pointOri.z);
if (saes.eigenvalues()[2] > 3 * saes.eigenvalues()[1])
{
Eigen::Vector3d point_on_line = center;
Eigen::Vector3d point_a, point_b;
point_a = 0.1 * unit_direction + point_on_line;
point_b = -0.1 * unit_direction + point_on_line;
ceres::CostFunction *cost_function = LidarEdgeFactor::Create(curr_point, point_a, point_b, 1.0);
problem.AddResidualBlock(cost_function, loss_function, parameters, parameters + 4);
corner_num++;
}
}
/*
else if(pointSearchSqDis[4] < 0.01 * sqrtDis)
{
Eigen::Vector3d center(0, 0, 0);
for (int j = 0; j < 5; j++)
{
Eigen::Vector3d tmp(laserCloudCornerFromMap->points[pointSearchInd[j]].x,
laserCloudCornerFromMap->points[pointSearchInd[j]].y,
laserCloudCornerFromMap->points[pointSearchInd[j]].z);
center = center + tmp;
}
center = center / 5.0;
Eigen::Vector3d curr_point(pointOri.x, pointOri.y, pointOri.z);
ceres::CostFunction *cost_function = LidarDistanceFactor::Create(curr_point, center);
problem.AddResidualBlock(cost_function, loss_function, parameters, parameters + 4);
}
*/
}
//根据法线判断是否为面特征
int surf_num = 0;
for (int i = 0; i < laserCloudSurfStackNum; i++)
{
pointOri = laserCloudSurfStack->points[i];
//double sqrtDis = pointOri.x * pointOri.x + pointOri.y * pointOri.y + pointOri.z * pointOri.z;
pointAssociateToMap(&pointOri, &pointSel);
kdtreeSurfFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis);
Eigen::Matrix matA0;
Eigen::Matrix matB0 = -1 * Eigen::Matrix::Ones();
if (pointSearchSqDis[4] < 1.0)
{
for (int j = 0; j < 5; j++)
{
matA0(j, 0) = laserCloudSurfFromMap->points[pointSearchInd[j]].x;
matA0(j, 1) = laserCloudSurfFromMap->points[pointSearchInd[j]].y;
matA0(j, 2) = laserCloudSurfFromMap->points[pointSearchInd[j]].z;
//printf(" pts %f %f %f ", matA0(j, 0), matA0(j, 1), matA0(j, 2));
}
// find the norm of plane
//可以根据这个学习一下https://www.cnblogs.com/wangxiaoyong/p/8977343.html
Eigen::Vector3d norm = matA0.colPivHouseholderQr().solve(matB0);
double negative_OA_dot_norm = 1 / norm.norm();
norm.normalize();
// Here n(pa, pb, pc) is unit norm of plane
bool planeValid = true;
for (int j = 0; j < 5; j++)
{
// if OX * n > 0.2, then plane is not fit well
if (fabs(norm(0) * laserCloudSurfFromMap->points[pointSearchInd[j]].x +
norm(1) * laserCloudSurfFromMap->points[pointSearchInd[j]].y +
norm(2) * laserCloudSurfFromMap->points[pointSearchInd[j]].z + negative_OA_dot_norm) > 0.2)
{
planeValid = false;
break;
}
}
Eigen::Vector3d curr_point(pointOri.x, pointOri.y, pointOri.z);
if (planeValid)
{
ceres::CostFunction *cost_function = LidarPlaneNormFactor::Create(curr_point, norm, negative_OA_dot_norm);
problem.AddResidualBlock(cost_function, loss_function, parameters, parameters + 4);
surf_num++;
}
}
/*
else if(pointSearchSqDis[4] < 0.01 * sqrtDis)
{
Eigen::Vector3d center(0, 0, 0);
for (int j = 0; j < 5; j++)
{
Eigen::Vector3d tmp(laserCloudSurfFromMap->points[pointSearchInd[j]].x,
laserCloudSurfFromMap->points[pointSearchInd[j]].y,
laserCloudSurfFromMap->points[pointSearchInd[j]].z);
center = center + tmp;
}
center = center / 5.0;
Eigen::Vector3d curr_point(pointOri.x, pointOri.y, pointOri.z);
ceres::CostFunction *cost_function = LidarDistanceFactor::Create(curr_point, center);
problem.AddResidualBlock(cost_function, loss_function, parameters, parameters + 4);
}
*/
}
//printf("corner num %d used corner num %d \n", laserCloudCornerStackNum, corner_num);
//printf("surf num %d used surf num %d \n", laserCloudSurfStackNum, surf_num);
printf("mapping data assosiation time %f ms \n", t_data.toc());
TicToc t_solver;
ceres::Solver::Options options;
options.linear_solver_type = ceres::DENSE_QR;
options.max_num_iterations = 4;
options.minimizer_progress_to_stdout = false;
options.check_gradients = false;
options.gradient_check_relative_precision = 1e-4;
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);
printf("mapping solver time %f ms \n", t_solver.toc());
//printf("time %f \n", timeLaserOdometry);
//printf("corner factor num %d surf factor num %d\n", corner_num, surf_num);
//printf("result q %f %f %f %f result t %f %f %f\n", parameters[3], parameters[0], parameters[1], parameters[2],
// parameters[4], parameters[5], parameters[6]);
}
printf("mapping optimization time %f \n", t_opt.toc());
}
else
{
ROS_WARN("time Map corner and surf num are not enough");
}
//迭代结束更新相关的转移矩阵
transformUpdate();
TicToc t_add;
//将corner points按距离（比例尺缩小）归入相应的立方体
for (int i = 0; i < laserCloudCornerStackNum; i++)
{
//转移到世界坐标系
pointAssociateToMap(&laserCloudCornerStack->points[i], &pointSel);
//按50的比例尺缩小，四舍五入，偏移laserCloudCen*的量，计算索引
int cubeI = int((pointSel.x + 25.0) / 50.0) + laserCloudCenWidth;
int cubeJ = int((pointSel.y + 25.0) / 50.0) + laserCloudCenHeight;
int cubeK = int((pointSel.z + 25.0) / 50.0) + laserCloudCenDepth;
if (pointSel.x + 25.0 < 0)
cubeI--;
if (pointSel.y + 25.0 < 0)
cubeJ--;
if (pointSel.z + 25.0 < 0)
cubeK--;
//只挑选-laserCloudCenWidth * 50.0 < point.x < laserCloudCenWidth * 50.0范围内的点，y和z同理
//按照尺度放进不同的组，每个组的点数量各异
if (cubeI >= 0 && cubeI < laserCloudWidth &&
cubeJ >= 0 && cubeJ < laserCloudHeight &&
cubeK >= 0 && cubeK < laserCloudDepth)
{
int cubeInd = cubeI + laserCloudWidth * cubeJ + laserCloudWidth * laserCloudHeight * cubeK;
laserCloudCornerArray[cubeInd]->push_back(pointSel);
}
}
//将surf points按距离（比例尺缩小）归入相应的立方体
for (int i = 0; i < laserCloudSurfStackNum; i++)
{
pointAssociateToMap(&laserCloudSurfStack->points[i], &pointSel);
int cubeI = int((pointSel.x + 25.0) / 50.0) + laserCloudCenWidth;
int cubeJ = int((pointSel.y + 25.0) / 50.0) + laserCloudCenHeight;
int cubeK = int((pointSel.z + 25.0) / 50.0) + laserCloudCenDepth;
if (pointSel.x + 25.0 < 0)
cubeI--;
if (pointSel.y + 25.0 < 0)
cubeJ--;
if (pointSel.z + 25.0 < 0)
cubeK--;
if (cubeI >= 0 && cubeI < laserCloudWidth &&
cubeJ >= 0 && cubeJ < laserCloudHeight &&
cubeK >= 0 && cubeK < laserCloudDepth)
{
int cubeInd = cubeI + laserCloudWidth * cubeJ + laserCloudWidth * laserCloudHeight * cubeK;
laserCloudSurfArray[cubeInd]->push_back(pointSel);
}
}
printf("add points time %f ms\n", t_add.toc());
TicToc t_filter;
//特征点下采样
for (int i = 0; i < laserCloudValidNum; i++)
{
int ind = laserCloudValidInd[i];
pcl::PointCloud::Ptr tmpCorner(new pcl::PointCloud());
downSizeFilterCorner.setInputCloud(laserCloudCornerArray[ind]);
downSizeFilterCorner.filter(*tmpCorner);
laserCloudCornerArray[ind] = tmpCorner;
pcl::PointCloud::Ptr tmpSurf(new pcl::PointCloud());
downSizeFilterSurf.setInputCloud(laserCloudSurfArray[ind]);
downSizeFilterSurf.filter(*tmpSurf);
laserCloudSurfArray[ind] = tmpSurf;
}
printf("filter time %f ms \n", t_filter.toc());
TicToc t_pub;
//每5帧填冲一下临近点云地图
if (frameCount % 5 == 0)
{
laserCloudSurround->clear();
for (int i = 0; i < laserCloudSurroundNum; i++)
{
int ind = laserCloudSurroundInd[i];
*laserCloudSurround += *laserCloudCornerArray[ind];
*laserCloudSurround += *laserCloudSurfArray[ind];
}
sensor_msgs::PointCloud2 laserCloudSurround3;
pcl::toROSMsg(*laserCloudSurround, laserCloudSurround3);
laserCloudSurround3.header.stamp = ros::Time().fromSec(timeLaserOdometry);
laserCloudSurround3.header.frame_id = "/camera_init";
pubLaserCloudSurround.publish(laserCloudSurround3);
}
//每20帧填冲一下总点云地图(降采样后的)
if (frameCount % 20 == 0)
{
pcl::PointCloud laserCloudMap;
for (int i = 0; i < 4851; i++)
{
laserCloudMap += *laserCloudCornerArray[i];
laserCloudMap += *laserCloudSurfArray[i];
}
sensor_msgs::PointCloud2 laserCloudMsg;
pcl::toROSMsg(laserCloudMap, laserCloudMsg);
laserCloudMsg.header.stamp = ros::Time().fromSec(timeLaserOdometry);
laserCloudMsg.header.frame_id = "/camera_init";
pubLaserCloudMap.publish(laserCloudMsg);
}
int laserCloudFullResNum = laserCloudFullRes->points.size();
for (int i = 0; i < laserCloudFullResNum; i++)
{
pointAssociateToMap(&laserCloudFullRes->points[i], &laserCloudFullRes->points[i]);
}
sensor_msgs::PointCloud2 laserCloudFullRes3;
pcl::toROSMsg(*laserCloudFullRes, laserCloudFullRes3);
laserCloudFullRes3.header.stamp = ros::Time().fromSec(timeLaserOdometry);
laserCloudFullRes3.header.frame_id = "/camera_init";
pubLaserCloudFullRes.publish(laserCloudFullRes3);
printf("mapping pub time %f ms \n", t_pub.toc());
printf("whole mapping time %f ms +++++\n", t_whole.toc());
nav_msgs::Odometry odomAftMapped;
odomAftMapped.header.frame_id = "/camera_init";
odomAftMapped.child_frame_id = "/aft_mapped";
odomAftMapped.header.stamp = ros::Time().fromSec(timeLaserOdometry);
odomAftMapped.pose.pose.orientation.x = q_w_curr.x();
odomAftMapped.pose.pose.orientation.y = q_w_curr.y();
odomAftMapped.pose.pose.orientation.z = q_w_curr.z();
odomAftMapped.pose.pose.orientation.w = q_w_curr.w();
odomAftMapped.pose.pose.position.x = t_w_curr.x();
odomAftMapped.pose.pose.position.y = t_w_curr.y();
odomAftMapped.pose.pose.position.z = t_w_curr.z();
pubOdomAftMapped.publish(odomAftMapped);
geometry_msgs::PoseStamped laserAfterMappedPose;
laserAfterMappedPose.header = odomAftMapped.header;
laserAfterMappedPose.pose = odomAftMapped.pose.pose;
laserAfterMappedPath.header.stamp = odomAftMapped.header.stamp;
laserAfterMappedPath.header.frame_id = "/camera_init";
laserAfterMappedPath.poses.push_back(laserAfterMappedPose);
pubLaserAfterMappedPath.publish(laserAfterMappedPath);
static tf::TransformBroadcaster br;
tf::Transform transform;
tf::Quaternion q;
transform.setOrigin(tf::Vector3(t_w_curr(0),
t_w_curr(1),
t_w_curr(2)));
q.setW(q_w_curr.w());
q.setX(q_w_curr.x());
q.setY(q_w_curr.y());
q.setZ(q_w_curr.z());
transform.setRotation(q);
br.sendTransform(tf::StampedTransform(transform, odomAftMapped.header.stamp, "/camera_init", "/aft_mapped"));
frameCount++;
}
//暂缓2ms
std::chrono::milliseconds dura(2);
std::this_thread::sleep_for(dura);
}
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "laserMapping");
ros::NodeHandle nh;
float lineRes = 0;
float planeRes = 0;
//降采样
nh.param("mapping_line_resolution", lineRes, 0.4);
nh.param("mapping_plane_resolution", planeRes, 0.8);
printf("line resolution %f plane resolution %f \n", lineRes, planeRes);
downSizeFilterCorner.setLeafSize(lineRes, lineRes,lineRes);
downSizeFilterSurf.setLeafSize(planeRes, planeRes, planeRes);
//订阅角点，面点，里程计下当前帧的四元数与位移，全体点云
ros::Subscriber subLaserCloudCornerLast = nh.subscribe("/laser_cloud_corner_last", 100, laserCloudCornerLastHandler);
ros::Subscriber subLaserCloudSurfLast = nh.subscribe("/laser_cloud_surf_last", 100, laserCloudSurfLastHandler);
ros::Subscriber subLaserOdometry = nh.subscribe("/laser_odom_to_init", 100, laserOdometryHandler);
ros::Subscriber subLaserCloudFullRes = nh.subscribe("/velodyne_cloud_3", 100, laserCloudFullResHandler);
//发布周围五帧点云集合（降采样后的），总点云地图（降采样后的），全部点云，构图处理后的当前世界坐标系下雷达位姿，构图处理前的当前世界坐标系下雷达位姿，构图处理后的雷达全部位姿
pubLaserCloudSurround = nh.advertise("/laser_cloud_surround", 100);
pubLaserCloudMap = nh.advertise("/laser_cloud_map", 100);
pubLaserCloudFullRes = nh.advertise("/velodyne_cloud_registered", 100);
pubOdomAftMapped = nh.advertise("/aft_mapped_to_init", 100);
pubOdomAftMappedHighFrec = nh.advertise("/aft_mapped_to_init_high_frec", 100);
pubLaserAfterMappedPath = nh.advertise("/aft_mapped_path", 100);
for (int i = 0; i < laserCloudNum; i++)
{
laserCloudCornerArray[i].reset(new pcl::PointCloud());
laserCloudSurfArray[i].reset(new pcl::PointCloud());
}
std::thread mapping_process{process};
ros::spin();
return 0;
}