目录
scanRegistration.cpp ************************************/
laserOdometry.cpp ************************************/
lidarFactor.hpp ************************************/
laserMapping.cpp ************************************/
A-LOAM代码逻辑细节
scanRegistration.cpp ************************************/
1.3D点云预处理
1.1 记录每个scan有曲率的点的开始和结束索引 scanStartInd(N_SCANS, 0) scanEndInd(N_SCANS, 0)
1.2 把ros包的点云ros形式转化为pcl形式 laserCloudIn ，并去除过远点和过近点
1.3 计算起始点和结束点的水平角度startOri和endOri ，以及计算每个点属于第几条线上 scanID，计算水平角ori，以上都是记录每个点是第几根线和扫描的时间的多少记录在laserCloudScans[scanID].push_back(point);按线分类保存 每条线开辟一个数组。
1.4 把所有线保存在laserCloud一个数据集合里，把每条线的第五个和倒数第五个点在laserCloud中的位置反馈给scanStartInd和scanEndInd，例如scanEndInd[5]代表第5条扫描线的结束点在laserCloud中的第scanEndInd[5]个位置。
2.特征点提取
2.1计算曲率，i从5开始cloudCurvature[i]，cloudSortInd[i], cloudNeighborPicked[i]， cloudLabel[i]
2.2将每条扫描线的曲率点分成6等份处理,确保周围都有点被选作特征点
2.3对每个6等分按照曲率大小进行排序，将曲率比较大的点的前后各5个连续距离比较近的点做标记不再筛选为特征点，将最大的两个点放入cornerPointsSharp中，按照曲率大小将前20个点放入cornerPointsLessSharp中。
2.4 挑选每个分段的曲率很小比较小的点，将最小的4个点放入surfPointsFlat中，将小于阈值0.1剩余的点（包括之前被排除的点）全部归入平面点中surfPointsLessFlatScan类别中，由于less flat点最多，对每个分段surfPointsLessFlatScan的点进行提速栅格滤波
2.5 将laserCloud、cornerPointsSharp、cornerPointsLessSharp、surfPointsFlat、 surfPointsLessFlatScan这5个pcl点云库转化为ROS消息发布出去
laserOdometry.cpp ************************************/
1.角点的匹配
1.1定义ceres的核函数 当残差超过0.1之后，就会把权重变小，并构建了ceres优化问题
1.2将当前帧此角点运动补偿开始
1.3laserCloudCornerLast 来自上一帧的corner_less_sharp特征点，当前帧的角点存放在cornerPointsSharp中
1.4寻找最近临点A，使用KD-tree求解相对上一帧里点云，pointSel和他们的距离，返回一个最近点的点云线数pointSearchInd和距离pointSearchSqDis，赋值给closestPointScanID
1.5 找临近线的第二临近点B，该点线数不能小于等于A线数。
1.6用点0 A B构造点到线的距离的残差项，注意这三个点都是在上一帧的Lidar坐标系下，即，残差 = 点O到直线AB的距离，具体到介绍lidarFactor.cpp时再说明该残差的具体计算方法
2.面点的匹配
2.1前几步骤和角点的匹配一样
2.2寻找kd-tree中的最近临点0， 寻找小于等于同线数的第二个临近点B，以及寻找大于同线数的第三个临近点C
2.3用点O，A，B，C构造点到面的距离的残差项，注意这三个点都是在上一帧的Lidar坐标系下，即，残差 = 点O到平面ABC的距离;同样的，具体到介绍lidarFactor.cpp时再说明该残差的具体计算方法
3.进行Ceres优化
3.1 定义了一个优化问题 ceres::Problem problem(problem_options)
3.2 给优化问题添加参数快，problem.AddParameterBlock(para_q, 4, q_parameterization); //这个接口是为了告诉ceres哪个是待优化变量
3.3 定义残差计算，构建代价函数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); //增加残差块的接口
3.4配置ceres solver求解器 ceres::Solver::Options options; 设置一系列solver选项
3.5基于构建的所有残差项，求解最优的当前帧位姿与上一帧位姿的位姿增量：para_q和para_t ceres::Solve(options, &problem, &summary); 开始优化
3.6 用最新计算出的位姿增量，更新上一帧的位姿，得到当前帧的位姿，注意这里说的位姿都指的是世界坐标系下的位姿。
4.发布里程计信息和雷达路径信息
5.更新KD-tree
5.1 将当前的特征点运动补偿到结束点位置
5.2 用当前帧的特征点cornerPointsLessSharp和surfPointsLessFlat 更新上一帧特征点laserCloudCornerLast和laserCloudSurfLast以及相应的kdtree;需要注意的是上一帧的laserCloudCornerLast和laserCloudSurfLast和kdtree内存放的都是运动补偿到结束位置的特征点。
lidarFactor.hpp ************************************/
1.struct LidarEdgeFactor{} 编写了点到线的残差距离计算
2.struct LidarPlaneFactor{} 编写了Odometry线程中点到面的残差距离计算
3.struct LidarPlaneNormFactor{}编写了Mapping线程中点到面的残差距离
4.struct LidarDistanceFactor{} 此处还不太清楚
laserMapping.cpp ************************************/
0.预处理
0.1 降采样，当前帧雷达坐标系下一个快内只保存一个特征点
0.2订阅角点，面点，里程计下当前帧的四元数与位移，全体点云
0.3发布周围五帧点云集合（降采样后的），总点云地图（降采样后的），全部点云，构图处理后的当前世界坐标系下雷达位姿，构图处理前的当前世界坐标系下雷达位姿，构图处理后的雷达全部位姿
0.4为了保证LOAM算法整体的实时性，Mapping线程每次只处理cornerLastBuf.front()及其他与之时间同步的消息
0.5 清空上次角特征点云、面特征点点云、全部点云，并接收新的
1.局部地图的维护
1.1计算当前帧lidar位姿在珊格地图中的相对位置
1.2根据liar位姿实时动态调整珊格地图
1.3 cubeI cubeJ cubeK 的坐标代表角点相对于大的局部珊格地图中心的相对位置，因为局部珊格地图是变化的;laserCloudCornerArray和laserCloudSurfArray是存放局部珊格地图的一维数组，laserCloudCornerArray[5]中存放的数据为局部地图中第5个珊格中存放的是某个角点的世界坐标系信息
1.4在局部地图中选出一个以当前位姿为中心的更小的局部地图用来特征点的匹配
,laserCloudValidInd[i]表示小的局部地图中第i个位置在大的局部地图laserCloudCornerArray中的索引;并将相应的局部地图中的点存放到更小的线地图和面地图中laserCloudCornerFromMap、laserCloudSurfFromMap，里面存放的是某个特征点的世界坐标系信息
2.线特征的提取
2.1将线地图和点地图（地图坐标系中的点）送入kdtree便于最近临的搜索
2.2将降采样之后的当前帧特征点进行变换到世界坐标系下，并利用`kdtree寻找每个当前帧特征点最近的5个小地图中的点
2.3 如果五个点中最远的那个还小于1米，则求解协方差矩阵;进行特征值分解 计算协方差矩阵的特征值和特征向量，用于判断这5个点是不是呈线状分布，此为PCA的原理
2.4 构建ceres问题，残差函数的形式就跟Odometry一样了，残差距离即点到线的距离
3.面特征的提取
3.1同样首先通过 kdtree 在地图中找到最近的面特征，原则上面特征也可以使用特征值分解的方式,选出最小特征值对应的特征向量及平面的法向量,不过代码里选用的是平面拟合的方式
3.2平面方程为Ax + By + Cz + D = 0,考虑到等式的形式,可以进一步写成, Ax + By + Cz + 1 = 0,也就是三个未知数,五个方程,写成矩阵的形式就是一个 5×3 大小的矩阵,求出平面的法向量norm即abc向量并进行归一化
3.3点(x0, y0, z0)到平面Ax + By + Cz + D = 0 的距离公式 = fabs(Ax0 + By0 + Cz0 + D) / sqrt(A^2 + B^2 + C^2)分别求出 5 个点到求出平面的距离,如果太远,则说明该平面拟合不成功。
3.4构建ceres问题，构造点到面的距离残差项，对位姿进行优化。
3.5 完成ICP（迭代2次）的特征匹配后，当Mapping的位姿w_curr计算完毕后，更新增量wmap_wodom，旨在为下一次执行transformAssociateToMap函数时做准备
(注：transformAssociateToMap函数只将lidar位姿从里程计到map做了转换，并没有将特征点进行转换，因为当lidar位姿转化后，计算特征点的地图坐标系坐标时，也就相应的做了转换)
4.局部地图点的更新
4.1将特征点从lidar坐标系转到world坐标系
4.2计算特征点在珊格地图中的相对坐标IJK，进而确定添加到哪个格子中，因为是相对坐标IJK，
所以不管珊格地图怎样动态变化，都可以根据IJK计算出在一维数组中的索引？？？？
cubeInd，并根据索引将当前帧特征点加入到局部地图中去laserCloudCornerArray[cubeInd]->push_back(pointSel)
4.3对新的局部珊格地图做降采样
此处有个问题：
当珊格地图动态变化之后，同一个特征点在珊格地图中的相对位置也跟着变化;但是其世界坐标系的坐标不会发生变化，即其在格栅中的坐标cubeI、cubeJ、cubeK根据计算公式结果不会发生变化; 逻辑和公式矛盾？
A-LOAM代码注释
1.kittiHelper.cpp 注释
#include <iostream>
#include <fstream>
#include <iterator>
#include <string>
#include <vector>
#include <ros/ros.h>
#include <rosbag/bag.h>
#include <eigen3/Eigen/Dense>
// image
#include <opencv2/opencv.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
// point cloud
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/PointCloud2.h>
// odometry
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include <geometry_msgs/PoseStamped.h>
std::vector<float> read_lidar_data(const std::string lidar_data_path) {
// https://blog.csdn.net/alex1997222/article/details/78976154
std::ifstream lidar_data_file(lidar_data_path, std::ifstream::in | std::ifstream::binary);
// seekp 函数用于已经打开要进行输出的文件，而 seekg 函数则用于已经打开要进行输入的文件。可以将 "p" 理解为 "put"，将 "g" 理解为 "get"
// 第一个实参是一个long类型的整数，表示文件中的偏移量。
// 第二个实参称为模式标志，它指定从哪里计算偏移量
// ios::beg 从文件头开始计算偏移量
// ios::end 从文件末尾开始计算偏移量
// ios::cur 从当前位置开始计算偏移量
lidar_data_file.seekg(0, std::ios::end); //追溯到流的尾部
// tellp 用于返回写入位置，tellg 则用于返回读取位置
const size_t num_elements = lidar_data_file.tellg() / sizeof(float); //获取流的长度
lidar_data_file.seekg(0, std::ios::beg); //回到流的头部
std::vector<float> lidar_data_buffer(num_elements);
// 函数原型istream& read (char* s, streamsize n); //用来暂存内容的数组(必须是char*型), 以及流的长度
lidar_data_file.read(reinterpret_cast<char*>(&lidar_data_buffer[0]), num_elements * sizeof(float));
return lidar_data_buffer;
}
int main(int argc, char** argv){
ros::init(argc, argv, "kitti_helper");
ros::NodeHandle n("~");
// 读取launch文件中的参数：数据文件夹位置，是否输出rosbag以及存放位置和延迟（频率）
std::string dataset_folder, sequence_number, output_bag_file;
n.getParam("dataset_folder", dataset_folder);
n.getParam("sequence_number", sequence_number);
std::cout << "Reading sequence " << sequence_number << " from " << dataset_folder << '\n';
bool to_bag;
n.getParam("to_bag", to_bag);
if (to_bag)
n.getParam("output_bag_file", output_bag_file);
int publish_delay;
n.getParam("publish_delay", publish_delay);
publish_delay = publish_delay <= 0 ? 1 : publish_delay;
// 定义点云发布者
ros::Publisher pub_laser_cloud = n.advertise<sensor_msgs::PointCloud2>("/velodyne_points", 2);
// 定义图片发布者
image_transport::ImageTransport it(n);
image_transport::Publisher pub_image_left = it.advertise("/image_left", 2);
image_transport::Publisher pub_image_right = it.advertise("/image_right", 2);
// 定义里程计真值发布者
ros::Publisher pubOdomGT = n.advertise<nav_msgs::Odometry> ("/odometry_gt", 5);
nav_msgs::Odometry odomGT;
odomGT.header.frame_id = "/camera_init";
odomGT.child_frame_id = "/ground_truth";
// 定义路径真值发布者
ros::Publisher pubPathGT = n.advertise<nav_msgs::Path> ("/path_gt", 5);
nav_msgs::Path pathGT;
pathGT.header.frame_id = "/camera_init";
// 时间信息
std::string timestamp_path = "sequences/" + sequence_number + "/times.txt";
std::ifstream timestamp_file(dataset_folder + timestamp_path, std::ifstream::in);
// 真值信息
std::string ground_truth_path = "poses/" + sequence_number + ".txt";
std::ifstream ground_truth_file(dataset_folder + ground_truth_path, std::ifstream::in);
rosbag::Bag bag_out;
if (to_bag)
bag_out.open(output_bag_file, rosbag::bagmode::Write);
// kitti的训练集真值pose的坐标系和点云的坐标系不相同, 真值z向前，x向右，y向下
Eigen::Matrix3d R_transform;
R_transform << 0, 0, 1, -1, 0, 0, 0, -1, 0;
Eigen::Quaterniond q_transform(R_transform);
std::string line;
std::size_t line_num = 0;
// 定义发布频率
ros::Rate r(10.0 / publish_delay);
while (std::getline(timestamp_file, line) && ros::ok())
{
// 读取图片数据
float timestamp = stof(line);
std::stringstream left_image_path, right_image_path;
left_image_path << dataset_folder << "sequences/" + sequence_number + "/image_0/" << std::setfill('0') << std::setw(6) << line_num << ".png";
cv::Mat left_image = cv::imread(left_image_path.str(), CV_LOAD_IMAGE_GRAYSCALE);
right_image_path << dataset_folder << "sequences/" + sequence_number + "/image_1/" << std::setfill('0') << std::setw(6) << line_num << ".png";
cv::Mat right_image = cv::imread(left_image_path.str(), CV_LOAD_IMAGE_GRAYSCALE);
// 读取真值数据
std::getline(ground_truth_file, line);
std::stringstream pose_stream(line);
std::string s;
Eigen::Matrix<double, 3, 4> gt_pose;
for (std::size_t i = 0; i < 3; ++i)
{
for (std::size_t j = 0; j < 4; ++j)
{
std::getline(pose_stream, s, ' ');
gt_pose(i, j) = stof(s);
}
}
// 旋转, 其实更准确的应该使用KITTI提供的左相机到Lidar的标定参数进行变换
Eigen::Quaterniond q_w_i(gt_pose.topLeftCorner<3, 3>());
Eigen::Quaterniond q = q_transform * q_w_i;
q.normalize();
// 平移
Eigen::Vector3d t = q_transform * gt_pose.topRightCorner<3, 1>();
odomGT.header.stamp = ros::Time().fromSec(timestamp);
odomGT.pose.pose.orientation.x = q.x();
odomGT.pose.pose.orientation.y = q.y();
odomGT.pose.pose.orientation.z = q.z();
odomGT.pose.pose.orientation.w = q.w();
odomGT.pose.pose.position.x = t(0);
odomGT.pose.pose.position.y = t(1);
odomGT.pose.pose.position.z = t(2);
pubOdomGT.publish(odomGT);
/*
PoseStamped.msg
#定义有时空基准的位姿
#文件位置：geometry_msgs/PoseStamped.msg
Header header
Pose pose
*/
geometry_msgs::PoseStamped poseGT;
poseGT.header = odomGT.header;
poseGT.pose = odomGT.pose.pose;
pathGT.header.stamp = odomGT.header.stamp;
pathGT.poses.push_back(poseGT);
pubPathGT.publish(pathGT);
// 读取点云数据，转换为ROS格式并发布
std::stringstream lidar_data_path;
lidar_data_path << dataset_folder << "sequences/" + sequence_number + "/velodyne/"
<< std::setfill('0') << std::setw(6) << line_num << ".bin";
std::vector<float> lidar_data = read_lidar_data(lidar_data_path.str());
std::cout << "totally " << lidar_data.size() / 4.0 << " points in this lidar frame \n";
std::vector<Eigen::Vector3d> lidar_points; // lidar的x y z 坐标
std::vector<float> lidar_intensities; // lidar的强度
pcl::PointCloud<pcl::PointXYZI> laser_cloud; // 含强度点云信息
for (std::size_t i = 0; i < lidar_data.size(); i += 4)
{
lidar_points.emplace_back(lidar_data[i], lidar_data[i+1], lidar_data[i+2]);
lidar_intensities.push_back(lidar_data[i+3]);
pcl::PointXYZI point;
point.x = lidar_data[i];
point.y = lidar_data[i + 1];
point.z = lidar_data[i + 2];
point.intensity = lidar_data[i + 3];
laser_cloud.push_back(point);
}
sensor_msgs::PointCloud2 laser_cloud_msg;
pcl::toROSMsg(laser_cloud, laser_cloud_msg);
laser_cloud_msg.header.stamp = ros::Time().fromSec(timestamp);
laser_cloud_msg.header.frame_id = "/camera_init";
pub_laser_cloud.publish(laser_cloud_msg);
// 将图像数据转换为ROS格式并发布
// ROS header 该消息的encoding以及 OpenCV的Mat格式的图像 https://blog.csdn.net/bigdog_1027/article/details/79090571
sensor_msgs::ImagePtr image_left_msg = cv_bridge::CvImage(laser_cloud_msg.header, "mono8", left_image).toImageMsg();
sensor_msgs::ImagePtr image_right_msg = cv_bridge::CvImage(laser_cloud_msg.header, "mono8", right_image).toImageMsg();
pub_image_left.publish(image_left_msg);
pub_image_right.publish(image_right_msg);
// 将数据写出ROSBag包中
if (to_bag)
{
bag_out.write("/image_left", ros::Time::now(), image_left_msg);
bag_out.write("/image_right", ros::Time::now(), image_right_msg);
bag_out.write("/velodyne_points", ros::Time::now(), laser_cloud_msg);
bag_out.write("/path_gt", ros::Time::now(), pathGT);
bag_out.write("/odometry_gt", ros::Time::now(), odomGT);
}
line_num ++;
r.sleep();
}
bag_out.close();
std::cout << "Done \n";
return 0;
}2.scanRegistration.cpp 注释
#include <cmath>
#include <vector>
#include <string>
#include "aloam_velodyne/common.h"
#include "aloam_velodyne/tic_toc.h"
// OPENCV
#include <opencv/cv.h>
// PCL
#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>
// ROS
#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/PointCloud2.h>
#include <nav_msgs/Odometry.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-代表曲率比较大, 0-曲率比较小, -1-代表曲率很小(其中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(时间戳)和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)
{ //计算每个点的xyz的平方和是否小于阈值的平方和
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; //如果是则continue
cloud_out.points[j] = cloud_in.points[i]; //如果不是则将这点保留下来
j++;
}
//有点被剔除时，size改变
if (j != cloud_in.points.size())
{
cloud_out.points.resize(j);
}
//收到的lidar点云是一个有序的点云
//数据行数，默认1为无组织的数据，因为要对一些点做去除，所以只能将3维空间的点降到2维，即将线数默认为1，宽度为n的所有值
cloud_out.height = 1; //高度代表线数
//可以理解成点数
cloud_out.width = static_cast<uint32_t>(j); //每一次scan的宽度
//点数是否有限
cloud_out.is_dense = true;
}
//订阅点云句柄 形参叫做收到这个点云的消息
void laserCloudHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudMsg)
{
/* ***********************************************3D点云预处理**********************************************************/
//是否剔除前systemDelay帧 如果系统没有初始化的话，就等几帧 通过设置systemDelay大小
if (!systemInited)
{
systemInitCount++;
if (systemInitCount >= systemDelay)
{
systemInited = true;
}
else
return;
}
//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包的点云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);
/* **********************************************以下计算都是为了记录每个点是第几根线和扫描的时间的多少,laserCloudScans***************************************************** */
//计算起始点和结束点的水平角度
//该次sweep的点数
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);
//此处加2pi是为了保证起始点到结束点相差2pi，符合实际，按照原理上起始点应该和结束点在一个位置，所以加2pi区分
float endOri = -atan2(laserCloudIn.points[cloudSize - 1].y, laserCloudIn.points[cloudSize - 1].x) + 2 * M_PI;
//总有一些例外，激光间距收束到1pi~3pi，本应相减差2pi
//去除补偿2pi的情况
if (endOri - startOri > 3 * M_PI)
{
endOri -= 2 * M_PI;
}
//额外补偿2pi的情况
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)
{
//16线的激光雷达是30度（-15,15）+15为了补偿，每相邻的scan的夹角是2度，+0.5是为了四舍五入
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 outlie
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;
}
//这种case不会发生
else if (ori > startOri + M_PI * 3 / 2)
{
ori -= 2 * M_PI;
}
//如果超过180度，就说明过了一半了
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; //两种情况，补偿2pi
}
else if (ori > endOri + M_PI / 2)
{
ori -= 2 * M_PI; //去除补偿的2pi
}
}
}
//角度的计算是为了看看旋转多少了，记录比例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（一维数组）一个数据集合里，把每条线的第五个和倒数第五个点在laserCloud中的位置反馈给scanStartInd和scanEndInd
//这10个点的曲率计算不方便，所就不计算，scanStartInd和scanEndInd用来记录每条线的起始和结束的id（去除前五和后五之后的）
pcl::PointCloud<PointType>::Ptr laserCloud(new pcl::PointCloud<PointType>());
for (int i = 0; i < N_SCANS; i++)
{
scanStartInd[i] = laserCloud->size() + 5;
*laserCloud += laserCloudScans[i];
scanEndInd[i] = laserCloud->size() - 6;
}
//预处理部分终于完成了
printf("prepare time %f \n", t_prepare.toc());
/* **************************************************特征值提取********************************************************* */
//开始计算曲率
//十点求曲率，自然是在一条线上的十个点，利用前后5个点计算每个点的曲率（前后5点不计算）
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;
//开始提取特征
//遍历每条scan
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++)
{
//每一个等分计算起始和终结的id，sp和ep表示在laserCloud中的位置
//六等份起点：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;
//返回的是结束地址后面那一个，comp定义的是排序是按照从大到小对cloudSortInd[sp, ep + 1]进行重新排序
//本来cloudSortInd[1,6]分别是12345 排序之后变成245613，现在cloudSortInd[3]的值表示laserCloud中第5个索引
std::sort (cloudSortInd + sp, cloudSortInd + ep + 1, comp);
t_q_sort += t_tmp.toc();
//挑选每个分段的曲率很大和比较大的点
int largestPickedNum = 0;
for (int k = ep; k >= sp; k--)
{
//排序后cloudSortInd顺序就乱了，这个时候索引的作用就体现出来了
//ind的值表示为在laserCloud中的位置 ind=cloudSortInd[k]=k,laserCloud[ind]表此点,cloudCurvature[ind]表示此点曲率
int ind = cloudSortInd[k];
//如果筛选标志为0，并且曲率较大 阈值为0.1
if (cloudNeighborPicked[ind] == 0 && cloudCurvature[ind] > 0.1)
{
//则曲率大的点记数一下
largestPickedNum++;
//满足阈值的点少于等于两个（将满足条件的最大曲率的两个点放入cornerPointsSharp中）
if (largestPickedNum <= 2)
{
cloudLabel[ind] = 2;
cornerPointsSharp.push_back(laserCloud->points[ind]);
cornerPointsLessSharp.push_back(laserCloud->points[ind]);
}
//保存曲率较大的前20个角点放入cornerPointsLessSharp中
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;
}
}
//将小于阈值0.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);
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"); //第三个参数是节点的名称，在launch文件会指定
ros::NodeHandle nh; //ros句柄
//参数，线数
nh.param<int>("sacn_line", N_SCANS, 16); //读取参数，默认情况为16
//参数，过近去除 (最后一个参数为默认值) 最小有效距离
nh.param<double>("minimum_range", MINIMUM_RANGE, 0.1);
printf("scan line number %d \n", N_SCANS);
//只有线数是16 32 64 的才可以继续运行
if(N_SCANS != 16 && N_SCANS != 32 && N_SCANS != 64)
{
printf("only support velodyne with 16, 32 or 64 scan line!");
return 0;
}
//接收激光雷达信号 订阅/velodyne_points这个消息，ros包发出的消息名被叫做这个，一旦每次收到了这个消息，就会执行一次回调函数laserCloudHandler
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);
//发布每行scan
if(PUB_EACH_LINE)
{
for(int i = 0; i < N_SCANS; i++)
{
ros::Publisher tmp = nh.advertise<sensor_msgs::PointCloud2>("/laser_scanid_" + std::to_string(i), 100);
pubEachScan.push_back(tmp);
}
}
ros::spin();
return 0;
}3.laserOdometry.cpp 注释
#include <cmath>
#include <mutex>
#include <queue>
#include <eigen3/Eigen/Dense>
// PCL
#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>
// ROS
#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 <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include <geometry_msgs/PoseStamped.h>
#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<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);
// 点云特征匹配时的优化变量
double para_q[4] = {0, 0, 0, 1};
double para_t[3] = {0, 0, 0};
// 下面的2个分别是优化变量para_q和para_t的映射：表示的是两个world坐标系下的位姿P之间的增量，例如△P = P0.inverse() * P1
Eigen::Map<Eigen::Quaterniond> q_last_curr(para_q); // Map相当于引用
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
double s;
if (DISTORTION) //若点位还未去除畸变 intensity中小数部分代表扫描到这个点所用的时间
s = (pi->intensity - int(pi->intensity)) / SCAN_PERIOD; // 用s求解某个点在本次scan中在的比例
else
s = 1.0; // kitti点云已经去除了畸变，所以不再考虑运动补偿
//s = 1;
//再根据比例求解变换矩阵的变换比例，再求出推理位姿
//这里相当于是一个匀速模型的假设 q_point_last代表的旋转的差值 q_last_curr代表已知的上个时刻从end到start的旋转
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;
}
//把点补偿到最后时刻
void TransformToEnd(PointType const *const pi, PointType *const po)
{
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;
//从配置文件拿到一个参数 skipFrameNum是下采样的频率
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);
ros::Publisher pubLaserOdometry = nh.advertise<nav_msgs::Odometry>("/laser_odom_to_init", 100);
ros::Publisher pubLaserPath = nh.advertise<nav_msgs::Path>("/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();
//如果订阅的东西应有尽有，首先保证订阅的5个消息都有
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格式消息转换为PCL格式消息
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();
/* **************************************************************lidar里程计的求解**************************************************** */
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;
// 点到线以及点到面的ICP，迭代2次
for (size_t opti_counter = 0; opti_counter < 2; ++opti_counter)
{
//匹配数量
corner_correspondence = 0;
plane_correspondence = 0;
//定义ceres的核函数 当残差超过0.1之后，就会把权重变小
ceres::LossFunction *loss_function = new ceres::HuberLoss(0.1);
//为Eigen的表示实现四元数局部参数
//输入顺序为[w，x，y，z]
//由于旋转不满足一般意义的加法，因此这里使用ceres自带的local param
ceres::LocalParameterization *q_parameterization = new ceres::EigenQuaternionParameterization();
ceres::Problem::Options problem_options; // 问题选项
ceres::Problem problem(problem_options); //定义了一个优化问题problem
//待优化的变量是帧间位姿，平移和旋转，这里旋转使用四元数来表示
problem.AddParameterBlock(para_q, 4, q_parameterization); //这个接口是为了告诉ceres哪个是待优化变量
problem.AddParameterBlock(para_t,3);//第一个是数组，第二个是参数的长度，第三个参数表示如果不满足加法定义，需要引入local param
pcl::PointXYZI pointSel;
std::vector<int> pointSearchInd;
std::vector<float> pointSearchSqDis;
TicToc t_data;
// find correspondence for corner features
// 基于最近邻（只找2个最近邻点）原理建立corner特征点之间关联，
// 寻找角点的约束
for (int i = 0; i < cornerPointsSharpNum; ++i)
{
// 将当前帧的corner_sharp特征点O_cur，从当前帧的Lidar坐标系下变换到上一帧的Lidar坐标系下（记为点O，注意与前面的点O_cur不同），
// 以利于寻找corner特征点的correspondence
TransformToStart(&(cornerPointsSharp->points[i]), &pointSel); //运动畸变补偿到这一帧的开始
// 使用KD-tree求解相对上一帧里点云，pointSel和他们的距离，返回一个最近点的点云线数pointSearchInd和距离pointSearchSqDis
// 需要注意的是KD-tree中存放的特征点是在上一帧的结束位置，和pointSel这一帧的起始位置是同一个位置
// 可以看https://zhuanlan.zhihu.com/p/112246942
// kdtree中的点云是上一帧的corner_less_sharp，所以这是在上一帧的corner_less_sharp中寻找当前帧corner_sharp特征点O的最近邻点（记为A）
kdtreeCornerLast->nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis); //1表示找一个点
// closestPointInd 是离pointSel最近点的索引
// minPointInd2 是最近点以外离pointSel最近点的索引
int closestPointInd = -1, minPointInd2 = -1;
if (pointSearchSqDis[0] < DISTANCE_SQ_THRESHOLD) // 如果最近邻的corner特征点之间距离平方小于阈值，则最近邻点A有效
{
closestPointInd = pointSearchInd[0]; // 最近点索引 应该是在kd-tree中的索引
// 找到最近点所在线号id，线束信息藏在intensity的整数部分
int closestPointScanID = int(laserCloudCornerLast->points[closestPointInd].intensity);
// 最短距离之后更新
double minPointSqDis2 = DISTANCE_SQ_THRESHOLD; //25
// 寻找点O的另外一个最近邻的点（记为点B） in the direction of increasing scan line
// laserCloudCornerLast 来自上一帧的corner_less_sharp特征点,由于提取特征时是
// 按照scan的顺序提取的，所以laserCloudCornerLast中的点也是按照scanID递增的顺序存放的
// 找临近线的点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)// intensity整数部分存放的是scanID
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记录id和距离
minPointSqDis2 = pointSqDis; // 第二个最近邻点有效，更新点B
minPointInd2 = j;
}
}
// 即特征点O的两个最近邻点A和B都有效
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;// 运动补偿系数，kitti数据集的点云已经被补偿过，所以s = 1.0
if (DISTORTION)
s = (cornerPointsSharp->points[i].intensity - int(cornerPointsSharp->points[i].intensity)) / SCAN_PERIOD;
else
s = 1.0;
// 用点O，A，B构造点到线的距离的残差项，注意这三个点都是在上一帧的Lidar坐标系下，即，残差 = 点O到直线AB的距离
// 具体到介绍lidarFactor.cpp时再说明该残差的具体计算方法
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
// 下面说的点符号与上述相同
// 与上面的建立corner特征点之间的关联类似，寻找平面特征点O的最近邻点ABC（只找3个最近邻点），
// 即基于最近邻原理建立surf特征点之间的关联，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)// 找到的最近邻点A有效
{
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;// 找到的第2个最近邻点有效，更新点B，注意如果scanID准确的话，一般点A和点B的scanID相同
minPointInd2 = j;
}
// if in the higher scan line
else if (int(laserCloudSurfLast->points[j].intensity) > closestPointScanID && pointSqDis < minPointSqDis3)
{
minPointSqDis3 = pointSqDis;// 找到的第3个最近邻点有效，更新点C，注意如果scanID准确的话，一般点A和点B的scanID相同,且与点C的scanID不同，与LOAM的paper叙述一致
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;
// 用点O，A，B，C构造点到面的距离的残差项，注意这三个点都是在上一帧的Lidar坐标系下，即，残差 = 点O到平面ABC的距离
// 同样的，具体到介绍lidarFactor.cpp时再说明该残差的具体计算方法
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");
}
// ceres优化
TicToc t_solver;
ceres::Solver::Options options; // solver选项
options.linear_solver_type = ceres::DENSE_QR; //利用稠密矩阵求解方式qr
options.max_num_iterations = 4; //最大迭代次数
options.minimizer_progress_to_stdout = false;
ceres::Solver::Summary summary;
// 基于构建的所有残差项，求解最优的当前帧位姿与上一帧位姿的位姿增量：para_q和para_t
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]);
}
}
// 用cornerPointsLessSharp和surfPointsLessFlat 更新 laserCloudCornerLast和laserCloudSurfLast以及相应的kdtree，
// 为下一次点云特征匹配提供target
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;
}4.laserMapping.cpp 注释
#include <math.h>
#include <vector>
#include <mutex>
#include <queue>
#include <thread>
#include <iostream>
#include <string>
#include <eigen3/Eigen/Dense>
#include <ceres/ceres.h>
// PCL
#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>
// ROS
#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 <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include <geometry_msgs/PoseStamped.h>
#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;
// cube的总数量，也就是上图中的所有小格子个总数量 21 * 21 * 11 = 4851
const int laserCloudNum = laserCloudWidth * laserCloudHeight * laserCloudDepth;
// LOAM对submap中cube的管理都是以1维数组的形式
// 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>());
// 存放cube点云特征的数组，数组大小4851，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};
// Mapping线程估计的frame在world坐标系的位姿P,因为Mapping的算法耗时很有可能会超过100ms，所以
// 这个位姿P不是实时的，LOAM最终输出的实时位姿P_realtime,需要Mapping线程计算的相对低频位姿和
// Odometry线程计算的相对高频位姿做整合，详见后面laserOdometryHandler函数分析。此外需要注意
// 的是，不同于Odometry线程，这里的位姿P，即q_w_curr和t_w_curr，本身就是匹配时的优化变量。
Eigen::Map<Eigen::Quaterniond> q_w_curr(parameters);
Eigen::Map<Eigen::Vector3d> t_w_curr(parameters + 4);
// 下面的两个变量是world坐标系下的Odometry计算的位姿和Mapping计算的位姿之间的增量（也即变换，transformation）
// wmap_odom * wodom_curr = wmap_curr(即前面的q/t_w_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);
// Odometry线程计算的frame在world坐标系的位姿
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;
nav_msgs::Path laserAfterMappedPath;
// set initial guess，上一帧的增量wmap_wodom * 本帧Odometry位姿wodom_curr，旨在为本帧Mapping位姿w_curr设置一个初始值
void transformAssociateToMap()
{
q_w_curr = q_wmap_wodom * q_wodom_curr;
t_w_curr = q_wmap_wodom * t_wodom_curr + t_wmap_wodom;
}
// 用在最后，当Mapping的位姿w_curr计算完毕后，更新增量wmap_wodom，旨在为下一次执行transformAssociateToMap函数时做准备
// 更新odom到map之间的位姿变换
void transformUpdate()
{
q_wmap_wodom = q_w_curr * q_wodom_curr.inverse();
t_wmap_wodom = t_w_curr - q_wmap_wodom * t_wodom_curr;
}
// 用Mapping的位姿w_curr，将Lidar坐标系下的点变换到world坐标系下
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;
}
// 这个没有用到，是上面pointAssociateToMap的逆变换，即用Mapping的位姿w_curr，将world坐标系下的点变换到Lidar坐标系下
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 里程计坐标系到map坐标系
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)
{
// 为了保证LOAM算法整体的实时性，Mapping线程每次只处理cornerLastBuf.front()及其他与之时间同步的消息
while (!cornerLastBuf.empty() && !surfLastBuf.empty() &&
!fullResBuf.empty() && !odometryBuf.empty())
{
mBuf.lock();
// odometryBuf只保留一个与cornerLastBuf.front()时间同步的最新消息
while (!odometryBuf.empty() && odometryBuf.front()->header.stamp.toSec() < cornerLastBuf.front()->header.stamp.toSec())
odometryBuf.pop();
if (odometryBuf.empty())
{
mBuf.unlock();
break;
}
// surfLastBuf也如此
while (!surfLastBuf.empty() && surfLastBuf.front()->header.stamp.toSec() < cornerLastBuf.front()->header.stamp.toSec())
surfLastBuf.pop();
if (surfLastBuf.empty())
{
mBuf.unlock();
break;
}
// fullResBuf也如此
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();
// 清空cornerLastBuf的历史缓存，为了LOAM的整体实时性
while(!cornerLastBuf.empty())
{
cornerLastBuf.pop();
printf("drop lidar frame in mapping for real time performance \n");
}
mBuf.unlock();
TicToc t_whole;
// 上一帧的增量wmap_wodom * 本帧Odometry位姿wodom_curr，旨在为本帧Mapping位姿w_curr设置一个初始值
transformAssociateToMap();
/************************************************ 以下操作相当于维护了一个局部地图 ***************************************/
TicToc t_shift;
// 下面这是计算当前帧lidar位姿t_w_curr（在上图中用红色五角星表示的位置）IJK坐标（三维珊格的坐标），
// 因为LOAM用1维数组进行cube的管理，而数组的index只用是正数，所以要保证IJK坐标都是正数，所以加了laserCloudCenWidth/Heigh/Depth
// 的偏移，来使得当前位置尽量位于submap的中心处，也就是使得上图中的五角星位置尽量处于所有格子的中心附近，
// 偏移laserCloudCenWidth/Heigh/Depth会动态调整，来保证当前位置尽量位于submap的中心处。
// 360度旋转式的激光雷达不需要考虑旋转，livox需要考虑旋转
// 一开始地图是21*21*11
// JQ：根据初始估计值计算寻找当前位姿在地图中的索引，一个格的边长是50m，/50算出具体在哪个珊格，+25是为了取整，四舍五入
// 后端的地图本质上是一个以当前点为中心的一个珊格地图
int centerCubeI = int((t_w_curr.x() + 25.0) / 50.0) + laserCloudCenWidth; //10
int centerCubeJ = int((t_w_curr.y() + 25.0) / 50.0) + laserCloudCenHeight; //10
int centerCubeK = int((t_w_curr.z() + 25.0) / 50.0) + laserCloudCenDepth; //5
// 由于计算机求余是向零取整，因此，例如-1.66就取成了-1,但是应该是-2才正确，因此这里自减一
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--;
// 注意世界坐标系下的点云地图是固定的，但是IJK坐标系我们是可以移动的，所以这6个while loop
// 的作用就是调整IJK坐标系（也就是调整所有cube位置），使得五角星在IJK坐标系的坐标范围处于
// 3 < centerCubeI < 18， 3 < centerCubeJ < 18, 3 < centerCubeK < 8，目的是为了防止后续向
// 四周拓展cube（图中的黄色cube就是拓展的cube）时，index（即IJK坐标）成为负数。
while (centerCubeI < 3) //小于3代表危险的情况，需要进行移动
{
for (int j = 0; j < laserCloudHeight; j++)
{
for (int k = 0; k < laserCloudDepth; k++)
{
int i = laserCloudWidth - 1;
//下面是已知ijk在三维地图的已知坐标，如何转化到二位数组的操作
//以下是线点
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 >= 1; i--)// 在I方向cube[I]=cube[I-1],清空最后一个空出来的cube，实现IJK坐标系向I轴负方向移动一个cube的
// 效果，从相对运动的角度看是图中的五角星在IJK坐标系下向I轴正方向移动了一个cube，如下面动图所示，所
// 以centerCubeI最后++，laserCloudCenWidth也++，为下一帧Mapping时计算五角星的IJK坐标做准备
{
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++;
}
}
//以下同理，对y和z方向进行移动
//如果处于上边界，表明地图向正方向延伸的可能性比较大，则循环移位，将数组中心点向下边界调整一个单位
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--;
}
/************************************************ 以下操作是创建一个更小的局部地图用来特征点的匹配 ***************************************/
// 下面的两个定义可以认为是清空submap
int laserCloudValidNum = 0;
int laserCloudSurroundNum = 0;
// 向IJ坐标轴的正负方向各拓展2个cube，K坐标轴的正负方向各拓展1个cube，这些cube就是submap的范围
//从当前格子为中心，选出地图中一定范围的点云，从局部地图中选出一个更小的局部地图
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)// 如果坐标合法
{
// 记录一维数组submap中的所有cube的index，记为有效index
laserCloudValidInd[laserCloudValidNum] = i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k;
laserCloudValidNum++;
laserCloudSurroundInd[laserCloudSurroundNum] = i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k;
laserCloudSurroundNum++;
}
}
}
}
laserCloudCornerFromMap->clear(); //更小的局部地图分为线地图和面地图
laserCloudSurfFromMap->clear();
//开始构建用来这一帧优化的小的局部地图
for (int i = 0; i < laserCloudValidNum; i++)
{
// 将有效index的cube中的点云叠加到一起组成submap的特征点云
*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;
//将线地图和点地图（地图坐标系中的点）送入kdtree便于最近临的搜索
kdtreeCornerFromMap->setInputCloud(laserCloudCornerFromMap);
kdtreeSurfFromMap->setInputCloud(laserCloudSurfFromMap);
printf("build tree time %f ms \n", t_tree.toc());
for (int iterCount = 0; iterCount < 2; iterCount++)
{
//建立ceres问题 此处和前端类似
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);//前四个数据是旋转，所以平移是+4
TicToc t_data;
int corner_num = 0;
//构建角点相关的约束
for (int i = 0; i < laserCloudCornerStackNum; i++)
{
// 对于每一个降采样后的角点
pointOri = laserCloudCornerStack->points[i];
// 需要注意的是submap中的点云都是world坐标系，而当前帧的点云都是Lidar坐标系，所以
// 在搜寻最近邻点时，先用预测的Mapping位姿w_curr，将Lidar坐标系下的特征点变换到world坐标系下
pointAssociateToMap(&pointOri, &pointSel);
// 在submap的corner特征点（target）中，寻找距离当前帧corner特征点（source）最近的5个点
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);
}
// 计算这个5个最近邻点的中心
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();
}
//特征值分解 计算协方差矩阵的特征值和特征向量，用于判断这5个点是不是呈线状分布，此为PCA的原理
Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> saes(covMat);
// if is indeed line feature
// note Eigen library sort eigenvalues in increasing order
Eigen::Vector3d unit_direction = saes.eigenvectors().col(2);// 如果5个点呈线状分布，最大的特征值对应的特征向量就是该线的方向向量
Eigen::Vector3d curr_point(pointOri.x, pointOri.y, pointOri.z);
if (saes.eigenvalues()[2] > 3 * saes.eigenvalues()[1]) // 如果最大的特征值 >> 其他特征值，则5个点确实呈线状分布，否则认为直线“不够直”
{
Eigen::Vector3d point_on_line = center;
Eigen::Vector3d point_a, point_b;
// 从中心点沿着方向向量向两端移动0.1m，构造线上的两个点
point_a = 0.1 * unit_direction + point_on_line;
point_b = -0.1 * unit_direction + point_on_line;
// 然后残差函数的形式就跟Odometry一样了，残差距离即点到线的距离，到介绍lidarFactor.cpp时再说明具体计算方法
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);
//也找5个最近邻点
kdtreeSurfFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis);
// 求面的法向量就不是用的PCA了（虽然论文中说还是PCA），使用的是最小二乘拟合，是为了提效？不确定
// 假设平面不通过原点，则平面的一般方程为Ax + By + Cz + 1 = 0，用这个假设可以少算一个参数，提效。
Eigen::Matrix<double, 5, 3> matA0;
Eigen::Matrix<double, 5, 1> matB0 = -1 * Eigen::Matrix<double, 5, 1>::Ones();
// 用上面的2个矩阵表示平面方程就是 matA0 * norm（A, B, C） = matB0，这是个超定方程组，因为数据个数超过未知数的个数
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;
}
// 求解这个最小二乘问题，可得平面的法向量，find the norm of plane
Eigen::Vector3d norm = matA0.colPivHouseholderQr().solve(matB0); //norm表示abc向量
// Ax + By + Cz + 1 = 0，全部除以法向量的模长，方程依旧成立，而且使得法向量归一化了
double negative_OA_dot_norm = 1 / norm.norm(); // D
norm.normalize(); //法向量归一化
// Here n(pa, pb, pc) is unit norm of plane
bool planeValid = true;
for (int j = 0; j < 5; j++)
{
// 点(x0, y0, z0)到平面Ax + By + Cz + D = 0 的距离公式 = fabs(Ax0 + By0 + Cz0 + D) / sqrt(A^2 + B^2 + C^2)
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)
{
// 构造点到面的距离残差项，同样的，具体到介绍lidarFactor.cpp时再说明该残差的具体计算方法
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");
}
// 完成ICP（迭代2次）的特征匹配后，用最后匹配计算出的优化变量w_curr，更新增量wmap_wodom，为下一次Mapping做准备
transformUpdate();
/************************************************************* 局部地图点的更新 *****************************************************/
TicToc t_add;
// 下面两个for loop的作用就是将当前帧的特征点云，逐点进行操作：转换到world坐标系并添加到局部地图中
for (int i = 0; i < laserCloudCornerStackNum; i++)
{
// Lidar坐标系转到world坐标系
pointAssociateToMap(&laserCloudCornerStack->points[i], &pointSel);
// 计算本次的特征点的IJK坐标，进而确定添加到哪个格子中
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)
{
//根据xyz的索引计算在一维数组中的索引
int cubeInd = cubeI + laserCloudWidth * cubeJ + laserCloudWidth * laserCloudHeight * cubeK;
//把当前帧加入局部地图中去
laserCloudCornerArray[cubeInd]->push_back(pointSel);
}
}
//面点也做的同样的处理
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;
// 因为新增加了点云，对之前已经存有点云的cube全部重新进行一次降采样
// 这个地方可以简单优化一下：如果之前的cube没有新添加点就不需要再降采样
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;
//publish surround map for every 5 frame
//小的局部地图每隔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);
//发布tf
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 为了避免上述操作一直占用cpu，保证cpu在这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<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}; //mapping线程，在process函数中
ros::spin();
return 0;
}5.lidarFactor.hpp 注释
#include <ceres/ceres.h>
#include <ceres/rotation.h>
#include <eigen3/Eigen/Dense>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl_conversions/pcl_conversions.h>
// 点到线的残差距离计算
struct LidarEdgeFactor
{
LidarEdgeFactor(Eigen::Vector3d curr_point_, Eigen::Vector3d last_point_a_,
Eigen::Vector3d last_point_b_, double s_)
: curr_point(curr_point_), last_point_a(last_point_a_), last_point_b(last_point_b_), s(s_) {}
template <typename T>
bool operator()(const T *q, const T *t, T *residual) const //参数为 待优化的变量和残差
{
//将double数组转化为eigen的数据结构，注意这里必须都写为模板
Eigen::Matrix<T, 3, 1> cp{T(curr_point.x()), T(curr_point.y()), T(curr_point.z())};
Eigen::Matrix<T, 3, 1> lpa{T(last_point_a.x()), T(last_point_a.y()), T(last_point_a.z())};
Eigen::Matrix<T, 3, 1> lpb{T(last_point_b.x()), T(last_point_b.y()), T(last_point_b.z())};
//Eigen::Quaternion<T> q_last_curr{q[3], T(s) * q[0], T(s) * q[1], T(s) * q[2]};
Eigen::Quaternion<T> q_last_curr{q[3], q[0], q[1], q[2]};
Eigen::Quaternion<T> q_identity{T(1), T(0), T(0), T(0)};
// 考虑运动补偿，ktti点云已经补偿过所以可以忽略下面的对四元数slerp插值以及对平移的线性插值
q_last_curr = q_identity.slerp(T(s), q_last_curr);
Eigen::Matrix<T, 3, 1> t_last_curr{T(s) * t[0], T(s) * t[1], T(s) * t[2]};
Eigen::Matrix<T, 3, 1> lp;
// Odometry线程时，下面是将当前帧Lidar坐标系下的cp点变换到上一帧的Lidar坐标系下，然后在上一帧的Lidar坐标系计算点到线的残差距离
// Mapping线程时，下面是将当前帧Lidar坐标系下的cp点变换到world坐标系下，然后在world坐标系下计算点到线的残差距离
lp = q_last_curr * cp + t_last_curr;
Eigen::Matrix<T, 3, 1> nu = (lp - lpa).cross(lp - lpb);
Eigen::Matrix<T, 3, 1> de = lpa - lpb;
// 最终的残差本来应该是residual[0] = nu.norm() / de.norm(); 为啥也分成3个，我也不知
// 道，从我试验的效果来看，确实是下面的残差函数形式，最后输出的pose精度会好一点点，这里需要
// 注意的是，所有的residual都不用加fabs，因为Ceres内部会对其求 平方 作为最终的残差项
residual[0] = nu.x() / de.norm();
residual[1] = nu.y() / de.norm();
residual[2] = nu.z() / de.norm();
return true;
}
static ceres::CostFunction *Create(const Eigen::Vector3d curr_point_, const Eigen::Vector3d last_point_a_,
const Eigen::Vector3d last_point_b_, const double s_)
{
return (new ceres::AutoDiffCostFunction< //创建自动求导的模板类
LidarEdgeFactor, 3, 4, 3>(
// ^ ^ ^
// | | |
// 残差的维度 ____| | |
// 优化变量q的维度 _______| |
// 优化变量t的维度 __________|
new LidarEdgeFactor(curr_point_, last_point_a_, last_point_b_, s_)));
}
Eigen::Vector3d curr_point, last_point_a, last_point_b;
double s;
};
// 计算Odometry线程中点到面的残差距离
struct LidarPlaneFactor
{
LidarPlaneFactor(Eigen::Vector3d curr_point_, Eigen::Vector3d last_point_j_,
Eigen::Vector3d last_point_l_, Eigen::Vector3d last_point_m_, double s_)
: curr_point(curr_point_), last_point_j(last_point_j_), last_point_l(last_point_l_),
last_point_m(last_point_m_), s(s_)
{
// 点l、j、m就是搜索到的最近邻的3个点，下面就是计算出这三个点构成的平面ljlm的法向量
ljm_norm = (last_point_j - last_point_l).cross(last_point_j - last_point_m);
// 归一化法向量
ljm_norm.normalize();
}
template <typename T>
bool operator()(const T *q, const T *t, T *residual) const
{
Eigen::Matrix<T, 3, 1> cp{T(curr_point.x()), T(curr_point.y()), T(curr_point.z())};
Eigen::Matrix<T, 3, 1> lpj{T(last_point_j.x()), T(last_point_j.y()), T(last_point_j.z())};
//Eigen::Matrix<T, 3, 1> lpl{T(last_point_l.x()), T(last_point_l.y()), T(last_point_l.z())};
//Eigen::Matrix<T, 3, 1> lpm{T(last_point_m.x()), T(last_point_m.y()), T(last_point_m.z())};
Eigen::Matrix<T, 3, 1> ljm{T(ljm_norm.x()), T(ljm_norm.y()), T(ljm_norm.z())};
//Eigen::Quaternion<T> q_last_curr{q[3], T(s) * q[0], T(s) * q[1], T(s) * q[2]};
Eigen::Quaternion<T> q_last_curr{q[3], q[0], q[1], q[2]};
Eigen::Quaternion<T> q_identity{T(1), T(0), T(0), T(0)};
q_last_curr = q_identity.slerp(T(s), q_last_curr);
Eigen::Matrix<T, 3, 1> t_last_curr{T(s) * t[0], T(s) * t[1], T(s) * t[2]};
Eigen::Matrix<T, 3, 1> lp;
lp = q_last_curr * cp + t_last_curr;
// 计算点到平面的残差距离，如下图所示
residual[0] = (lp - lpj).dot(ljm);
return true;
}
static ceres::CostFunction *Create(const Eigen::Vector3d curr_point_, const Eigen::Vector3d last_point_j_,
const Eigen::Vector3d last_point_l_, const Eigen::Vector3d last_point_m_,
const double s_)
{
return (new ceres::AutoDiffCostFunction<
LidarPlaneFactor, 1, 4, 3>(
new LidarPlaneFactor(curr_point_, last_point_j_, last_point_l_, last_point_m_, s_)));
}
Eigen::Vector3d curr_point, last_point_j, last_point_l, last_point_m;
Eigen::Vector3d ljm_norm;
double s;
};
// 计算Mapping线程中点到面的残差距离
struct LidarPlaneNormFactor
{
LidarPlaneNormFactor(Eigen::Vector3d curr_point_, Eigen::Vector3d plane_unit_norm_,
double negative_OA_dot_norm_) : curr_point(curr_point_), plane_unit_norm(plane_unit_norm_),
negative_OA_dot_norm(negative_OA_dot_norm_) {}
template <typename T>
bool operator()(const T *q, const T *t, T *residual) const
{
Eigen::Quaternion<T> q_w_curr{q[3], q[0], q[1], q[2]};
Eigen::Matrix<T, 3, 1> t_w_curr{t[0], t[1], t[2]};
Eigen::Matrix<T, 3, 1> cp{T(curr_point.x()), T(curr_point.y()), T(curr_point.z())};
Eigen::Matrix<T, 3, 1> point_w;
point_w = q_w_curr * cp + t_w_curr;
Eigen::Matrix<T, 3, 1> norm(T(plane_unit_norm.x()), T(plane_unit_norm.y()), T(plane_unit_norm.z()));
// 点(x0, y0, z0)到平面Ax + By + Cz + D = 0 的距离 = fabs(A*x0 + B*y0 + C*z0 + D) / sqrt(A^2 + B^2 + C^2)，
// 因为法向量（A, B, C）已经归一化了，所以距离公式可以简写为：距离 = fabs(A*x0 + B*y0 + C*z0 + D) ，即：
residual[0] = norm.dot(point_w) + T(negative_OA_dot_norm); //残差即点到平面的距离
return true;
}
static ceres::CostFunction *Create(const Eigen::Vector3d curr_point_, const Eigen::Vector3d plane_unit_norm_,
const double negative_OA_dot_norm_)
{
return (new ceres::AutoDiffCostFunction<
LidarPlaneNormFactor, 1, 4, 3>(
new LidarPlaneNormFactor(curr_point_, plane_unit_norm_, negative_OA_dot_norm_)));
}
Eigen::Vector3d curr_point;
Eigen::Vector3d plane_unit_norm;
double negative_OA_dot_norm;
};
struct LidarDistanceFactor
{
LidarDistanceFactor(Eigen::Vector3d curr_point_, Eigen::Vector3d closed_point_)
: curr_point(curr_point_), closed_point(closed_point_){}
template <typename T>
bool operator()(const T *q, const T *t, T *residual) const
{
Eigen::Quaternion<T> q_w_curr{q[3], q[0], q[1], q[2]};
Eigen::Matrix<T, 3, 1> t_w_curr{t[0], t[1], t[2]};
Eigen::Matrix<T, 3, 1> cp{T(curr_point.x()), T(curr_point.y()), T(curr_point.z())};
Eigen::Matrix<T, 3, 1> point_w;
point_w = q_w_curr * cp + t_w_curr;
residual[0] = point_w.x() - T(closed_point.x());
residual[1] = point_w.y() - T(closed_point.y());
residual[2] = point_w.z() - T(closed_point.z());
return true;
}
static ceres::CostFunction *Create(const Eigen::Vector3d curr_point_, const Eigen::Vector3d closed_point_)
{
return (new ceres::AutoDiffCostFunction<
LidarDistanceFactor, 3, 4, 3>(
new LidarDistanceFactor(curr_point_, closed_point_)));
}
Eigen::Vector3d curr_point;
Eigen::Vector3d closed_point;
};
2533
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
