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 <cmath>
#include <vector>
#include <string>
#include "aloam_velodyne/common.h"
#include "aloam_velodyne/tic_toc.h"
#include <nav_msgs/Odometry.h>
#include <opencv/cv.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;
//扫描周期, 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]<cloudCurvature[j]); }
//设置发布内容，整体点云，角点，降采样角点，面点，降采样面点，剔除点
ros::Publisher pubLaserCloud;
ros::Publisher pubCornerPointsSharp;
ros::Publisher pubCornerPointsLessSharp;
ros::Publisher pubSurfPointsFlat;
ros::Publisher pubSurfPointsLessFlat;
ros::Publisher pubRemovePoints;
//ros形式的一线扫描
std::vector<ros::Publisher> pubEachScan;
//是否发布每行Scan
bool PUB_EACH_LINE = false;
//根据距离去除过远的点，距离的参数
double MINIMUM_RANGE = 0.1;
//过远点去除 使用template进行兼容
template <typename PointT>
void removeClosedPointCloud(const pcl::PointCloud<PointT> &cloud_in,pcl::PointCloud<PointT> &cloud_out, float thres)
{//统一header(时间戳)和sizeif (&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<uint32_t>(j);//点数是否有限cloud_out.is_dense = true;
}//订阅点云句柄
void laserCloudHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudMsg)
{//是否剔除前systemDelay帧if (!systemInited){ systemInitCount++;if (systemInitCount >= systemDelay){systemInited = true;}elsereturn;}//registration计时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);//该次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到3piif (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);//循环对每个点进行以下操作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);elsescanID = 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/2if (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/2else{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;}}//看看旋转多少了，记录比例relTimefloat relTime = (ori - startOri) / (endOri - startOri);//第几根线和本线进度到多少记录在point.intensitypoint.intensity = scanID + scanPeriod * relTime;//按线分类保存laserCloudScans[scanID].push_back(point); }//cloudSize = count;printf("points size %d \n", cloudSize);//也就是把所有线保存在laserCloud一个数据集合里，把每条线的第五个和倒数第五个位置反馈给scanStartInd和scanEndIndpcl::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;cloudNeighborPicked[i] = 0;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++){//点数小于6就退出if( scanEndInd[i] - scanStartInd[i] < 6)continue;pcl::PointCloud<PointType>::Ptr surfPointsLessFlatScan(new pcl::PointCloud<PointType>);//将每个scan的曲率点分成6等份处理,确保周围都有点被选作特征点,或者说每两行都有for (int j = 0; j < 6; j++){//六等份起点：sp = scanStartInd + (scanEndInd - scanStartInd)*j/6//六等份终点：ep = scanStartInd - 1 + (scanEndInd - scanStartInd)*(j+1)/6int 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<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 scanif(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<int>("scan_line", N_SCANS, 16);//参数，过远去除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;}//接收激光雷达信号ros::Subscriber subLaserCloud = nh.subscribe<sensor_msgs::PointCloud2>("/velodyne_points", 100, laserCloudHandler);//发布laserCloud，laserCloud是按线堆栈的全部点云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);//发布每行scanif(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;
}

laserOdometry.cpp:

