文件目录

代码1、kittiHelper.cpp2、scanRegistration.cpp3、laserOdometry.cpp4、laserMapping.cpp5、lidarFactor.cpp 参考

代码

1、kittiHelper.cpp

/* * @Author: your name * @Date: 2020-09-18 09:28:16 * @LastEditTime: 2020-10-05 21:29:26 * @LastEditors: Please set LastEditors * @Description: 读取kitti文件，发布odomGT,pathGT * @FilePath: \A-LOAM-NOTED-devel\src\kittiHelper.cpp */ // Author: Tong Qin qintonguav@gmail.com // Shaozu Cao saozu.cao@connect.ust.hk #include <iostream> #include <fstream> #include <iterator> #include <string> #include <vector> #include <opencv2/opencv.hpp> #include <image_transport/image_transport.h> #include <opencv2/highgui/highgui.hpp> #include <nav_msgs/Odometry.h> #include <nav_msgs/Path.h> #include <ros/ros.h> #include <rosbag/bag.h> #include <geometry_msgs/PoseStamped.h> #include <cv_bridge/cv_bridge.h> #include <sensor_msgs/image_encodings.h> #include <eigen3/Eigen/Dense> #include <pcl/point_cloud.h> #include <pcl/point_types.h> #include <pcl_conversions/pcl_conversions.h> #include <sensor_msgs/PointCloud2.h> std::vector<float> read_lidar_data(const std::string lidar_data_path) { std::ifstream lidar_data_file(lidar_data_path, std::ifstream::in | std::ifstream::binary); lidar_data_file.seekg(0, std::ios::end); 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); 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("~"); 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); // 是否转换为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); // 发布odomGT ros::Publisher pubOdomGT = n.advertise<nav_msgs::Odometry> ("/odometry_gt", 5); nav_msgs::Odometry odomGT; odomGT.header.frame_id = "/camera_init"; // 发布从camera_init到 ground_trurh的坐标系变换 已经转换为lidar的odom了 odomGT.child_frame_id = "/ground_truth"; // 发布pathGT 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); // 读取groundtruth_path std::string ground_truth_path = "results/" + sequence_number + ".txt"; std::ifstream ground_truth_file(dataset_folder + ground_truth_path, std::ifstream::in); // 是否导出未bag文件 rosbag::Bag bag_out; if (to_bag) bag_out.open(output_bag_file, rosbag::bagmode::Write); // camera相对于lidar的旋转矩阵 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); // 读取ground_truth_path 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); } } Eigen::Quaterniond q_w_i(gt_pose.topLeftCorner<3, 3>()); // q_w_i代表camera的odom // Eigen::Quaterniond q = q_transform * q_w_i; // 此处应该添加 * q_transform.inverse()，如下所示 Eigen::Quaterniond q = q_transform * q_w_i *q_transform.inverse(); // camera-odom变为lidar的odom q.normalize(); Eigen::Vector3d t = q_transform * gt_pose.topRightCorner<3, 1>(); // lidar-->camera的旋转矩阵*camera坐标系的平移 // odomGT代表lidar的pose 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); // 发布odomGT // 发布lidar的pathGT 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); // 发布lidar的pathGT // 读取点云数据 // read lidar point cloud std::stringstream lidar_data_path; lidar_data_path << dataset_folder << "velodyne/sequences/" + sequence_number + "/velodyne/" << std::setfill('0') << std::setw(6) << line_num << ".bin"; // 设置前面5个0 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; std::vector<float> lidar_intensities; 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); // 发布点云信息 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); // 发布图像信息 // 保存为bag文件 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

scanRegistration的功能就是提取特征点。

comp函数，比较两个点的曲率 bool comp (int i,int j) { return (cloudCurvature[i]<cloudCurvature[j]); } removeClosedPointCloud函数，丢弃一定距离以内的点 // 丢弃一定距离以内的点 template <typename PointT> void removeClosedPointCloud(const pcl::PointCloud<PointT> &cloud_in, pcl::PointCloud<PointT> &cloud_out, float thres) { if (&cloud_in != &cloud_out) { cloud_out.header = cloud_in.header; cloud_out.points.resize(cloud_in.points.size()); } size_t j = 0; for (size_t i = 0; i < cloud_in.points.size(); ++i) { if (cloud_in.points[i].x * cloud_in.points[i].x + cloud_in.points[i].y * cloud_in.points[i].y + cloud_in.points[i].z * cloud_in.points[i].z < thres * thres) continue; cloud_out.points[j] = cloud_in.points[i]; j++; } if (j != cloud_in.points.size()) { cloud_out.points.resize(j); } cloud_out.height = 1; cloud_out.width = static_cast<uint32_t>(j); cloud_out.is_dense = true; } laserCloudHandler函数

