|
|
@@ -0,0 +1,332 @@
|
|
|
+#include "laser_odom.hpp"
|
|
|
+#include "hdl_graph_slam/registrations.hpp"
|
|
|
+
|
|
|
+#include <memory>
|
|
|
+#include <iostream>
|
|
|
+
|
|
|
+#include <ros/ros.h>
|
|
|
+#include <ros/time.h>
|
|
|
+#include <ros/duration.h>
|
|
|
+#include <pcl_ros/point_cloud.h>
|
|
|
+#include <tf_conversions/tf_eigen.h>
|
|
|
+#include <tf/transform_listener.h>
|
|
|
+#include <tf/transform_broadcaster.h>
|
|
|
+
|
|
|
+#include <std_msgs/Time.h>
|
|
|
+#include <nav_msgs/Odometry.h>
|
|
|
+#include <sensor_msgs/PointCloud2.h>
|
|
|
+#include <geometry_msgs/TransformStamped.h>
|
|
|
+#include <geometry_msgs/PoseWithCovarianceStamped.h>
|
|
|
+
|
|
|
+#include <nodelet/nodelet.h>
|
|
|
+#include <pluginlib/class_list_macros.h>
|
|
|
+
|
|
|
+#include <pcl/filters/voxel_grid.h>
|
|
|
+#include <pcl/filters/passthrough.h>
|
|
|
+#include <pcl/filters/approximate_voxel_grid.h>
|
|
|
+
|
|
|
+#include <hdl_graph_slam/ros_utils.hpp>
|
|
|
+#include <hdl_graph_slam/registrations.hpp>
|
|
|
+#include <hdl_graph_slam/ScanMatchingStatus.h>
|
|
|
+
|
|
|
+#include <laser_geometry/laser_geometry.h>
|
|
|
+#include <nav_msgs/Path.h>
|
|
|
+
|
|
|
+#include <tf2/LinearMath/Quaternion.h>
|
|
|
+
|
|
|
+int main(int argc, char* argv[]) {
|
|
|
+ // 执行 ros 节点初始化
|
|
|
+ ros::init(argc, argv, "laser_odom");
|
|
|
+
|
|
|
+ Laser_Odom laser_odom;
|
|
|
+ laser_odom.init();
|
|
|
+ // odom_pub = nh.advertise<nav_msgs::Odometry>("/laser_odom", 32); // 发布机器人的里程计信息
|
|
|
+
|
|
|
+ return 0;
|
|
|
+}
|
|
|
+
|
|
|
+Laser_Odom::Laser_Odom() {
|
|
|
+ laser_odom = this;
|
|
|
+ cloud.reset(new pcl::PointCloud<PointT>());
|
|
|
+ trans_cloud.reset(new pcl::PointCloud<PointT>());
|
|
|
+ registration.reset(new fast_gicp::FastGICP<PointT, PointT>());
|
|
|
+}
|
|
|
+
|
|
|
+Laser_Odom::~Laser_Odom() {}
|
|
|
+
|
|
|
+void Laser_Odom::init() {
|
|
|
+ auto voxelgrid = new pcl::VoxelGrid<PointT>();
|
|
|
+ voxelgrid->setLeafSize(downsample_resolution, downsample_resolution, downsample_resolution);
|
|
|
+ downsample_filter.reset(voxelgrid);
|
|
|
+
|
|
|
+ fast_gicp::FastGICP<PointT, PointT>::Ptr gicp(new fast_gicp::FastGICP<PointT, PointT>());
|
|
|
+ gicp->setNumThreads(0); // 把 registration = hdl_graph_slam::select_registration_method(nh); 核心部分弄出来
|
|
|
+ gicp->setTransformationEpsilon(1e-5);
|
|
|
+ gicp->setMaximumIterations(128);
|
|
|
+ gicp->setMaxCorrespondenceDistance(2.0);
|
|
|
+ gicp->setCorrespondenceRandomness(20);
|
|
|
+
|
|
|
+ registration = gicp;
|
|
|
+
|
|
|
+
|
|
|
+ odom_pub = nh.advertise<nav_msgs::Odometry>("/laser_odom", 32); // 发布机器人的里程计信息
|
|
|
+ // keyframe_pub = nh.advertise<pcl::PointCloud<PointT>>("/keyframe", 32);
|
|
|
+ // read_until_pub = nh.advertise<std_msgs::Header>("/keyframe", 32);
|
|
|
+ source_points_pub = nh.advertise<sensor_msgs::PointCloud2>("/aligned_points", 32); // 对齐后的点云
|
|
|
+ map_points_pub = nh.advertise<sensor_msgs::PointCloud2>("/frame_points", 1, true);
|
|
|
+
|
|
|
+ ros::Subscriber sub = nh.subscribe<sensor_msgs::LaserScan>("/scan", 10, Laser_Odom::cloud_callback);
|
|
|
+ ros::spin();
|
|
|
+}
|
|
|
+
|
|
|
+Laser_Odom* Laser_Odom::laser_odom = nullptr; // 在 .cpp 文件中定义静态成员变量
|
|
|
+// 这种实现方法不算单例实现，如果有多个laser_odom的实例，this指针都会被覆盖，指向最后创建的那个实例
|
|
|
+// 这里就不改了吧,后端不能这么写,因为后端要多线程
|
|
|
+// 多线程实现中有一个跟这个很类似的,是 懒汉式（线程不安全） 实现, 它是在给 laser_odom 赋值前先判断是不是等于 nullptr , 如果是才赋值,否则直接返回laser_odom
|
|
|
+// 但多线程很有可能同时满足 laser_odom == nullptr, 然后创建多个实例
|
|
|
+// 但其实我的cpp文件只跑一次main而已, 多线程问题在这里不会出现呀,
|
|
|
+void Laser_Odom::cloud_callback(const sensor_msgs::LaserScan::ConstPtr& laser_scan_msg) {
|
|
|
+ if(!ros::ok()) {
|
|
|
+ return;
|
|
|
+ }
|
|
|
+ // 消息类型转换
|
|
|
+ laser_odom->projector.projectLaser(*laser_scan_msg, laser_odom->cloud2);
|
|
|
+ pcl::fromROSMsg(laser_odom->cloud2, *laser_odom->cloud);
|
|
|
+
|
|
|
+ // 点云匹配
|
|
|
+ Eigen::Matrix4f pose = laser_odom->matching(laser_scan_msg->header.stamp, laser_odom->cloud); // 点云匹配函数，返回
|
|
|
+
|
|
|
+ // 发布激光里程计数据
|
|
|
+ laser_odom->publish_odometry(laser_scan_msg->header.stamp, laser_scan_msg->header.frame_id, pose);
|
|
|
+
|
|
|
+ // 发布对齐后的点云
|
|
|
+ // pcl::PointCloud<PointT>::Ptr aligned(new pcl::PointCloud<PointT>());
|
|
|
+ // pcl::transformPointCloud(*laser_odom->cloud, *aligned, laser_odom->odom); //将输入的点云（laser_odom->cloud）根据指定的变换矩阵（laser_odom->odom）进行变换，并将变换后的点云结果存储到输出点云（*aligned）中
|
|
|
+ // aligned->header.frame_id = "odom";
|
|
|
+ // laser_odom->aligned_points_pub.publish(*aligned);
|
|
|
+
|
|
|
+}
|
|
|
+
|
|
|
+/**
|
|
|
+ * @brief estimate the relative pose between an input cloud and a keyframe cloud
|
|
|
+ * @param stamp the timestamp of the input cloud
|
|
|
+ * @param cloud the input cloud
|
|
|
+ * @return the relative pose between the input cloud and the keyframe cloud
|
|
|
+ */
|
|
|
+
|
|
|
+bool first = false;
|
|
|
+Eigen::Matrix4f last_odom;
|
|
|
+Eigen::Matrix4f Laser_Odom::matching(const ros::Time& stamp, const pcl::PointCloud<PointT>::ConstPtr& cloud) { // 执行扫描匹配算法，估计输入点云和关键帧之间的相对位置
|
|
|
+
|
|
|
+ if(!first) { // 判断 keyframe 是否为空指针，是的话说明还没有初始化关键真
|
|
|
+ prev_time = ros::Time(); //
|
|
|
+ prev_trans.setIdentity(); // 设置为单位矩阵，表示：与上一次的位资没有发生变化
|
|
|
+ keyframe_pose.setIdentity(); // 关键帧的初始位资
|
|
|
+ keyframe_stamp = stamp; // 关键帧的时间戳
|
|
|
+ keyframe = downsample(cloud); // 对点云数据进行下采样，减少点的数量，提高处理效率和精度
|
|
|
+ registration->setInputTarget(keyframe); // 将 keyframe 设置成关键帧
|
|
|
+ first = true;
|
|
|
+ last_odom.setIdentity();
|
|
|
+
|
|
|
+ sensor_msgs::PointCloud2Ptr cloud_msg(new sensor_msgs::PointCloud2());
|
|
|
+ pcl::toROSMsg(*keyframe, *cloud_msg);
|
|
|
+ cloud_msg->header.frame_id = "odom";
|
|
|
+
|
|
|
+ map_points_pub.publish(*cloud_msg); // 话题名:frame_points
|
|
|
+
|
|
|
+ return Eigen::Matrix4f::Identity(); // 没有之前的位资信息，假设与上次位资没有发生变化，返回单位矩阵
|
|
|
+
|
|
|
+ // registration 对象负责计算输入点云（cloud）与已有关键帧（keyframe）之间的相对变换。这种变换估算允许系统理解传感器（如激光雷达）在两次扫描之间的移动
|
|
|
+ }
|
|
|
+ auto filtered = downsample(cloud); // 下采样
|
|
|
+ registration->setInputSource(filtered); // 把点云数据给到 registration
|
|
|
+
|
|
|
+ std::string msf_source; // 记录初始位资估计的来源（imu 或者 odometry)
|
|
|
+ Eigen::Isometry3f msf_delta = Eigen::Isometry3f::Identity(); // 三维仿射变换的单位矩阵，用于存储上一帧到当前帧的位资变化，3表示三维
|
|
|
+ // 缩放变换和旋转变换称为线性变换（linear transform） 线性变换和平移变换统称为仿射变换（affine transfrom）
|
|
|
+
|
|
|
+ pcl::PointCloud<PointT>::Ptr aligned(new pcl::PointCloud<PointT>()); // 创建一个新的点云对象aligned，用于存储对齐后的点云数据
|
|
|
+ // registration->align(*aligned, prev_trans * msf_delta.matrix()); // 用 registration 来对齐点云, 如果没有imu或者odom信息, msf_delta 是单位矩阵, 即将观测到的点云数据对齐到关键帧?
|
|
|
+ registration->align(*aligned, Eigen::Isometry3f::Identity().matrix()); // 用 registration 来对齐点云, 如果没有imu或者odom信息, msf_delta 是单位矩阵, 即将观测到的点云数据对齐到关键帧?
|
|
|
+
|
|
|
+ LOG(INFO) << "===== getFitnessScore = " << registration->getFitnessScore() ;
|
|
|
+ // publish_scan_matching_status(stamp, cloud->header.frame_id, aligned, msf_source, msf_delta); // 发布扫描匹配的状态信息，包括时间戳、坐标帧ID、对齐后的点云、位姿来源和位姿更新
|
|
|
+
|
|
|
+ if(!registration->hasConverged()) { // 检查扫描匹配是否收敛(是否匹配成功?)。如果没有收敛，输出信息并返回上一次的位姿
|
|
|
+ ROS_ERROR("scan matching has not converged!!");
|
|
|
+ return keyframe_pose * prev_trans;
|
|
|
+ }
|
|
|
+
|
|
|
+ Eigen::Matrix4f trans = registration->getFinalTransformation(); // 获得当前点云和上一帧点云 关键帧 的仿射变换
|
|
|
+ odom = keyframe_pose * trans; // 算出来 odom
|
|
|
+
|
|
|
+ std::cout << std::fixed << std::setprecision(5);
|
|
|
+ std::cout << "keyframe_pose\n" << keyframe_pose << std::endl;
|
|
|
+ LOG(INFO) << "keyframe_pose = " << atan2(keyframe_pose(1, 0), keyframe_pose(0, 0)) * (180.0 / M_PI);
|
|
|
+
|
|
|
+ std::cout << "trans\n" << trans << std::endl;
|
|
|
+ LOG(INFO) << "trans = " << atan2(trans(1, 0), trans(0, 0)) * (180.0 / M_PI);
|
|
|
+
|
|
|
+ std::cout << "odom\n" << odom << std::endl;
|
|
|
+ LOG(INFO) << "odom = " << atan2(odom(1, 0), odom(0, 0)) * (180.0 / M_PI);
|
|
|
+
|
|
|
+
|
|
|
+ prev_time = stamp; // 当前帧的时间戳
|
|
|
+ // prev_trans = trans; // 当前帧的仿射变换
|
|
|
+
|
|
|
+ auto keyframe_trans = matrix2transform(stamp, keyframe_pose, "odom", "keyframe"); // 将变换矩阵转换为tf::Transform对象，用于发布关键帧的变换
|
|
|
+ keyframe_broadcaster.sendTransform(keyframe_trans); // 发布关键帧的变换(这个是发送到哪里? )
|
|
|
+
|
|
|
+ double delta_trans = trans.block<3, 1>(0, 3).norm(); // 计算 当前帧 与 关键帧 变换的 平移距离
|
|
|
+ double delta_angle = std::acos(Eigen::Quaternionf(trans.block<3, 3>(0, 0)).w()); // 计算 当前帧 与 关键帧 变换的 旋转角度
|
|
|
+ double delta_time = (stamp - keyframe_stamp).toSec(); // 计算 当前帧 与 关键帧 变换的 时间差
|
|
|
+ // if(delta_trans > 5 || delta_angle > 3.14) { // 如果有一个超过阈值,更新关键帧
|
|
|
+ // keyframe = filtered;
|
|
|
+ // registration->setInputTarget(keyframe);
|
|
|
+
|
|
|
+ // keyframe_pose = odom;
|
|
|
+ // keyframe_stamp = stamp;
|
|
|
+ // prev_time = stamp;
|
|
|
+ // prev_trans.setIdentity();
|
|
|
+ // }
|
|
|
+ pcl::PointCloud<PointT>::Ptr filter_trans(new pcl::PointCloud<PointT>());
|
|
|
+ pcl::transformPointCloud(*filtered, *filter_trans, odom); // transformPointCloud(A,B,c): A * c = B
|
|
|
+
|
|
|
+ pcl::PointCloud<PointT>::Ptr aligned_trans(new pcl::PointCloud<PointT>());
|
|
|
+ pcl::transformPointCloud(*registration->getInputSource(), *aligned_trans, last_odom);
|
|
|
+ *trans_cloud+=*aligned_trans;
|
|
|
+
|
|
|
+ // 发布
|
|
|
+ sensor_msgs::PointCloud2Ptr cloud2_msg(new sensor_msgs::PointCloud2());
|
|
|
+ pcl::toROSMsg(*trans_cloud, *cloud2_msg);
|
|
|
+ cloud2_msg->header.frame_id = "map";
|
|
|
+ source_points_pub.publish(*cloud2_msg); // 话题名:aligned_points
|
|
|
+
|
|
|
+ // keyframe_pose = odom;
|
|
|
+ // 发布对齐后的点云
|
|
|
+ sensor_msgs::PointCloud2Ptr cloud_msg(new sensor_msgs::PointCloud2());
|
|
|
+ pcl::toROSMsg(*registration->getInputTarget(), *cloud_msg);
|
|
|
+ // pcl::toROSMsg(*aligned, *cloud_msg);
|
|
|
+ cloud_msg->header.frame_id = "map";
|
|
|
+
|
|
|
+ map_points_pub.publish(*cloud_msg); // 话题名:frame_points
|
|
|
+
|
|
|
+ // Eigen::Matrix4f odom;
|
|
|
+ registration->setInputTarget(filtered);
|
|
|
+ last_odom = last_odom * trans;
|
|
|
+ std::cout << "The matrix last_odom is: \n" << last_odom << std::endl;
|
|
|
+ return last_odom; // 返回里程计
|
|
|
+}
|
|
|
+
|
|
|
+/**
|
|
|
+ * @brief downsample a point cloud
|
|
|
+ * @param cloud input cloud
|
|
|
+ * @return downsampled point cloud
|
|
|
+ */
|
|
|
+pcl::PointCloud<PointT>::ConstPtr Laser_Odom::downsample(const pcl::PointCloud<PointT>::ConstPtr& cloud) { // 对点云数据进行向下采样，减少点的数量以提高处理速度
|
|
|
+ if(!downsample_filter) {
|
|
|
+ return cloud;
|
|
|
+ }
|
|
|
+ pcl::PointCloud<PointT>::Ptr filtered(new pcl::PointCloud<PointT>()); // 创建一个新的点云对象，用来存储下采样后的点云数据
|
|
|
+ downsample_filter->setInputCloud(cloud);
|
|
|
+ downsample_filter->filter(*filtered);
|
|
|
+ return filtered;
|
|
|
+}
|
|
|
+
|
|
|
+/**
|
|
|
+ * @brief convert Eigen::Matrix to geometry_msgs::TransformStamped
|
|
|
+ * @param stamp timestamp
|
|
|
+ * @param pose Eigen::Matrix to be converted
|
|
|
+ * @param frame_id tf frame_id
|
|
|
+ * @param child_frame_id tf child frame_id
|
|
|
+ * @return converted TransformStamped
|
|
|
+ */
|
|
|
+geometry_msgs::TransformStamped Laser_Odom::matrix2transform(const ros::Time& stamp, const Eigen::Matrix4f& pose, const std::string& frame_id, const std::string& child_frame_id) {
|
|
|
+ Eigen::Quaternionf quat(pose.block<3, 3>(0, 0));
|
|
|
+ quat.normalize();
|
|
|
+ geometry_msgs::Quaternion odom_quat;
|
|
|
+ odom_quat.w = quat.w();
|
|
|
+ odom_quat.x = quat.x();
|
|
|
+ odom_quat.y = quat.y();
|
|
|
+ odom_quat.z = quat.z();
|
|
|
+
|
|
|
+ geometry_msgs::TransformStamped odom_trans;
|
|
|
+ odom_trans.header.stamp = stamp;
|
|
|
+ odom_trans.header.frame_id = frame_id;
|
|
|
+ odom_trans.child_frame_id = child_frame_id;
|
|
|
+
|
|
|
+ odom_trans.transform.translation.x = pose(0, 3);
|
|
|
+ odom_trans.transform.translation.y = pose(1, 3);
|
|
|
+ odom_trans.transform.translation.z = pose(2, 3);
|
|
|
+ odom_trans.transform.rotation = odom_quat;
|
|
|
+
|
|
|
+ return odom_trans;
|
|
|
+}
|
|
|
+
|
|
|
+/**
|
|
|
+ * @brief publish odometry
|
|
|
+ * @param stamp timestamp
|
|
|
+ * @param pose odometry pose to be published
|
|
|
+ */
|
|
|
+void Laser_Odom::publish_odometry(const ros::Time& stamp, const std::string& base_frame_id, const Eigen::Matrix4f& pose) { // 发布里程计数据
|
|
|
+ // publish transform stamped for IMU integration
|
|
|
+ // geometry_msgs::TransformStamped odom_trans = matrix2transform(stamp, pose, odom_frame_id, base_frame_id);
|
|
|
+ geometry_msgs::TransformStamped odom_trans = matrix2transform(stamp, pose, "odom", base_frame_id);
|
|
|
+ // trans_pub.publish(odom_trans);
|
|
|
+
|
|
|
+ // // broadcast the transform over tf
|
|
|
+ // odom_broadcaster.sendTransform(odom_trans);
|
|
|
+
|
|
|
+ // publish the transform
|
|
|
+ nav_msgs::Odometry odom;
|
|
|
+ odom.header.stamp = stamp;
|
|
|
+ odom.header.frame_id = "odom";
|
|
|
+ odom.child_frame_id = "base_link";
|
|
|
+
|
|
|
+ odom.pose.pose.position.x = pose(0, 3);
|
|
|
+ odom.pose.pose.position.y = pose(1, 3);
|
|
|
+ odom.pose.pose.position.z = pose(2, 3);
|
|
|
+ odom.pose.pose.orientation = odom_trans.transform.rotation;
|
|
|
+
|
|
|
+ odom.child_frame_id = base_frame_id;
|
|
|
+ odom.twist.twist.linear.x = 0.0;
|
|
|
+ odom.twist.twist.linear.y = 0.0;
|
|
|
+ odom.twist.twist.angular.z = 0.0;
|
|
|
+
|
|
|
+
|
|
|
+ // 上面算出来的odom朝向有问题,需要顺时针转90度修正,代码如下:
|
|
|
+
|
|
|
+ odom_pub.publish(odom);
|
|
|
+}
|
|
|
+
|
|
|
+// /**
|
|
|
+// * @brief publish odometry
|
|
|
+// * @param stamp timestamp
|
|
|
+// * @param pose odometry pose to be published
|
|
|
+// */
|
|
|
+// void Laser_Odom::publish_keyfrrame(const ros::Time& stamp, const std::string& base_frame_id, const Eigen::Matrix4f& pose) { // 发布里程计数据
|
|
|
+// // publish transform stamped for IMU integration
|
|
|
+// // geometry_msgs::TransformStamped odom_trans = matrix2transform(stamp, pose, odom_frame_id, base_frame_id);
|
|
|
+// geometry_msgs::TransformStamped odom_trans = matrix2transform(stamp, pose, "odom", base_frame_id);
|
|
|
+// // trans_pub.publish(odom_trans);
|
|
|
+
|
|
|
+// // // broadcast the transform over tf
|
|
|
+// // odom_broadcaster.sendTransform(odom_trans);
|
|
|
+
|
|
|
+// // publish the transform
|
|
|
+// nav_msgs::Odometry odom;
|
|
|
+// odom.header.stamp = stamp;
|
|
|
+// odom.header.frame_id = "odom";
|
|
|
+
|
|
|
+// odom.pose.pose.position.x = pose(0, 3);
|
|
|
+// odom.pose.pose.position.y = pose(1, 3);
|
|
|
+// odom.pose.pose.position.z = pose(2, 3);
|
|
|
+// odom.pose.pose.orientation = odom_trans.transform.rotation;
|
|
|
+
|
|
|
+// odom.child_frame_id = base_frame_id;
|
|
|
+// odom.twist.twist.linear.x = 0.0;
|
|
|
+// odom.twist.twist.linear.y = 0.0;
|
|
|
+// odom.twist.twist.angular.z = 0.0;
|
|
|
+
|
|
|
+// odom_pub.publish(odom);
|
|
|
+// }
|
Mj23366