#include <cmath>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include <geometry_msgs/PoseStamped.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl_conversions/pcl_conversions.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>
#include <eigen3/Eigen/Dense>
#include <mutex>
#include <queue>#include "aloam_velodyne/common.h"
#include "aloam_velodyne/tic_toc.h"
#include "lidarFactor.hpp"#define DISTORTION 0int 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<pcl::PointXYZI>::Ptr kdtreeCornerLast(new pcl::KdTreeFLANN<pcl::PointXYZI>());
pcl::KdTreeFLANN<pcl::PointXYZI>::Ptr kdtreeSurfLast(new pcl::KdTreeFLANN<pcl::PointXYZI>());//pcl保存形式的输入，角点，降采样角点，面点，降采样面点，上一帧角点，上一帧面点，全部点
pcl::PointCloud<PointType>::Ptr cornerPointsSharp(new pcl::PointCloud<PointType>());
pcl::PointCloud<PointType>::Ptr cornerPointsLessSharp(new pcl::PointCloud<PointType>());
pcl::PointCloud<PointType>::Ptr surfPointsFlat(new pcl::PointCloud<PointType>());
pcl::PointCloud<PointType>::Ptr surfPointsLessFlat(new pcl::PointCloud<PointType>());pcl::PointCloud<PointType>::Ptr laserCloudCornerLast(new pcl::PointCloud<PointType>());
pcl::PointCloud<PointType>::Ptr laserCloudSurfLast(new pcl::PointCloud<PointType>());
pcl::PointCloud<PointType>::Ptr laserCloudFullRes(new pcl::PointCloud<PointType>());//存储上一帧的特征点数量
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<Eigen::Quaterniond> q_last_curr(para_q);
//位移量t
Eigen::Map<Eigen::Vector3d> t_last_curr(para_t);//定义ros格式的订阅内容
std::queue<sensor_msgs::PointCloud2ConstPtr> cornerSharpBuf;
std::queue<sensor_msgs::PointCloud2ConstPtr> cornerLessSharpBuf;
std::queue<sensor_msgs::PointCloud2ConstPtr> surfFlatBuf;
std::queue<sensor_msgs::PointCloud2ConstPtr> surfLessFlatBuf;
std::queue<sensor_msgs::PointCloud2ConstPtr> 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;elses = 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 firstpcl::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 infopo->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<int>("mapping_skip_frame", skipFrameNum, 2);printf("Mapping %d Hz \n", 10 / skipFrameNum);ros::Subscriber subCornerPointsSharp = nh.subscribe<sensor_msgs::PointCloud2>("/laser_cloud_sharp", 100, laserCloudSharpHandler);ros::Subscriber subCornerPointsLessSharp = nh.subscribe<sensor_msgs::PointCloud2>("/laser_cloud_less_sharp", 100, laserCloudLessSharpHandler);ros::Subscriber subSurfPointsFlat = nh.subscribe<sensor_msgs::PointCloud2>("/laser_cloud_flat", 100, laserCloudFlatHandler);ros::Subscriber subSurfPointsLessFlat = nh.subscribe<sensor_msgs::PointCloud2>("/laser_cloud_less_flat", 100, laserCloudLessFlatHandler);ros::Subscriber subLaserCloudFullRes = nh.subscribe<sensor_msgs::PointCloud2>("/velodyne_cloud_2", 100, laserCloudFullResHandler);ros::Publisher pubLaserCloudCornerLast = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_corner_last", 100);ros::Publisher pubLaserCloudSurfLast = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_surf_last", 100);ros::Publisher pubLaserCloudFullRes = nh.advertise<sensor_msgs::PointCloud2>("/velodyne_cloud_3", 100);//pubLaserOdometry包括当前帧四元数Q和位置tros::Publisher pubLaserOdometry = nh.advertise<nav_msgs::Odometry>("/laser_odom_to_init", 100);//pubLaserPath包含当前帧的位置tros::Publisher pubLaserPath = nh.advertise<nav_msgs::Path>("/laser_odom_path", 100);//定义路径，用于保存帧的位置，发布于pubLaserPathnav_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<int> pointSearchInd;std::vector<float> pointSearchSqDis;TicToc t_data;// find correspondence for corner featuresfor (int i = 0; i < cornerPointsSharpNum; ++i){//利用变换矩阵反推上一帧该点位置TransformToStart(&(cornerPointsSharp->points[i]), &pointSel);//使用KD-tree求解相对上一帧里点云，pointSel和他们的距离，返回一个最近点的点云线数pointSearchInd和距离pointSearchSqDis//可以看https://zhuanlan.zhihu.com/p/112246942kdtreeCornerLast->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, continueif (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 pointminPointSqDis2 = 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, continueif (int(laserCloudCornerLast->points[j].intensity) >= closestPointScanID)continue;// if not in nearby scans, end the loopif (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 pointminPointSqDis2 = pointSqDis;minPointInd2 = j;}}}//存在比点A更近点pointSelif (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;elses = 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 featuresfor (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 IDint closestPointScanID = int(laserCloudSurfLast->points[closestPointInd].intensity);double minPointSqDis2 = DISTANCE_SQ_THRESHOLD, minPointSqDis3 = DISTANCE_SQ_THRESHOLD;// search in the direction of increasing scan linefor (int j = closestPointInd + 1; j < (int)laserCloudSurfLast->points.size(); ++j){// if not in nearby scans, end the loopif (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 lineif (int(laserCloudSurfLast->points[j].intensity) <= closestPointScanID && pointSqDis < minPointSqDis2){minPointSqDis2 = pointSqDis;minPointInd2 = j;}// if in the higher scan lineelse if (int(laserCloudSurfLast->points[j].intensity) > closestPointScanID && pointSqDis < minPointSqDis3){minPointSqDis3 = pointSqDis;minPointInd3 = j;}}// search in the direction of decreasing scan linefor (int j = closestPointInd - 1; j >= 0; --j){// if not in nearby scans, end the loopif (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 lineif (int(laserCloudSurfLast->points[j].intensity) >= closestPointScanID && pointSqDis < minPointSqDis2){minPointSqDis2 = pointSqDis;minPointInd2 = j;}else if (int(laserCloudSurfLast->points[j].intensity) < closestPointScanID && pointSqDis < minPointSqDis3){// find nearer pointminPointSqDis3 = 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;elses = 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;//进度是否发到STDOUToptions.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 odometrynav_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<PointType>::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 <math.h>
#include <vector>
#include <aloam_velodyne/common.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include <geometry_msgs/PoseStamped.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>
#include <eigen3/Eigen/Dense>
#include <ceres/ceres.h>
#include <mutex>
#include <queue>
#include <thread>
#include <iostream>
#include <string>#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<PointType>::Ptr laserCloudCornerLast(new pcl::PointCloud<PointType>());
pcl::PointCloud<PointType>::Ptr laserCloudSurfLast(new pcl::PointCloud<PointType>());// ouput: all visualble cube points
pcl::PointCloud<PointType>::Ptr laserCloudSurround(new pcl::PointCloud<PointType>());// surround points in map to build tree
pcl::PointCloud<PointType>::Ptr laserCloudCornerFromMap(new pcl::PointCloud<PointType>());
pcl::PointCloud<PointType>::Ptr laserCloudSurfFromMap(new pcl::PointCloud<PointType>());//input & output: points in one frame. local --> global
pcl::PointCloud<PointType>::Ptr laserCloudFullRes(new pcl::PointCloud<PointType>());// points in every cube
pcl::PointCloud<PointType>::Ptr laserCloudCornerArray[laserCloudNum];
pcl::PointCloud<PointType>::Ptr laserCloudSurfArray[laserCloudNum];//kd-tree
pcl::KdTreeFLANN<PointType>::Ptr kdtreeCornerFromMap(new pcl::KdTreeFLANN<PointType>());
pcl::KdTreeFLANN<PointType>::Ptr kdtreeSurfFromMap(new pcl::KdTreeFLANN<PointType>());double parameters[7] = {0, 0, 0, 1, 0, 0, 0};
//世界坐标系下某个点的四元数和位移
Eigen::Map<Eigen::Quaterniond> q_w_curr(parameters);
Eigen::Map<Eigen::Vector3d> 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<sensor_msgs::PointCloud2ConstPtr> cornerLastBuf;
std::queue<sensor_msgs::PointCloud2ConstPtr> surfLastBuf;
std::queue<sensor_msgs::PointCloud2ConstPtr> fullResBuf;
std::queue<nav_msgs::Odometry::ConstPtr> odometryBuf;
std::mutex mBuf;//降采样角点和面点
pcl::VoxelGrid<PointType> downSizeFilterCorner;
pcl::VoxelGrid<PointType> downSizeFilterSurf;//KD-tree使用的找到点的序号和距离
std::vector<int> pointSearchInd;
std::vector<float> 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 publishEigen::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_maptransformAssociateToMap();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<PointType>::Ptr laserCloudCubeCornerPointer =laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k]; pcl::PointCloud<PointType>::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<PointType>::Ptr laserCloudCubeCornerPointer =laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];pcl::PointCloud<PointType>::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<PointType>::Ptr laserCloudCubeCornerPointer =laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];pcl::PointCloud<PointType>::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<PointType>::Ptr laserCloudCubeCornerPointer =laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];pcl::PointCloud<PointType>::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<PointType>::Ptr laserCloudCubeCornerPointer =laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];pcl::PointCloud<PointType>::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<PointType>::Ptr laserCloudCubeCornerPointer =laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];pcl::PointCloud<PointType>::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里面进一步筛选在视域范围内的cubefor (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<PointType>::Ptr laserCloudCornerStack(new pcl::PointCloud<PointType>());downSizeFilterCorner.setInputCloud(laserCloudCornerLast);downSizeFilterCorner.filter(*laserCloudCornerStack);int laserCloudCornerStackNum = laserCloudCornerStack->points.size();pcl::PointCloud<PointType>::Ptr laserCloudSurfStack(new pcl::PointCloud<PointType>());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-treekdtreeCornerFromMap->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<Eigen::Vector3d> 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<double, 3, 1> tmpZeroMean = nearCorners[j] - center;//协方差矩阵covMat = covMat + tmpZeroMean * tmpZeroMean.transpose();}Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> saes(covMat);//根据协方差矩阵的特征值判定是否真的为角特征//可以跑一下这个程序了解一下运算https://blog.csdn.net/u012936940/article/details/79871941Eigen::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<double, 5, 3> matA0;Eigen::Matrix<double, 5, 1> matB0 = -1 * Eigen::Matrix<double, 5, 1>::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.htmlEigen::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 planebool planeValid = true;for (int j = 0; j < 5; j++){// if OX * n > 0.2, then plane is not fit wellif (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<PointType>::Ptr tmpCorner(new pcl::PointCloud<PointType>());downSizeFilterCorner.setInputCloud(laserCloudCornerArray[ind]);downSizeFilterCorner.filter(*tmpCorner);laserCloudCornerArray[ind] = tmpCorner;pcl::PointCloud<PointType>::Ptr tmpSurf(new pcl::PointCloud<PointType>());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<PointType> 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++;}//暂缓2msstd::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<float>("mapping_line_resolution", lineRes, 0.4);nh.param<float>("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<sensor_msgs::PointCloud2>("/laser_cloud_corner_last", 100, laserCloudCornerLastHandler);ros::Subscriber subLaserCloudSurfLast = nh.subscribe<sensor_msgs::PointCloud2>("/laser_cloud_surf_last", 100, laserCloudSurfLastHandler);ros::Subscriber subLaserOdometry = nh.subscribe<nav_msgs::Odometry>("/laser_odom_to_init", 100, laserOdometryHandler);ros::Subscriber subLaserCloudFullRes = nh.subscribe<sensor_msgs::PointCloud2>("/velodyne_cloud_3", 100, laserCloudFullResHandler);//发布周围五帧点云集合（降采样后的），总点云地图（降采样后的），全部点云，构图处理后的当前世界坐标系下雷达位姿，构图处理前的当前世界坐标系下雷达位姿，构图处理后的雷达全部位姿pubLaserCloudSurround = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_surround", 100);pubLaserCloudMap = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_map", 100);pubLaserCloudFullRes = nh.advertise<sensor_msgs::PointCloud2>("/velodyne_cloud_registered", 100);pubOdomAftMapped = nh.advertise<nav_msgs::Odometry>("/aft_mapped_to_init", 100);pubOdomAftMappedHighFrec = nh.advertise<nav_msgs::Odometry>("/aft_mapped_to_init_high_frec", 100);pubLaserAfterMappedPath = nh.advertise<nav_msgs::Path>("/aft_mapped_path", 100);for (int i = 0; i < laserCloudNum; i++){laserCloudCornerArray[i].reset(new pcl::PointCloud<PointType>());laserCloudSurfArray[i].reset(new pcl::PointCloud<PointType>());}std::thread mapping_process{process};ros::spin();return 0;
}