这个函数主要就是对接收到的点云数据进行处理，最终发布关键点（laser_cloud_sharp、laser_cloud_less_sharp、laser_cloud_flat、laser_cloud_less_flat），以方便后续使用。

1、首先计算当前帧的第一个点和最后一个点的角度

// 计算起始点和终止点角度 // 解释一下：atan2的范围是[-180,180]，atan的范围是[-90,90] int cloudSize = laserCloudIn.points.size(); float startOri = -atan2(laserCloudIn.points[0].y, laserCloudIn.points[0].x); // 第一个扫描点的角度 float endOri = -atan2(laserCloudIn.points[cloudSize - 1].y, // 最后一个扫描点的角度 laserCloudIn.points[cloudSize - 1].x) + 2 * M_PI; // 最后一个点的角度 - 第一个点的角度>PI if (endOri - startOri > 3 * M_PI) { endOri -= 2 * M_PI; } else if (endOri - startOri < M_PI) { endOri += 2 * M_PI; } printf("[scanRegistration] start Ori %f\n",startOri); printf("[scanRegistration] end Ori %f\n", endOri);

2、对每一个点计算其scanID和该点对应的扫描到的时间，将结果放在intensity中。

// 对每一个点计算scanID和扫描的点的时间 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; // 根据激光扫描线的俯仰角获取scanid[0,15] float angle = atan(point.z / sqrt(point.x * point.x + point.y * point.y)) * 180 / M_PI; // 俯仰角[-90,90] int scanID = 0; if (N_SCANS == 16) { scanID = int((angle + 15) / 2 + 0.5); if (scanID > (N_SCANS - 1) || scanID < 0) { count--; continue; } } else if (N_SCANS == 32) { scanID = int((angle + 92.0/3.0) * 3.0 / 4.0); if (scanID > (N_SCANS - 1) || scanID < 0) { count--; continue; } } else if (N_SCANS == 64) { if (angle >= -8.83) scanID = int((2 - angle) * 3.0 + 0.5); else scanID = N_SCANS / 2 + int((-8.83 - angle) * 2.0 + 0.5); // use [0 50] > 50 remove outlies if (angle > 2 || angle < -24.33 || scanID > 50 || scanID < 0) { count--; continue; } } else { printf("[scanRegistration] wrong scan number\n"); ROS_BREAK(); } //printf("angle %f scanID %d \n", angle, scanID); // 获取每一个点对应的角度 float ori = -atan2(point.y, point.x); // 每一个点的水平角度 if (!halfPassed) // 判断当前点过没过一半（开始的时候是没过一半的） { if (ori < startOri - M_PI / 2) { ori += 2 * M_PI; } else if (ori > startOri + M_PI * 3 / 2) { ori -= 2 * M_PI; } if (ori - startOri > M_PI) { halfPassed = true; } } else // 后半段时间 { ori += 2 * M_PI; if (ori < endOri - M_PI * 3 / 2) { ori += 2 * M_PI; } else if (ori > endOri + M_PI / 2) { ori -= 2 * M_PI; } } float relTime = (ori - startOri) / (endOri - startOri); // 时间占比 point.intensity = scanID + scanPeriod * relTime; // intensity=scanID+点对应的在线上的时间 laserCloudScans[scanID].push_back(point); // 把当前点放在对应的scanID对应的线上 }

3、计算16条scan的起始点的索引和终止点的索引

// 记录16个scan的每一个的startInd，endInd 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; }

4、对每一个点计算曲率

// 有序地计算曲率 for (int i = 5; i < cloudSize - 5; i++) // 忽略前后的5个点 { 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;// Label 2: corner_sharp // Label 1: corner_less_sharp, ????Label 2 // Label -1: surf_flat // Label 0: surf_less_flat?? ????Label -1?????????????????? }

5、对每一个scan分为6大块，在每一个块中，分别根据曲率提取角点(2个)和平面点(20个)