ALOAM试跑及程序注释相关推荐

程序员要避免的五种程序注释方式
导读:注释,本是提高代码可读性.让其他开发人员更快速地理解程序的,然而一些无意义的注释会让人郁闷至极.本文是国外网站GreaterGeek上的一篇文章,作者通过分类介绍了五种一定要避免的程序注释方式. ...
MF_RC522_射频识别参考程序注释（2018年4月27号）
MF_RC522_射频识别参考程序注释(2018年4月27号) https://wenku.baidu.com/view/478e6bb17f1922791688e8f8.html
千万要避免的五种程序注释方式
你是否有过复查程序时发现有些注释毫无用处?程序注释是为了提高代码的可读性,为了让原作者以外的其他开发人员更容易理解这段程序. 我把这些让人郁闷的注释方式归为了五类,同时把写出这些注释的程序员也归为了五 ...
三菱fx5u plc项目模板程序（含触摸屏程序）程序注释全面，用的三菱fx5u系列plc和威纶触摸屏
三菱fx5u plc项目模板程序(含触摸屏程序) 程序注释全面,用的三菱fx5u系列plc和威纶触摸屏,本程序可做三菱fx5u编程框架模板,自己辛苦编写的程序,借鉴价值高,是新手入门级三菱fx5u编程 ...
❤️ 程序注释及格式化工具❤️
❤️ 程序注释及格式化工具❤️ 一.注释模板化统一为什么要做注释和格式化:①.方便自己阅读:不知道你有没有遇到自己写的代码自己看不懂的情况
PointNet.pytorch程序注释（二）点云分割
PointNet.pytorch程序注释(二)点云分割论文及程序地址运行环境训练train 测试test 论文及程序地址论文原文 PointNet: Deep Learning on Poin ...
MATLAB程序采用非支配排序遗传算法（NSGA2）求解分布式电源选址定容问题，可作为一个有用的参考，程序注释明确，算法原理可以自己搜。
MATLAB程序采用非支配排序遗传算法(NSGA2)求解分布式电源选址定容问题,可作为一个有用的参考,程序注释明确,算法原理可以自己搜. :8620651507678049浪迹天涯
matlab里的程序注释复制到word中时变成乱码
matlab里的程序注释复制到word中时变成乱码的解决办法今天在复制matlab里的代码时发现注释都变成了乱码,碰巧手边打开了一个代码编辑器软件:notepad++. 自己尝试了一下先把Matla ...
【R】程序注释乱码怎么办
R 小白发问! 大佬给我的程序注释在我这边直接乱码怎么办! 大佬马上就给解决了 1.重新打开 2.选择这个 NICE!成功火星文直接变回来!身边有一个啥都会的大佬真的爽!

ALOAM试跑及程序注释

ALOAM试跑及程序注释相关推荐

最新文章

热门文章