pcl::PointCloud<PointType> cornerPointsSharp; pcl::PointCloud<PointType> cornerPointsLessSharp; pcl::PointCloud<PointType> surfPointsFlat; pcl::PointCloud<PointType> surfPointsLessFlat; // 提取特征点 float t_q_sort = 0; for (int i = 0; i < N_SCANS; i++)// 0-15 { if( scanEndInd[i] - scanStartInd[i] < 6)// 每一个scan的start和end continue; pcl::PointCloud<PointType>::Ptr surfPointsLessFlatScan(new pcl::PointCloud<PointType>); for (int j = 0; j < 6; j++)// 把每一个scan均分为6份，0-5 { int sp = scanStartInd[i] + (scanEndInd[i] - scanStartInd[i]) * j / 6;// subscan?????index int ep = scanStartInd[i] + (scanEndInd[i] - scanStartInd[i]) * (j + 1) / 6 - 1;// subscan?????index TicToc t_tmp; std::sort (cloudSortInd + sp, cloudSortInd + ep + 1, comp);// 对这6份中的每一份按照曲率小到大排序 t_q_sort += t_tmp.toc(); // 提取角点 int largestPickedNum = 0; for (int k = ep; k >= sp; k--)// 在subscan中 倒序查找 { int ind = cloudSortInd[k]; if (cloudNeighborPicked[ind] == 0 && cloudCurvature[ind] > 0.1)// ????????б??????????????????0.1 { largestPickedNum++; if (largestPickedNum <= 2)// 一个subscan中最多有 2个corner_sharp { cloudLabel[ind] = 2; //corner_sharp cornerPointsSharp.push_back(laserCloud->points[ind]); cornerPointsLessSharp.push_back(laserCloud->points[ind]); } else if (largestPickedNum <= 20)// 一个subscan中最多有20个corner_less_sharp,corner_less_sharp包括corner_sharp { cloudLabel[ind] = 1; // corner_less_sharp cornerPointsLessSharp.push_back(laserCloud->points[ind]); } else // corner_less_sharp>20了 { break; } cloudNeighborPicked[ind] = 1; // 该点已经被处理过了，记为1 // 与临近点的距离的平方 <= 0.05的点标记为选择过，避免特征点密集分布 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) // 如果相邻两根scan的距离大于0.05，那么退出 { break; } cloudNeighborPicked[ind + l] = 1; } // 与临近点的距离的平方 <= 0.05的点标记为选择过，避免特征点密集分布，越往上scanID越大越远，所以可以直接break 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; } } } // 提取平面点 // 提取surf平面feature，与上述类似，选取该subscan曲率最小的前4个点为surf_flat int smallestPickedNum = 0; for (int k = sp; k <= ep; k++) // 寻找平的，曲率由小到大排序 { int ind = cloudSortInd[k]; // 判断是否满足平面点的条件 if (cloudNeighborPicked[ind] == 0 && cloudCurvature[ind] < 0.1) { cloudLabel[ind] = -1; surfPointsFlat.push_back(laserCloud->points[ind]); smallestPickedNum++; if (smallestPickedNum >= 4) { break; } cloudNeighborPicked[ind] = 1; // 根据scanID往上查找，防止关键点密集 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; } } } // 对剩下的点提取为lessFlat for (int k = sp; k <= ep; k++) { if (cloudLabel[k] <= 0) { surfPointsLessFlatScan->push_back(laserCloud->points[k]); } } } // 对lessFlat进行降采样 pcl::PointCloud<PointType> surfPointsLessFlatScanDS; pcl::VoxelGrid<PointType> downSizeFilter; downSizeFilter.setInputCloud(surfPointsLessFlatScan); downSizeFilter.setLeafSize(0.2, 0.2, 0.2); downSizeFilter.filter(surfPointsLessFlatScanDS); surfPointsLessFlat += surfPointsLessFlatScanDS; }

3、laserOdometry.cpp

laserOdometry的作用就是对相邻帧的两帧点云做帧间匹配得到位姿的变换

1、当前帧的在world世界坐标系下的位姿

// Lidar Odometry线程估计的frame在world坐标系的位姿P，Transformation from current frame to world frame Eigen::Quaterniond q_w_curr(1, 0, 0, 0); Eigen::Vector3d t_w_curr(0, 0, 0);

2、待优化变量

// 点云特征匹配时的优化变量 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); // 相当于引用 Eigen::Map<Eigen::Vector3d> t_last_curr(para_t);

3、将当前帧点云转换到上一帧，根据运动模型对点云去畸变

TransformToStart：将当前帧Lidar坐标系下的点云变换到上一帧Lidar坐标系下（也就是当前帧的初始位姿，起始位姿，所以函数名是TransformToStart），因为kitti点云已经去除了畸变，所以不再考虑运动补偿。（如果点云没有去除畸变，用slerp差值的方式计算出每个点在fire时刻的位姿，然后进行TransformToStart的坐标变换，一方面实现了变换到上一帧Lidar坐标系下；另一方面也可以理解成点都将fire时刻统一到了开始时刻，即去除了畸变，完成了运动补偿）

// undistort lidar point // 将当前帧的点云转为上一帧，其中根据运动模型对点云进行了去畸变 void TransformToStart(PointType const *const pi, PointType *const po) { //interpolation ratio double s; if (DISTORTION) s = (pi->intensity - int(pi->intensity)) / SCAN_PERIOD; // 获取每一点在周期中的位置 else s = 1.0; //s = 1; Eigen::Quaterniond q_point_last = Eigen::Quaterniond::Identity().slerp(s, q_last_curr); // 四元数插值 Eigen::Vector3d t_point_last = s * t_last_curr; // 平移量插值 Eigen::Vector3d point(pi->x, pi->y, pi->z); // 当前帧的点 Eigen::Vector3d un_point = q_point_last * point + t_point_last; // 将当前帧的点变换到上一帧坐标系下 po->x = un_point.x(); po->y = un_point.y(); po->z = un_point.z(); po->intensity = pi->intensity; }

4、接收上游的5个topic函数，将消息放在queue中，方便后续处理

// 存放数据的queue std::queue<sensor_msgs::PointCloud2ConstPtr> cornerSharpBuf; // 只能访问 queue<T> 容器适配器的第一个和最后一个元素。只能在容器的末尾添加新元素，只能从头部移除元素。 std::queue<sensor_msgs::PointCloud2ConstPtr> cornerLessSharpBuf; std::queue<sensor_msgs::PointCloud2ConstPtr> surfFlatBuf; std::queue<sensor_msgs::PointCloud2ConstPtr> surfLessFlatBuf; std::queue<sensor_msgs::PointCloud2ConstPtr> fullPointsBuf; //... // 之后的5个Handler函数为接受上游5个topic的回调函数，作用是将消息缓存到对应的queue中，以便后续处理。 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(); }

5、main函数中的发布者

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);

5、消息同步机制

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(); } // 5个点云的时间同步 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();

6、帧间匹配优化机制

// initializing if (!systemInited)// 第一帧不进行匹配，仅仅将 cornerPointsLessSharp 保存至 laserCloudCornerLast // 将 surfPointsLessFlat 保存至 laserCloudSurfLast // 为下次匹配提供target { 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)// 点到线以及点到面的ICP，迭代2次 { corner_correspondence = 0; // 角点的误差项数量 plane_correspondence = 0; // 平面点的误差项数量 //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(para_q, 4, q_parameterization); problem.AddParameterBlock(para_t, 3); pcl::PointXYZI pointSel; std::vector<int> pointSearchInd; std::vector<float> pointSearchSqDis; TicToc t_data; // 基于最近邻原理建立corner特征点之间关联，find correspondence for corner features for (int i = 0; i < cornerPointsSharpNum; ++i) { TransformToStart(&(cornerPointsSharp->points[i]), &pointSel);// 将当前帧的corner_sharp特征点O_cur，从当前帧的Lidar坐标系下变换到上一帧的Lidar坐标系下（记为点O，注意与前面的点O_cur不同），以利于寻找corner特征点的correspondence kdtreeCornerLast->nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis);// kdtree中的点云是上一帧的corner_less_sharp，所以这是在上一帧 // 的corner_less_sharp中寻找当前帧corner_sharp特征点O的最近邻点（记为A） int closestPointInd = -1, minPointInd2 = -1; if (pointSearchSqDis[0] < DISTANCE_SQ_THRESHOLD)// 如果最近邻的corner特征点之间距离平方小于阈值，则最近邻点A有效 { closestPointInd = pointSearchInd[0]; int closestPointScanID = int(laserCloudCornerLast->points[closestPointInd].intensity); // 最近点的scanID，第几条线 double minPointSqDis2 = DISTANCE_SQ_THRESHOLD; // 寻找点O的另外一个最近邻的点（记为点B） in the direction of increasing scan line // 顺着index增加的方向查找 // 遍历范围内的点，选择距离pointSel最近的点（除掉kd-tree直接检索到的最近点之外） for (int j = closestPointInd + 1; j < (int)laserCloudCornerLast->points.size(); ++j)// laserCloudCornerLast 来自上一帧的corner_less_sharp特征点,由于提取特征时是 { // 按照scan的顺序提取的，所以laserCloudCornerLast中的点也是按照scanID递增的顺序存放的 // if in the same scan line, continue // 如果查找到的scanID在closestPointScanID下面，直接跳过 if (int(laserCloudCornerLast->points[j].intensity) <= closestPointScanID)// intensity整数部分存放的是scanID continue; // 第二近的点距离closestPointScanID也不能太远 // 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)// 第二个最近邻点有效,，更新点B { // find nearer point minPointSqDis2 = pointSqDis; minPointInd2 = j; } } // 寻找点O的另外一个最近邻的点B in the direction of decreasing scan line for (int j = closestPointInd - 1; j >= 0; --j) { // if in the same scan line, continue if (int(laserCloudCornerLast->points[j].intensity) >= closestPointScanID) continue; // if not in nearby scans, end the loop if (int(laserCloudCornerLast->points[j].intensity) < (closestPointScanID - NEARBY_SCAN)) break; double pointSqDis = (laserCloudCornerLast->points[j].x - pointSel.x) * (laserCloudCornerLast->points[j].x - pointSel.x) + (laserCloudCornerLast->points[j].y - pointSel.y) * (laserCloudCornerLast->points[j].y - pointSel.y) + (laserCloudCornerLast->points[j].z - pointSel.z) * (laserCloudCornerLast->points[j].z - pointSel.z); if (pointSqDis < minPointSqDis2)// 第二个最近邻点有效，更新点B { // find nearer point minPointSqDis2 = pointSqDis; minPointInd2 = j; } } } // 构造误差项 if (minPointInd2 >= 0) // both closestPointInd and minPointInd2 is valid { // 即特征点O的两个最近邻点A和B都有效 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++; } } // 下面说的点符号与上述相同 // 与上面的建立corner特征点之间的关联类似，寻找平面特征点O的最近邻点ABC，即基于最近邻原理建立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; } } // search in the direction of decreasing scan line for (int j = closestPointInd - 1; j >= 0; --j) { // if not in nearby scans, end the loop if (int(laserCloudSurfLast->points[j].intensity) < (closestPointScanID - NEARBY_SCAN)) break; double pointSqDis = (laserCloudSurfLast->points[j].x - pointSel.x) * (laserCloudSurfLast->points[j].x - pointSel.x) + (laserCloudSurfLast->points[j].y - pointSel.y) * (laserCloudSurfLast->points[j].y - pointSel.y) + (laserCloudSurfLast->points[j].z - pointSel.z) * (laserCloudSurfLast->points[j].z - pointSel.z); // if in the same or higher scan line if (int(laserCloudSurfLast->points[j].intensity) >= closestPointScanID && pointSqDis < minPointSqDis2) { minPointSqDis2 = pointSqDis; minPointInd2 = j; } else if (int(laserCloudSurfLast->points[j].intensity) < closestPointScanID && pointSqDis < minPointSqDis3) { // find nearer point minPointSqDis3 = pointSqDis; minPointInd3 = j; } } } // 如果三个近邻点都有效，构造误差项 if (minPointInd2 >= 0 && minPointInd3 >= 0)// 如果三个最近邻点都有效 { Eigen::Vector3d curr_point(surfPointsFlat->points[i].x, surfPointsFlat->points[i].y, surfPointsFlat->points[i].z); Eigen::Vector3d last_point_a(laserCloudSurfLast->points[closestPointInd].x, laserCloudSurfLast->points[closestPointInd].y, laserCloudSurfLast->points[closestPointInd].z); Eigen::Vector3d last_point_b(laserCloudSurfLast->points[minPointInd2].x, laserCloudSurfLast->points[minPointInd2].y, laserCloudSurfLast->points[minPointInd2].z); Eigen::Vector3d last_point_c(laserCloudSurfLast->points[minPointInd3].x, laserCloudSurfLast->points[minPointInd3].y, laserCloudSurfLast->points[minPointInd3].z); double s; if (DISTORTION) s = (surfPointsFlat->points[i].intensity - int(surfPointsFlat->points[i].intensity)) / SCAN_PERIOD; else s = 1.0; // 用点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("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; options.minimizer_progress_to_stdout = false; // 输出到cout 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; }

7、发布odom和path

// 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);

8、更新配准的source

// 当前帧变上一帧 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-tree，如果是第一帧的话是直接将其保存为这个的 kdtreeCornerLast->setInputCloud(laserCloudCornerLast);// 更新kdtree的点云 kdtreeSurfLast->setInputCloud(laserCloudSurfLast);

9、每5帧执行一次发布

// 每隔5帧执行一次 if (frameCount % skipFrameNum == 0) { frameCount = 0; // 发布上一帧的Corner点 sensor_msgs::PointCloud2 laserCloudCornerLast2; pcl::toROSMsg(*laserCloudCornerLast, laserCloudCornerLast2); laserCloudCornerLast2.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat); laserCloudCornerLast2.header.frame_id = "/camera"; pubLaserCloudCornerLast.publish(laserCloudCornerLast2); // 发布上一帧的Surf点 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); }

4、laserMapping.cpp

0、submap的维护

参考：LOAM笔记及A-LOAM源码阅读

1、优化的变量，是当前帧在世界坐标系下的pose

// 点云特征匹配时的优化变量 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，本身就是匹配时的优化变量。 // mapping线程计算的scan-to-localmap匹配后得到的位姿 Eigen::Map<Eigen::Quaterniond> q_w_curr(parameters); // map计算后的在world下的pose Eigen::Map<Eigen::Vector3d> t_w_curr(parameters + 4);

2、mapping线程到Odometry线程的pose变换，Odometry线程计算得到的当前帧在world坐标系下的pose

// 下面的两个变量是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); // map到odom的变换 Eigen::Vector3d t_wmap_wodom(0, 0, 0); // Odometry线程计算的frame在world坐标系的位姿 Eigen::Quaterniond q_wodom_curr(1, 0, 0, 0); // odom计算的在world坐标系下的pose Eigen::Vector3d t_wodom_curr(0, 0, 0);

3、得到当前帧的在world世界坐标系下的pose

// set initial guess，上一帧的Transform(增量)wmap_wodom * 本帧Odometry位姿wodom_curr，旨在为本帧Mapping位姿w_curr设置一个初始值 void transformAssociateToMap() // 获取Mapping的初值 { q_w_curr = q_wmap_wodom * q_wodom_curr; t_w_curr = q_wmap_wodom * t_wodom_curr + t_wmap_wodom; }

4、利用mapping计算得到的pose，计算mapping线程和Odometry线程之间的pose变换

// 用在最后，当Mapping的位姿w_curr计算完毕后，更新增量wmap_wodom，旨在为下一次执行transformAssociateToMap函数时做准备 void transformUpdate() // 获取map到odom的变换 { q_wmap_wodom = q_w_curr * q_wodom_curr.inverse(); t_wmap_wodom = t_w_curr - q_wmap_wodom * t_wodom_curr; }

5、用mapping的位姿将Lidar坐标系下的点转换到world坐标系下

// 用Mapping的位姿w_curr，将Lidar坐标系下的点变换到world坐标系下.q_w_curr为优化量，代表lidar在世界坐标系中的pose 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; }

6、Odometry的回调函数

// receive odomtry void laserOdometryHandler(const nav_msgs::Odometry::ConstPtr &laserOdometry) { mBuf.lock(); odometryBuf.push(laserOdometry); mBuf.unlock(); // high frequence publish Eigen::Quaterniond q_wodom_curr; Eigen::Vector3d t_wodom_curr; q_wodom_curr.x() = laserOdometry->pose.pose.orientation.x; q_wodom_curr.y() = laserOdometry->pose.pose.orientation.y; q_wodom_curr.z() = laserOdometry->pose.pose.orientation.z; q_wodom_curr.w() = laserOdometry->pose.pose.orientation.w; t_wodom_curr.x() = laserOdometry->pose.pose.position.x; t_wodom_curr.y() = laserOdometry->pose.pose.position.y; t_wodom_curr.z() = laserOdometry->pose.pose.position.z; // 为了保证LOAM整体的实时性，防止Mapping线程耗时>100ms导致丢帧，用上一次的增量wmap_wodom来更新 // Odometry的位姿，旨在用Mapping位姿的初始值（也可以理解为预测值）来实时输出，进而实现LOAM整体的实时性 // 这里为啥不直接用transformAssociateToMap()函数来获取mapping的初始值？ 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); }

7、process函数

先读取Odometry线程的pose // odom线程得到的pose 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(); 更新当前帧在mapping线程中的pose // 上一帧的增量wmap_wodom * 本帧Odometry位姿wodom_curr，旨在为本帧Mapping位姿w_curr设置一个初始值 transformAssociateToMap(); // 第一次运行时，wmap_wodom=E 地图维护 // 地图维护 TicToc t_shift; // 下面这是计算当前帧位置t_w_curr（在上图中用红色五角星表示的位置）IJK坐标（见上图中的坐标轴）， // 参照LOAM_NOTED的注释，下面有关25呀，50啥的运算，等效于以50m为单位进行缩放，因为LOAM用1维数组 // 进行cube的管理，而数组的index只用是正数，所以要保证IJK坐标都是正数，所以加了laserCloudCenWidth/Heigh/Depth // 的偏移，来使得当前位置尽量位于submap的中心处，也就是使得上图中的五角星位置尽量处于所有格子的中心附近， // 偏移laserCloudCenWidth/Heigh/Depth会动态调整，来保证当前位置尽量位于submap的中心处。 // 当前帧在world中的IJK坐标 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; // 由于计算机求余是向零取整，为了不使（-50.0,50.0）求余后都向零偏移，当被求余数为负数时求余结果统一向左偏移一个单位，也即减一 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--; // 输出偏移量 printf("new----laserCloudCenWidth:%d,laserCloudCenHeight:%d,laserCloudCenDepth:%d",laserCloudCenWidth,laserCloudCenHeight,laserCloudCenDepth); printf("I:%f, J:%f, K:%f",centerCubeI,centerCubeJ,centerCubeK); // 以下注释部分参照LOAM_NOTED，结合我画的submap的示意图说明下面的6个while loop的作用：要 // 注意世界坐标系下的点云地图是固定的，但是IJK坐标系我们是可以移动的，所以这6个while loop // 的作用就是调整IJK坐标系（也就是调整所有cube位置），使得五角星在IJK坐标系的坐标范围处于 // 3 <= centerCubeI < 18， 3 < centerCubeJ < 8, 3 < centerCubeK < 18，目的是为了防止后续向 // 四周拓展cube（图中的黄色cube就是拓展的cube）时，index（即IJK坐标）成为负数。 while (centerCubeI < 3) // 以i轴为例，如果当前帧的（五角星）坐标小于3，那么要将其向I轴的正方向移动， { for (int j = 0; j < laserCloudHeight; j++) { for (int k = 0; k < laserCloudDepth; k++) { int i = laserCloudWidth - 1; // 先把最大的i的对应的点云取出来 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]; // i处的点云被i-1处的点云替代 laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = laserCloudSurfArray[i - 1 + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k]; } laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = laserCloudCubeCornerPointer; // 把之前最大的i对应的点云赋值给最小的i laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = laserCloudCubeSurfPointer; laserCloudCubeCornerPointer->clear(); laserCloudCubeSurfPointer->clear(); } } centerCubeI++; // 这里为啥要对laserCloudCenWidth加一行？ // 答：不是加一行，因为laserCloudCenWidth代表的当前帧坐标在IJK坐标系下的偏移量，而不是submap的尺寸，这个偏移量是可变的，目的就是为了使得submap位于中心，submap的尺寸是固定的5*5*5 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]; // i行的点云被i+1行的点云替代 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--; } // 输出最终的偏移量 printf("new----laserCloudCenWidth:%d,laserCloudCenHeight:%d,laserCloudCenDepth:%d",laserCloudCenWidth,laserCloudCenHeight,laserCloudCenDepth); // 输出最终的当前帧坐标 printf("new-I:%f, new-J:%f, new-K:%f",centerCubeI,centerCubeJ,centerCubeK); // -------------------至此，IJK坐标系维护完成，当前帧位于IJK坐标系中心------------------- 以当前帧为中心，扩展(5,5,3)的黄色小地图 int laserCloudValidNum = 0; int laserCloudSurroundNum = 0; // 向IJ坐标轴的正负方向各拓展2个cube，K坐标轴的正负方向各拓展1个cube，上图中五角星所在的蓝色cube就是当前位置 // 所处的cube，拓展的cube就是黄色的cube，这些cube就是submap的范围 submap的大小就是5,5,3 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)// 如果坐标合法 [0,21), [0,21),[0,10] { // 记录submap中的所有cube的index，记为有效index laserCloudValidInd[laserCloudValidNum] = i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k; laserCloudValidNum++; laserCloudSurroundInd[laserCloudSurroundNum] = i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k; laserCloudSurroundNum++; } } } } // submap大小 printf("submap size: %d\n",laserCloudValidNum); // -------------------至此，得到当前帧的局部地图索引------------------- 将submap的点云加在一起 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("submap prepare time %f ms\n", t_shift.toc()); printf("submap corner num %d surf num %d \n", laserCloudCornerFromMapNum, laserCloudSurfFromMapNum); // 输出(5,5,3)个cube中的点云的总数量 如果小地图的点足够多，开始优化匹配 if (laserCloudCornerFromMapNum > 10 && laserCloudSurfFromMapNum > 50) // 如果submap的特征点云数足够多，对其构建kd-tree { TicToc t_opt; TicToc t_tree; kdtreeCornerFromMap->setInputCloud(laserCloudCornerFromMap); // (5,5,3)的submap构建kd-tree kdtreeSurfFromMap->setInputCloud(laserCloudSurfFromMap); printf("build tree time %f ms \n", t_tree.toc()); for (int iterCount = 0; iterCount < 2; iterCount++) // 迭代两次 { //ceres::LossFunction *loss_function = NULL; ceres::LossFunction *loss_function = new ceres::HuberLoss(0.1); // LossFunction ceres::LocalParameterization *q_parameterization = new ceres::EigenQuaternionParameterization(); ceres::Problem::Options problem_options; ceres::Problem problem(problem_options); // Problem 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++) // 遍历上一帧的所有Corner点 { 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); 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]; pointAssociateToMap(&pointOri, &pointSel); 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); // Ax + By + Cz + 1 = 0，全部除以法向量的模长，方程依旧成立，而且使得法向量归一化了 double negative_OA_dot_norm = 1 / norm.norm(); norm.normalize(); // Here n(pa, pb, pc) is unit norm of plane bool planeValid = true; for (int j = 0; j < 5; j++) { // 点(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 twice time %f \n", t_opt.toc()); // -------------------至此，两次迭代完成了，得到w_curr，当前帧在map坐标系下的pose---------------------------- } else // submap的点数量不够 { ROS_WARN("time Map corner and surf num are not enough"); } 更新mapping到Odometry线程的T // 完成ICP（迭代2次）的特征匹配后，用最后匹配计算出的优化变量w_curr，更新增量wmap_wodom，为下一次 // Mapping做准备 transformUpdate(); // 获取map到odom的变换Transform 更新submap的点云 TicToc t_add; // 创建两个indices，用来存放当前帧的点所处cube的index std::vector<int> CornerInd(); std::vector<int> SurfInd(); // 下面两个for loop的作用就是将当前帧的特征点云，逐点进行操作：转换到world坐标系并添加到对应位置的cube中 for (int i = 0; i < laserCloudCornerStackNum; i++) { // Lidar坐标系转到world坐标系 pointAssociateToMap(&laserCloudCornerStack->points[i], &pointSel); // 计算本次的特征点的IJK坐标，进而确定添加到哪个cube中 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; laserCloudCornerArray[cubeInd]->push_back(pointSel); // PointCloud.push_back CornerInd.append(cubeInd); } } 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); SurfInd.append(cubeInd); } } printf("add current points to submap time %f ms\n", t_add.toc()); TicToc t_filter; // 对submap的cube点云进行下采样 // 因为新增加了点云，对之前已经存有点云的submap cube全部重新进行一次降采样 // 这个地方可以简单优化一下：如果之前的cube没有新添加点就不需要再降采样 for (int i = 0; i < laserCloudValidNum; i++) // 遍历submap的所有cube { int ind = laserCloudValidInd[i]; // submap中每一个cube的索引 // 判断当前的submap的cube id 是否在当前帧的索引的vector中 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()); 发布 // 每5帧发布一次submap (5,5,3)的cube大小 TicToc t_pub; //publish surround map for every 5 frames if (frameCount % 5 == 0) { laserCloudSurround->clear(); for (int i = 0; i < laserCloudSurroundNum; i++) // 遍历submap { 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"; // world坐标系 pubLaserCloudSurround.publish(laserCloudSurround3); } // 每20帧发布IJK局部地图 // pub laserCloudMap for every 20 frames if (frameCount % 20 == 0) { pcl::PointCloud<PointType> laserCloudMap; for (int i = 0; i < 4851; i++) // 遍历IJK坐标系 { 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); } // 全部点云转化到world坐标系，并发布 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()); // Odometry 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(); // 当前帧在world下的pose,ceres的优化变量 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); // Path 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变换,(world)/camera_init--->(当前帧)/aft_mapped 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++;

5、lidarFactor.cpp

1、构建误差项

// 点到线的残差距离计算 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 { 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]}; // last到curr的旋转四元数 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())}; // cp Eigen::Matrix<T, 3, 1> lpj{T(last_point_j.x()), T(last_point_j.y()), T(last_point_j.z())}; // last point j //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>( // ^ ^ ^ // | | | // 残差的维度 ____| | | // 优化变量q的维度 _______| | // 优化变量t的维度 __________| 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; }; 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())); 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; };

参考

LOAM笔记及A-LOAM源码阅读