A-loam-ros2 迁移（ros2-humble）
·
A-loam-ros2
复现结果
B站视频链接：aloam ros2 视频演示，Debug版本，延迟明显
rviz2
ros2 node list
rqt — Node_Graph
代码结构
关键文件
scanRegistration.cpp
// This is an advanced implementation of the algorithm described in the following paper:
// J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time.
// Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014.
// Modifier: Tong Qin qintonguav@gmail.com
// Shaozu Cao saozu.cao@connect.ust.hk
// Copyright 2013, Ji Zhang, Carnegie Mellon University
// Further contributions copyright (c) 2016, Southwest Research Institute
// All rights reserved.
#include <cmath>
#include <vector>
#include <string>
#include "aloam_velodyne_ros2/common.h"
#include "aloam_velodyne_ros2/tic_toc.h"
#include <opencv2/core.hpp>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/imu.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <tf2/transform_datatypes.h>
#include <tf2_ros/transform_broadcaster.h>
using std::atan2;
using std::cos;
using std::sin;
const double scanPeriod = 0.1;
const int systemDelay = 0;
int systemInitCount = 0;
bool systemInited = false;
int N_SCANS = 0;
float cloudCurvature[400000];
int cloudSortInd[400000];
int cloudNeighborPicked[400000];
int cloudLabel[400000];
bool comp (int i,int j) { return (cloudCurvature[i]<cloudCurvature[j]); }
bool PUB_EACH_LINE = false;
double MINIMUM_RANGE = 0.1;
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;
}
class ScanRegistration : public rclcpp::Node
{
public:
ScanRegistration() : Node("scanRegistration")
{
// 读取参数
this->declare_parameter<int>("scan_line", 16);
this->declare_parameter<double>("minimum_range", 0.1);
this->get_parameter("scan_line", N_SCANS);
this->get_parameter("minimum_range", MINIMUM_RANGE);
RCLCPP_INFO(this->get_logger(), "scan line number: %d", N_SCANS);
if(N_SCANS != 16 && N_SCANS != 32 && N_SCANS != 64)
{
RCLCPP_ERROR(this->get_logger(), "only support velodyne with 16, 32 or 64 scan line!");
rclcpp::shutdown();
return;
}
// 订阅
subLaserCloud_ = this->create_subscription<sensor_msgs::msg::PointCloud2>(
"/velodyne_points", 100,
std::bind(&ScanRegistration::laserCloudHandler, this, std::placeholders::_1));
// 发布
pubLaserCloud = this->create_publisher<sensor_msgs::msg::PointCloud2>("/velodyne_cloud_2", 100);
pubCornerPointsSharp = this->create_publisher<sensor_msgs::msg::PointCloud2>("/laser_cloud_sharp", 100);
pubCornerPointsLessSharp = this->create_publisher<sensor_msgs::msg::PointCloud2>("/laser_cloud_less_sharp", 100);
pubSurfPointsFlat = this->create_publisher<sensor_msgs::msg::PointCloud2>("/laser_cloud_flat", 100);
pubSurfPointsLessFlat = this->create_publisher<sensor_msgs::msg::PointCloud2>("/laser_cloud_less_flat", 100);
pubRemovePoints = this->create_publisher<sensor_msgs::msg::PointCloud2>("/laser_remove_points", 100);
if(PUB_EACH_LINE)
{
for(int i = 0; i < N_SCANS; i++)
{
auto tmp = this->create_publisher<sensor_msgs::msg::PointCloud2>(
"/laser_scanid_" + std::to_string(i), 100);
pubEachScan.push_back(tmp);
}
}
}
private:
void laserCloudHandler(const sensor_msgs::msg::PointCloud2::SharedPtr laserCloudMsg)
{
if (!systemInited)
{
systemInitCount++;
if (systemInitCount >= systemDelay)
systemInited = true;
else
return;
}
TicToc t_whole;
TicToc t_prepare;
std::vector<int> scanStartInd(N_SCANS, 0);
std::vector<int> scanEndInd(N_SCANS, 0);
pcl::PointCloud<pcl::PointXYZ> laserCloudIn;
pcl::fromROSMsg(*laserCloudMsg, laserCloudIn);
std::vector<int> indices;
pcl::removeNaNFromPointCloud(laserCloudIn, laserCloudIn, indices);
removeClosedPointCloud(laserCloudIn, laserCloudIn, MINIMUM_RANGE);
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;
if (endOri - startOri > 3 * M_PI)
endOri -= 2 * M_PI;
else if (endOri - startOri < M_PI)
endOri += 2 * M_PI;
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;
float angle = atan(point.z / sqrt(point.x * point.x + point.y * point.y)) * 180 / M_PI;
int scanID = 0;
if (N_SCANS == 16)
{
scanID = int((angle + 15) / 2 + 0.5);
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);
if (angle > 2 || angle < -24.33 || scanID > 50 || scanID < 0)
{ count--; continue; }
}
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;
laserCloudScans[scanID].push_back(point);
}
cloudSize = count;
RCLCPP_INFO(this->get_logger(), "points size %d", cloudSize);
pcl::PointCloud<PointType>::Ptr laserCloud(new pcl::PointCloud<PointType>());
for (int i = 0; i < N_SCANS; i++)//16-dim to 1-dim
{
scanStartInd[i] = laserCloud->size() + 5;
*laserCloud += laserCloudScans[i];
scanEndInd[i] = laserCloud->size() - 6;
}
RCLCPP_INFO(this->get_logger(), "prepare time %f", t_prepare.toc());
for (int i = 5; i < cloudSize - 5; i++)
{
float diffX = laserCloud->points[i - 5].x + laserCloud->points[i - 4].x +
laserCloud->points[i - 3].x + laserCloud->points[i - 2].x +
laserCloud->points[i - 1].x - 10 * laserCloud->points[i].x +
laserCloud->points[i + 1].x + laserCloud->points[i + 2].x +
laserCloud->points[i + 3].x + laserCloud->points[i + 4].x +
laserCloud->points[i + 5].x;
float diffY = laserCloud->points[i - 5].y + laserCloud->points[i - 4].y +
laserCloud->points[i - 3].y + laserCloud->points[i - 2].y +
laserCloud->points[i - 1].y - 10 * laserCloud->points[i].y +
laserCloud->points[i + 1].y + laserCloud->points[i + 2].y +
laserCloud->points[i + 3].y + laserCloud->points[i + 4].y +
laserCloud->points[i + 5].y;
float diffZ = laserCloud->points[i - 5].z + laserCloud->points[i - 4].z +
laserCloud->points[i - 3].z + laserCloud->points[i - 2].z +
laserCloud->points[i - 1].z - 10 * laserCloud->points[i].z +
laserCloud->points[i + 1].z + laserCloud->points[i + 2].z +
laserCloud->points[i + 3].z + laserCloud->points[i + 4].z +
laserCloud->points[i + 5].z;
cloudCurvature[i] = diffX * diffX + diffY * diffY + diffZ * diffZ;
cloudSortInd[i] = i;
cloudNeighborPicked[i] = 0;
cloudLabel[i] = 0;
}
TicToc t_pts;
pcl::PointCloud<PointType> cornerPointsSharp;
pcl::PointCloud<PointType> cornerPointsLessSharp;//diff level
pcl::PointCloud<PointType> surfPointsFlat;
pcl::PointCloud<PointType> surfPointsLessFlat;
float t_q_sort = 0;
for (int i = 0; i < N_SCANS; i++)
{
if( scanEndInd[i] - scanStartInd[i] < 6)
continue;
pcl::PointCloud<PointType>::Ptr surfPointsLessFlatScan(new pcl::PointCloud<PointType>);
for (int j = 0; j < 6; j++)//each line divided into 6 sections
{
int sp = scanStartInd[i] + (scanEndInd[i] - scanStartInd[i]) * j / 6;
int ep = scanStartInd[i] + (scanEndInd[i] - scanStartInd[i]) * (j + 1) / 6 - 1;
TicToc t_tmp;
std::sort (cloudSortInd + sp, cloudSortInd + ep + 1, comp);
t_q_sort += t_tmp.toc();
int largestPickedNum = 0;
for (int k = ep; k >= sp; k--)
{
int ind = cloudSortInd[k];
if (cloudNeighborPicked[ind] == 0 && cloudCurvature[ind] > 0.1)
{
largestPickedNum++;
if (largestPickedNum <= 2)
{
cloudLabel[ind] = 2;
cornerPointsSharp.push_back(laserCloud->points[ind]);
cornerPointsLessSharp.push_back(laserCloud->points[ind]);
}
else if (largestPickedNum <= 20)
{
cloudLabel[ind] = 1;
cornerPointsLessSharp.push_back(laserCloud->points[ind]);
}
else
break;
cloudNeighborPicked[ind] = 1;
for (int l = 1; l <= 5; l++)
{
float dx = laserCloud->points[ind + l].x - laserCloud->points[ind + l - 1].x;
float dy = laserCloud->points[ind + l].y - laserCloud->points[ind + l - 1].y;
float dz = laserCloud->points[ind + l].z - laserCloud->points[ind + l - 1].z;
if (dx*dx + dy*dy + dz*dz > 0.05) break;
cloudNeighborPicked[ind + l] = 1;
}
for (int l = -1; l >= -5; l--)
{
float dx = laserCloud->points[ind + l].x - laserCloud->points[ind + l + 1].x;
float dy = laserCloud->points[ind + l].y - laserCloud->points[ind + l + 1].y;
float dz = laserCloud->points[ind + l].z - laserCloud->points[ind + l + 1].z;
if (dx*dx + dy*dy + dz*dz > 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)
{
cloudLabel[ind] = -1;
surfPointsFlat.push_back(laserCloud->points[ind]);
smallestPickedNum++;
if (smallestPickedNum >= 4)
break;
cloudNeighborPicked[ind] = 1;
for (int l = 1; l <= 5; l++)
{
float dx = laserCloud->points[ind + l].x - laserCloud->points[ind + l - 1].x;
float dy = laserCloud->points[ind + l].y - laserCloud->points[ind + l - 1].y;
float dz = laserCloud->points[ind + l].z - laserCloud->points[ind + l - 1].z;
if (dx*dx + dy*dy + dz*dz > 0.05) break;
cloudNeighborPicked[ind + l] = 1;
}
for (int l = -1; l >= -5; l--)
{
float dx = laserCloud->points[ind + l].x - laserCloud->points[ind + l + 1].x;
float dy = laserCloud->points[ind + l].y - laserCloud->points[ind + l + 1].y;
float dz = laserCloud->points[ind + l].z - laserCloud->points[ind + l + 1].z;
if (dx*dx + dy*dy + dz*dz > 0.05) break;
cloudNeighborPicked[ind + l] = 1;
}
}
}
for (int k = sp; k <= ep; k++)
{
if (cloudLabel[k] <= 0)
surfPointsLessFlatScan->push_back(laserCloud->points[k]);
}
}
pcl::PointCloud<PointType> surfPointsLessFlatScanDS;
pcl::VoxelGrid<PointType> downSizeFilter;
downSizeFilter.setInputCloud(surfPointsLessFlatScan);
downSizeFilter.setLeafSize(0.2, 0.2, 0.2);
downSizeFilter.filter(surfPointsLessFlatScanDS);
surfPointsLessFlat += surfPointsLessFlatScanDS;
}
RCLCPP_INFO(this->get_logger(), "sort q time %f", t_q_sort);
RCLCPP_INFO(this->get_logger(), "seperate points time %f", t_pts.toc());
// 发布点云
sensor_msgs::msg::PointCloud2 laserCloudOutMsg;
pcl::toROSMsg(*laserCloud, laserCloudOutMsg);
laserCloudOutMsg.header = laserCloudMsg->header;
laserCloudOutMsg.header.frame_id = "camera_init";
pubLaserCloud->publish(laserCloudOutMsg);
sensor_msgs::msg::PointCloud2 cornerSharpMsg;
pcl::toROSMsg(cornerPointsSharp, cornerSharpMsg);
cornerSharpMsg.header = laserCloudMsg->header;
cornerSharpMsg.header.frame_id = "camera_init";
pubCornerPointsSharp->publish(cornerSharpMsg);
sensor_msgs::msg::PointCloud2 cornerLessSharpMsg;
pcl::toROSMsg(cornerPointsLessSharp, cornerLessSharpMsg);
cornerLessSharpMsg.header = laserCloudMsg->header;
cornerLessSharpMsg.header.frame_id = "camera_init";
pubCornerPointsLessSharp->publish(cornerLessSharpMsg);
sensor_msgs::msg::PointCloud2 surfFlatMsg;
pcl::toROSMsg(surfPointsFlat, surfFlatMsg);
surfFlatMsg.header = laserCloudMsg->header;
surfFlatMsg.header.frame_id = "camera_init";
pubSurfPointsFlat->publish(surfFlatMsg);
sensor_msgs::msg::PointCloud2 surfLessFlatMsg;
pcl::toROSMsg(surfPointsLessFlat, surfLessFlatMsg);
surfLessFlatMsg.header = laserCloudMsg->header;
surfLessFlatMsg.header.frame_id = "camera_init";
pubSurfPointsLessFlat->publish(surfLessFlatMsg);
if(PUB_EACH_LINE)
{
for(int i = 0; i< N_SCANS; i++)
{
sensor_msgs::msg::PointCloud2 scanMsg;
pcl::toROSMsg(laserCloudScans[i], scanMsg);
scanMsg.header = laserCloudMsg->header;
scanMsg.header.frame_id = "camera_init";
pubEachScan[i]->publish(scanMsg);
}
}
if(t_whole.toc() > 100)
RCLCPP_WARN(this->get_logger(), "scan registration over 100ms");
}
// 订阅器 & 发布器
rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr subLaserCloud_;
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pubLaserCloud;
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pubCornerPointsSharp;
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pubCornerPointsLessSharp;
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pubSurfPointsFlat;
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pubSurfPointsLessFlat;
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pubRemovePoints;
std::vector<rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr> pubEachScan;
};
int main(int argc, char **argv)
{
rclcpp::init(argc, argv);
auto node = std::make_shared<ScanRegistration>();
rclcpp::spin(node);
rclcpp::shutdown();
return 0;
}
laserOdometry.cpp
// This is an advanced implementation of the algorithm described in the following paper:
// J. Zhang and S. Zhang. LOAM: Lidar Odometry and Mapping in Real-time.
// Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014.
// Modifier: Tong Qin qintonguav@gmail.com
// Shaozu Cao saozu.cao@connect.ust.hk
// Copyright 2013, Ji Zhang, Carnegie Mellon University
// Further contributions copyright (c) 2016, Southwest Research Institute
// All rights reserved.
#include <cmath>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl_conversions/pcl_conversions.h>
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/imu.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <tf2/transform_datatypes.h>
#include <tf2_ros/transform_broadcaster.h>
#include <eigen3/Eigen/Dense>
#include <mutex>
#include <queue>
#include "aloam_velodyne_ros2/common.h"
#include "aloam_velodyne_ros2/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;
constexpr double NEARBY_SCAN = 2.5;
int skipFrameNum = 5;
bool systemInited = false;
double timeCornerPointsSharp = 0;
double timeCornerPointsLessSharp = 0;
double timeSurfPointsFlat = 0;
double timeSurfPointsLessFlat = 0;
double timeLaserCloudFullRes = 0;
pcl::KdTreeFLANN<pcl::PointXYZI>::Ptr kdtreeCornerLast(new pcl::KdTreeFLANN<pcl::PointXYZI>());
pcl::KdTreeFLANN<pcl::PointXYZI>::Ptr kdtreeSurfLast(new pcl::KdTreeFLANN<pcl::PointXYZI>());
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;
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};
Eigen::Map<Eigen::Quaterniond> q_last_curr(para_q);//
Eigen::Map<Eigen::Vector3d> t_last_curr(para_t);//
std::queue<sensor_msgs::msg::PointCloud2::SharedPtr> cornerSharpBuf;
std::queue<sensor_msgs::msg::PointCloud2::SharedPtr> cornerLessSharpBuf;
std::queue<sensor_msgs::msg::PointCloud2::SharedPtr> surfFlatBuf;
std::queue<sensor_msgs::msg::PointCloud2::SharedPtr> surfLessFlatBuf;
std::queue<sensor_msgs::msg::PointCloud2::SharedPtr> fullPointsBuf;
std::mutex mBuf;
void TransformToStart(PointType const *const pi, PointType *const po)
{
double s;
if (DISTORTION)
s = (pi->intensity - int(pi->intensity)) / SCAN_PERIOD;
else
s = 1.0;
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();
po->intensity = int(pi->intensity);
}
class LaserOdometry : public rclcpp::Node
{
public:
LaserOdometry() : Node("laserOdometry")
{
this->declare_parameter<int>("mapping_skip_frame", 2);
this->get_parameter("mapping_skip_frame", skipFrameNum);
RCLCPP_INFO(this->get_logger(), "Mapping %d Hz", 10 / skipFrameNum);
subCornerPointsSharp = this->create_subscription<sensor_msgs::msg::PointCloud2>(
"/laser_cloud_sharp", 100,
std::bind(&LaserOdometry::laserCloudSharpHandler, this, std::placeholders::_1));
subCornerPointsLessSharp = this->create_subscription<sensor_msgs::msg::PointCloud2>(
"/laser_cloud_less_sharp", 100,
std::bind(&LaserOdometry::laserCloudLessSharpHandler, this, std::placeholders::_1));
subSurfPointsFlat = this->create_subscription<sensor_msgs::msg::PointCloud2>(
"/laser_cloud_flat", 100,
std::bind(&LaserOdometry::laserCloudFlatHandler, this, std::placeholders::_1));
subSurfPointsLessFlat = this->create_subscription<sensor_msgs::msg::PointCloud2>(
"/laser_cloud_less_flat", 100,
std::bind(&LaserOdometry::laserCloudLessFlatHandler, this, std::placeholders::_1));
subLaserCloudFullRes = this->create_subscription<sensor_msgs::msg::PointCloud2>(
"/velodyne_cloud_2", 100,
std::bind(&LaserOdometry::laserCloudFullResHandler, this, std::placeholders::_1));
pubLaserCloudCornerLast = this->create_publisher<sensor_msgs::msg::PointCloud2>("/laser_cloud_corner_last", 100);
pubLaserCloudSurfLast = this->create_publisher<sensor_msgs::msg::PointCloud2>("/laser_cloud_surf_last", 100);
pubLaserCloudFullRes = this->create_publisher<sensor_msgs::msg::PointCloud2>("/velodyne_cloud_3", 100);
pubLaserOdometry = this->create_publisher<nav_msgs::msg::Odometry>("/laser_odom_to_init", 100);
pubLaserPath = this->create_publisher<nav_msgs::msg::Path>("/laser_odom_path", 100);
timer_ = this->create_wall_timer(std::chrono::milliseconds(10), std::bind(&LaserOdometry::runLoop, this));
}
private:
void laserCloudSharpHandler(const sensor_msgs::msg::PointCloud2::SharedPtr cornerPointsSharp2)
{
std::lock_guard<std::mutex> lock(mBuf);
cornerSharpBuf.push(cornerPointsSharp2);
}
void laserCloudLessSharpHandler(const sensor_msgs::msg::PointCloud2::SharedPtr cornerPointsLessSharp2)
{
std::lock_guard<std::mutex> lock(mBuf);
cornerLessSharpBuf.push(cornerPointsLessSharp2);
}
void laserCloudFlatHandler(const sensor_msgs::msg::PointCloud2::SharedPtr surfPointsFlat2)
{
std::lock_guard<std::mutex> lock(mBuf);
surfFlatBuf.push(surfPointsFlat2);
}
void laserCloudLessFlatHandler(const sensor_msgs::msg::PointCloud2::SharedPtr surfPointsLessFlat2)
{
std::lock_guard<std::mutex> lock(mBuf);
surfLessFlatBuf.push(surfPointsLessFlat2);
}
void laserCloudFullResHandler(const sensor_msgs::msg::PointCloud2::SharedPtr laserCloudFullRes2)
{
std::lock_guard<std::mutex> lock(mBuf);
fullPointsBuf.push(laserCloudFullRes2);
}
void runLoop()
{
if (!cornerSharpBuf.empty() && !cornerLessSharpBuf.empty() &&
!surfFlatBuf.empty() && !surfLessFlatBuf.empty() &&
!fullPointsBuf.empty())
{
timeCornerPointsSharp = cornerSharpBuf.front()->header.stamp.sec;
timeCornerPointsLessSharp = cornerLessSharpBuf.front()->header.stamp.sec;
timeSurfPointsFlat = surfFlatBuf.front()->header.stamp.sec;
timeSurfPointsLessFlat = surfLessFlatBuf.front()->header.stamp.sec;
timeLaserCloudFullRes = fullPointsBuf.front()->header.stamp.sec;
if (timeCornerPointsSharp != timeLaserCloudFullRes ||
timeCornerPointsLessSharp != timeLaserCloudFullRes ||
timeSurfPointsFlat != timeLaserCloudFullRes ||
timeSurfPointsLessFlat != timeLaserCloudFullRes)
{
RCLCPP_ERROR(this->get_logger(), "unsync message!");
rclcpp::shutdown();
return;
}
mBuf.lock();
cornerPointsSharp->clear();
pcl::fromROSMsg(*cornerSharpBuf.front(), *cornerPointsSharp);
cornerSharpBuf.pop();
cornerPointsLessSharp->clear();
pcl::fromROSMsg(*cornerLessSharpBuf.front(), *cornerPointsLessSharp);
cornerLessSharpBuf.pop();
surfPointsFlat->clear();
pcl::fromROSMsg(*surfFlatBuf.front(), *surfPointsFlat);
surfFlatBuf.pop();
surfPointsLessFlat->clear();
pcl::fromROSMsg(*surfLessFlatBuf.front(), *surfPointsLessFlat);
surfLessFlatBuf.pop();
laserCloudFullRes->clear();
pcl::fromROSMsg(*fullPointsBuf.front(), *laserCloudFullRes);
fullPointsBuf.pop();
mBuf.unlock();
TicToc t_whole;
if (!systemInited)
{
systemInited = true;
RCLCPP_INFO(this->get_logger(), "Initialization finished");
}
else
{//基于特征匹配的迭代最近点（ICP）优化，用于计算相邻两帧激光雷达之间的相对运动（位姿变换）
int cornerPointsSharpNum = cornerPointsSharp->points.size();
int surfPointsFlatNum = surfPointsFlat->points.size();
TicToc t_opt;
//双次迭代优化循环
for (size_t opti_counter = 0; opti_counter < 2; ++opti_counter)
{
corner_correspondence = 0;
plane_correspondence = 0;
/*Huber损失函数：鲁棒性优化，减少外点（误匹配点）的影响
四元数参数化：确保旋转矩阵的正交性约束
优化变量：
para_q：4维，旋转四元数 (qx, qy, qz, qw)
para_t：3维，平移向量 (x, y, z)*/
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;
for (int i = 0; i < cornerPointsSharpNum; ++i)
{
// 1. 变换到扫描起始时刻（去畸变）
TransformToStart(&(cornerPointsSharp->points[i]), &pointSel);
// 2. KD树搜索最近邻点
kdtreeCornerLast->nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis);
int closestPointInd = -1, minPointInd2 = -1;
// 3. 在同一扫描线或相邻扫描线中寻找第二个最近邻点
// 构成一条直线（A-B），用于点到线的距离计算
/*当前角点 → 在上一帧中找最近点A →
向前搜索 → 找同一条线上的点（同scan ID）
向后搜索 → 找同一条线上的点（同scan ID）
形成直线 A-B*/
if (pointSearchSqDis[0] < DISTANCE_SQ_THRESHOLD)
{
closestPointInd = pointSearchInd[0];
int closestPointScanID = int(laserCloudCornerLast->points[closestPointInd].intensity);
double minPointSqDis2 = DISTANCE_SQ_THRESHOLD;
for (int j = closestPointInd + 1; j < (int)laserCloudCornerLast->points.size(); ++j)
{
if (int(laserCloudCornerLast->points[j].intensity) <= closestPointScanID)
continue;
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)
{
minPointSqDis2 = pointSqDis;
minPointInd2 = j;
}
}
for (int j = closestPointInd - 1; j >= 0; --j)
{
if (int(laserCloudCornerLast->points[j].intensity) >= closestPointScanID)//intensity字段存储了相对时间（在0-1之间）
continue;
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)
{
minPointSqDis2 = pointSqDis;
minPointInd2 = j;
}
}
}
if (minPointInd2 >= 0)
{
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);
//当DISTORTION启用时，根据点在扫描中的时间戳进行线性插值
//s：归一化的时间比例（0表示扫描开始，1表示扫描结束）
double s = DISTORTION ? (cornerPointsSharp->points[i].intensity -
int(cornerPointsSharp->points[i].intensity)) / SCAN_PERIOD : 1.0;
// 角点：点到线距离
ceres::CostFunction *cost_function = LidarEdgeFactor::Create(curr_point, last_point_a, last_point_b, s);
problem.AddResidualBlock(cost_function, loss_function, para_q, para_t);
corner_correspondence++;
}
}
for (int i = 0; i < surfPointsFlatNum; ++i)
{
TransformToStart(&(surfPointsFlat->points[i]), &pointSel);
kdtreeSurfLast->nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis);
int closestPointInd = -1, minPointInd2 = -1, minPointInd3 = -1;
if (pointSearchSqDis[0] < DISTANCE_SQ_THRESHOLD)
{
closestPointInd = pointSearchInd[0];
int closestPointScanID = int(laserCloudSurfLast->points[closestPointInd].intensity);
double minPointSqDis2 = DISTANCE_SQ_THRESHOLD, minPointSqDis3 = DISTANCE_SQ_THRESHOLD;
for (int j = closestPointInd + 1; j < (int)laserCloudSurfLast->points.size(); ++j)
{
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 (int(laserCloudSurfLast->points[j].intensity) <= closestPointScanID && pointSqDis < minPointSqDis2)
{
minPointSqDis2 = pointSqDis;
minPointInd2 = j;
}
else if (int(laserCloudSurfLast->points[j].intensity) > closestPointScanID && pointSqDis < minPointSqDis3)
{
minPointSqDis3 = pointSqDis;
minPointInd3 = j;
}
}
for (int j = closestPointInd - 1; j >= 0; --j)
{
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 (int(laserCloudSurfLast->points[j].intensity) >= closestPointScanID && pointSqDis < minPointSqDis2)
{
minPointSqDis2 = pointSqDis;
minPointInd2 = j;
}
else if (int(laserCloudSurfLast->points[j].intensity) < closestPointScanID && pointSqDis < minPointSqDis3)
{
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 = DISTORTION ? (surfPointsFlat->points[i].intensity -
int(surfPointsFlat->points[i].intensity)) / SCAN_PERIOD : 1.0;
// 平面点：点到面距离
ceres::CostFunction *cost_function = LidarPlaneFactor::Create(curr_point, last_point_a, last_point_b, last_point_c, s);
problem.AddResidualBlock(cost_function, loss_function, para_q, para_t);
plane_correspondence++;
}
}
RCLCPP_INFO(this->get_logger(), "data association time %f ms", t_data.toc());
if ((corner_correspondence + plane_correspondence) < 10)
{
RCLCPP_WARN(this->get_logger(), "less correspondence!");
}
TicToc t_solver;
ceres::Solver::Options options;
options.linear_solver_type = ceres::DENSE_QR; // 稠密QR分解
options.max_num_iterations = 4; // 最大迭代次数 (平衡精度和计算效率)
options.minimizer_progress_to_stdout = false; // 不输出迭代信息
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);
RCLCPP_INFO(this->get_logger(), "solver time %f ms", t_solver.toc());
}
RCLCPP_INFO(this->get_logger(), "optimization twice time %f", 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;
nav_msgs::msg::Odometry laserOdometry;
laserOdometry.header.frame_id = "camera_init";
laserOdometry.child_frame_id = "laser_odom";
laserOdometry.header.stamp = rclcpp::Time(static_cast<int64_t>(timeSurfPointsLessFlat*1e9));
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::msg::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);
// 交换点云指针
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);
if (frameCount % skipFrameNum == 0)
{
frameCount = 0;
sensor_msgs::msg::PointCloud2 laserCloudCornerLast2;
pcl::toROSMsg(*laserCloudCornerLast, laserCloudCornerLast2);
laserCloudCornerLast2.header.stamp = rclcpp::Time(static_cast<int64_t>(timeSurfPointsLessFlat*1e9));
laserCloudCornerLast2.header.frame_id = "camera";
pubLaserCloudCornerLast->publish(laserCloudCornerLast2);
sensor_msgs::msg::PointCloud2 laserCloudSurfLast2;
pcl::toROSMsg(*laserCloudSurfLast, laserCloudSurfLast2);
laserCloudSurfLast2.header.stamp = rclcpp::Time(static_cast<int64_t>(timeSurfPointsLessFlat*1e9));
laserCloudSurfLast2.header.frame_id = "camera";
pubLaserCloudSurfLast->publish(laserCloudSurfLast2);
sensor_msgs::msg::PointCloud2 laserCloudFullRes3;
pcl::toROSMsg(*laserCloudFullRes, laserCloudFullRes3);
laserCloudFullRes3.header.stamp = rclcpp::Time(static_cast<int64_t>(timeSurfPointsLessFlat*1e9));
laserCloudFullRes3.header.frame_id = "camera";
pubLaserCloudFullRes->publish(laserCloudFullRes3);
}
RCLCPP_INFO(this->get_logger(), "publication time %f ms", t_pub.toc());
RCLCPP_INFO(this->get_logger(), "whole laserOdometry time %f ms\n", t_whole.toc());
if(t_whole.toc() > 100)
RCLCPP_WARN(this->get_logger(), "odometry process over 100ms");
frameCount++;
}
}
rclcpp::TimerBase::SharedPtr timer_;
rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr subCornerPointsSharp;
rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr subCornerPointsLessSharp;
rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr subSurfPointsFlat;
rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr subSurfPointsLessFlat;
rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr subLaserCloudFullRes;
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pubLaserCloudCornerLast;
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pubLaserCloudSurfLast;
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pubLaserCloudFullRes;
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr pubLaserOdometry;
rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr pubLaserPath;
nav_msgs::msg::Path laserPath;
int frameCount = 0;
};
int main(int argc, char **argv)
{
rclcpp::init(argc, argv);
auto node = std::make_shared<LaserOdometry>();
rclcpp::spin(node);
rclcpp::shutdown();
return 0;
}
laserMapping.cpp
// This is an advanced implementation of the algorithm described in the following paper:
// J. Zhang and S. Zhang. LOAM: Lidar Odometry and Mapping in Real-time.
// Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014.
// Modifier: Tong Qin qintonguav@gmail.com
// Shaozu Cao saozu.cao@connect.ust.hk
// Copyright 2013, Ji Zhang, Carnegie Mellon University
// Further contributions copyright (c) 2016, Southwest Research Institute
// All rights reserved.
/*
！！！ Debug 模式需要降低延时
1.1 增加下采样参数
# 修改参数文件
mapping_line_resolution: 0.8 # 从0.4改为0.8
mapping_plane_resolution: 1.2 # 从0.8改为1.2
1.2 减少局部地图范围
// 在laserMapping.cpp中
// 将提取范围从±2改为±1
for (int i = centerCubeI-1; i <= centerCubeI+1; i++)
for (int j = centerCubeJ-1; j <= centerCubeJ+1; j++)
for (int k = centerCubeK-1; k <= centerCubeK+1; k++)
从5×5×3=75个体素 → 3×3×3=27个体素
特征点数量减少约2/3
1.3 降低优化迭代次数
// 修改建图优化
for (int iterCount = 0; iterCount < 1; iterCount++) // 从2改为1
ceres::Solver::Options options;
options.max_num_iterations = 2; // 从4改为2
*/
#include <math.h>
#include <vector>
#include <nav_msgs/msg/odometry.hpp>
#include <nav_msgs/msg/path.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <geometry_msgs/msg/transform_stamped.hpp>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/imu.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <tf2/transform_datatypes.h>
#include <tf2_ros/transform_broadcaster.h>
#include <eigen3/Eigen/Dense>
#include <ceres/ceres.h>
#include <mutex>
#include <queue>
#include <thread>
#include <iostream>
#include <string>
#include "lidarFactor.hpp"
#include "aloam_velodyne_ros2/common.h"
#include "aloam_velodyne_ros2/tic_toc.h"
int frameCount = 0;
double timeLaserCloudCornerLast = 0;
double timeLaserCloudSurfLast = 0;
double timeLaserCloudFullRes = 0;
double timeLaserOdometry = 0;
/*每个网格（cube）大小：50×50×50米 地图网格范围 1050m 以上
总网格数：21×21×11 = 4851个
坐标原点在中心网格中心，不是网格边界
21×21×11网格是全局地图的存储结构：
存储所有历史特征点的累积
通过滑动窗口管理内存
局部地图从中提取用于匹配
*/
// 中心网格索引
int laserCloudCenWidth = 10;// 中心那个网格的x索引
int laserCloudCenHeight = 10;
int laserCloudCenDepth = 5;
int SkipMapFrame = 5;//及时清理缓存
// 网格维度
const int laserCloudWidth = 21;// x轴方向网格数
const int laserCloudHeight = 21;
const int laserCloudDepth = 11;
const int laserCloudNum = laserCloudWidth * laserCloudHeight * laserCloudDepth; //4851
int laserCloudValidInd[125];
int laserCloudSurroundInd[125];
pcl::PointCloud<PointType>::Ptr laserCloudCornerLast(new pcl::PointCloud<PointType>());
pcl::PointCloud<PointType>::Ptr laserCloudSurfLast(new pcl::PointCloud<PointType>());
pcl::PointCloud<PointType>::Ptr laserCloudSurround(new pcl::PointCloud<PointType>());
pcl::PointCloud<PointType>::Ptr laserCloudCornerFromMap(new pcl::PointCloud<PointType>());
pcl::PointCloud<PointType>::Ptr laserCloudSurfFromMap(new pcl::PointCloud<PointType>());
pcl::PointCloud<PointType>::Ptr laserCloudFullRes(new pcl::PointCloud<PointType>());
pcl::PointCloud<PointType>::Ptr laserCloudCornerArray[laserCloudNum];
pcl::PointCloud<PointType>::Ptr laserCloudSurfArray[laserCloudNum];
pcl::KdTreeFLANN<PointType>::Ptr kdtreeCornerFromMap(new pcl::KdTreeFLANN<PointType>());
pcl::KdTreeFLANN<PointType>::Ptr kdtreeSurfFromMap(new pcl::KdTreeFLANN<PointType>());
double parameters[7] = {0, 0, 0, 1, 0, 0, 0};
Eigen::Map<Eigen::Quaterniond> q_w_curr(parameters);
Eigen::Map<Eigen::Vector3d> t_w_curr(parameters + 4);
Eigen::Quaterniond q_wmap_wodom(1, 0, 0, 0);
Eigen::Vector3d t_wmap_wodom(0, 0, 0);
Eigen::Quaterniond q_wodom_curr(1, 0, 0, 0);
Eigen::Vector3d t_wodom_curr(0, 0, 0);
std::queue<sensor_msgs::msg::PointCloud2::SharedPtr> cornerLastBuf;
std::queue<sensor_msgs::msg::PointCloud2::SharedPtr> surfLastBuf;
std::queue<sensor_msgs::msg::PointCloud2::SharedPtr> fullResBuf;
std::queue<nav_msgs::msg::Odometry::SharedPtr> odometryBuf;
std::mutex mBuf;
pcl::VoxelGrid<PointType> downSizeFilterCorner;
pcl::VoxelGrid<PointType> downSizeFilterSurf;
std::vector<int> pointSearchInd;
std::vector<float> pointSearchSqDis;
PointType pointOri, pointSel;
nav_msgs::msg::Path laserAfterMappedPath;
std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pubLaserCloudSurround;
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pubLaserCloudMap;
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pubLaserCloudFullRes;
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr pubOdomAftMapped;
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr pubOdomAftMappedHighFrec;
rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr pubLaserAfterMappedPath;
rclcpp::Node::SharedPtr node_;
void transformAssociateToMap()
{
q_w_curr = q_wmap_wodom * q_wodom_curr;
t_w_curr = q_wmap_wodom * t_wodom_curr + t_wmap_wodom;
}
void transformUpdate()
{
q_wmap_wodom = q_w_curr * q_wodom_curr.inverse();
t_wmap_wodom = t_w_curr - q_wmap_wodom * t_wodom_curr;
}
void pointAssociateToMap(PointType const *const pi, PointType *const po)
{
Eigen::Vector3d point_curr(pi->x, pi->y, pi->z);
Eigen::Vector3d point_w = q_w_curr * point_curr + t_w_curr;
po->x = point_w.x();
po->y = point_w.y();
po->z = point_w.z();
po->intensity = pi->intensity;
}
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::msg::PointCloud2::SharedPtr laserCloudCornerLast2)
{
mBuf.lock();
cornerLastBuf.push(laserCloudCornerLast2);
mBuf.unlock();
}
void laserCloudSurfLastHandler(const sensor_msgs::msg::PointCloud2::SharedPtr laserCloudSurfLast2)
{
mBuf.lock();
surfLastBuf.push(laserCloudSurfLast2);
mBuf.unlock();
}
void laserCloudFullResHandler(const sensor_msgs::msg::PointCloud2::SharedPtr laserCloudFullRes2)
{
mBuf.lock();
fullResBuf.push(laserCloudFullRes2);
mBuf.unlock();
}
void laserOdometryHandler(const nav_msgs::msg::Odometry::SharedPtr laserOdometry)
{
mBuf.lock();
odometryBuf.push(laserOdometry);
mBuf.unlock();
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::msg::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()
{
// static int skip_counter = 0;//Debug 模式
while(rclcpp::ok())
{
// if (skip_counter % SkipMapFrame == 0) { // 每3帧处理1帧
// // 清空队列，避免堆积
// mBuf.lock();
// while (!cornerLastBuf.empty()) cornerLastBuf.pop();
// while (!surfLastBuf.empty()) surfLastBuf.pop();
// while (!fullResBuf.empty()) fullResBuf.pop();
// while (!odometryBuf.empty()) odometryBuf.pop();
// mBuf.unlock();
// skip_counter = 0;
// }
// skip_counter++;
while (!cornerLastBuf.empty() && !surfLastBuf.empty() &&
!fullResBuf.empty() && !odometryBuf.empty())
{
mBuf.lock();
while (!odometryBuf.empty() && (odometryBuf.front()->header.stamp.sec) < (cornerLastBuf.front()->header.stamp.sec))
odometryBuf.pop();
if (odometryBuf.empty()) { mBuf.unlock(); break; }
while (!surfLastBuf.empty() && (surfLastBuf.front()->header.stamp.sec) < (cornerLastBuf.front()->header.stamp.sec))
surfLastBuf.pop();
if (surfLastBuf.empty()) { mBuf.unlock(); break; }
while (!fullResBuf.empty() && (fullResBuf.front()->header.stamp.sec) < (cornerLastBuf.front()->header.stamp.sec))
fullResBuf.pop();
if (fullResBuf.empty()) { mBuf.unlock(); break; }
timeLaserCloudCornerLast = cornerLastBuf.front()->header.stamp.sec;
timeLaserCloudSurfLast = surfLastBuf.front()->header.stamp.sec;
timeLaserCloudFullRes = fullResBuf.front()->header.stamp.sec;
timeLaserOdometry = odometryBuf.front()->header.stamp.sec;
if (timeLaserCloudCornerLast != timeLaserOdometry ||
timeLaserCloudSurfLast != timeLaserOdometry ||
timeLaserCloudFullRes != timeLaserOdometry)
{
RCLCPP_ERROR(node_->get_logger(), "unsync message!");
mBuf.unlock();
break;
}
laserCloudCornerLast->clear();
pcl::fromROSMsg(*cornerLastBuf.front(), *laserCloudCornerLast);
cornerLastBuf.pop();
laserCloudSurfLast->clear();
pcl::fromROSMsg(*surfLastBuf.front(), *laserCloudSurfLast);
surfLastBuf.pop();
laserCloudFullRes->clear();
pcl::fromROSMsg(*fullResBuf.front(), *laserCloudFullRes);
fullResBuf.pop();
q_wodom_curr.x() = odometryBuf.front()->pose.pose.orientation.x;
q_wodom_curr.y() = odometryBuf.front()->pose.pose.orientation.y;
q_wodom_curr.z() = odometryBuf.front()->pose.pose.orientation.z;
q_wodom_curr.w() = odometryBuf.front()->pose.pose.orientation.w;
t_wodom_curr.x() = odometryBuf.front()->pose.pose.position.x;
t_wodom_curr.y() = odometryBuf.front()->pose.pose.position.y;
t_wodom_curr.z() = odometryBuf.front()->pose.pose.position.z;
odometryBuf.pop();
while(!cornerLastBuf.empty()) {cornerLastBuf.pop();}
mBuf.unlock();
TicToc t_whole;
transformAssociateToMap();
TicToc t_shift;
// 当前位姿所在的网格索引计算
/*分解计算：
1.t_w_curr.x() + 25.0：将世界坐标系平移到网格中心
网格范围：[-25, 25]，中心是0
加25后范围：[0, 50]
2.(x+25)/50.0：计算归一化网格坐标
结果范围：[0, 1.0)
3. int()：取整得到相对于中心网格的偏移（中心网格索引为10: -25~25范围都是)
例如：当前位置x=12.5 → (12.5+25)/50=0.75 → int=0
当前位置x=-12.5 → (-12.5+25)/50=0.25 → int=0
4. + laserCloudCenWidth：转换为绝对网格索引*/
int centerCubeI = int((t_w_curr.x() + 25.0) / 50.0) + laserCloudCenWidth;
int centerCubeJ = int((t_w_curr.y() + 25.0) / 50.0) + laserCloudCenHeight;
int centerCubeK = int((t_w_curr.z() + 25.0) / 50.0) + laserCloudCenDepth;
// 处理int()函数对负数的截断问题 int(-1.5)=-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--;
// cube shift logic (unchanged)
/*为什么要滑动？
机器人不断移动，但地图存储空间有限
需要保持机器人位于地图中心区域
当机器人接近地图边界时，移动地图而不是扩展地图
// 注意：laserCloudWidth=21，索引0-20，边界是索引3和18
*/
while (centerCubeI < 3) // 机器人太靠近左边界
{
for (int j = 0; j < laserCloudHeight; j++) for (int k = 0; k < laserCloudDepth; k++)
{
int i = laserCloudWidth - 1;
auto c = laserCloudCornerArray[i + laserCloudWidth*j + laserCloudWidth*laserCloudHeight*k];
auto s = laserCloudSurfArray[i + laserCloudWidth*j + laserCloudWidth*laserCloudHeight*k];
for (; i >= 1; i--) {
laserCloudCornerArray[i + laserCloudWidth*j + laserCloudWidth*laserCloudHeight*k] =
laserCloudCornerArray[i-1 + laserCloudWidth*j + laserCloudWidth*laserCloudHeight*k];
laserCloudSurfArray[i + laserCloudWidth*j + laserCloudWidth*laserCloudHeight*k] =
laserCloudSurfArray[i-1 + laserCloudWidth*j + laserCloudWidth*laserCloudHeight*k];
}
laserCloudCornerArray[i + laserCloudWidth*j + laserCloudWidth*laserCloudHeight*k] = c;
laserCloudSurfArray[i + laserCloudWidth*j + laserCloudWidth*laserCloudHeight*k] = s;
c->clear();
s->clear();
}
centerCubeI++;
laserCloudCenWidth++;
}
while (centerCubeI >= laserCloudWidth - 3) //18 机器人太靠近右边界
{
for (int j = 0; j < laserCloudHeight; j++) for (int k = 0; k < laserCloudDepth; k++)
{
int i = 0;
auto c = laserCloudCornerArray[i + laserCloudWidth*j + laserCloudWidth*laserCloudHeight*k];
auto s = 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] = c;
laserCloudSurfArray[i + laserCloudWidth*j + laserCloudWidth*laserCloudHeight*k] = s;
c->clear();
s->clear();
}
centerCubeI--;
laserCloudCenWidth--;
}
while (centerCubeJ < 3)
{
for (int i = 0; i < laserCloudWidth; i++) for (int k = 0; k < laserCloudDepth; k++)
{
int j = laserCloudHeight-1;
auto c = laserCloudCornerArray[i + laserCloudWidth*j + laserCloudWidth*laserCloudHeight*k];
auto s = 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] = c;
laserCloudSurfArray[i + laserCloudWidth*j + laserCloudWidth*laserCloudHeight*k] = s;
c->clear();
s->clear();
}
centerCubeJ++;
laserCloudCenHeight++;
}
while (centerCubeJ >= laserCloudHeight-3)
{
for (int i = 0; i < laserCloudWidth; i++) for (int k = 0; k < laserCloudDepth; k++)
{
int j = 0;
auto c = laserCloudCornerArray[i + laserCloudWidth*j + laserCloudWidth*laserCloudHeight*k];
auto s = 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] = c;
laserCloudSurfArray[i + laserCloudWidth*j + laserCloudWidth*laserCloudHeight*k] = s;
c->clear();
s->clear();
}
centerCubeJ--;
laserCloudCenHeight--;
}
while (centerCubeK < 3)
{
for (int i = 0; i < laserCloudWidth; i++) for (int j = 0; j < laserCloudHeight; j++)
{
int k = laserCloudDepth-1;
auto c = laserCloudCornerArray[i + laserCloudWidth*j + laserCloudWidth*laserCloudHeight*k];
auto s = 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] = c;
laserCloudSurfArray[i + laserCloudWidth*j + laserCloudWidth*laserCloudHeight*k] = s;
c->clear();
s->clear();
}
centerCubeK++;
laserCloudCenDepth++;
}
while (centerCubeK >= laserCloudDepth-3)
{
for (int i = 0; i < laserCloudWidth; i++) for (int j = 0; j < laserCloudHeight; j++)
{
int k = 0;
auto c = laserCloudCornerArray[i + laserCloudWidth*j + laserCloudWidth*laserCloudHeight*k];
auto s = 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] = c;
laserCloudSurfArray[i + laserCloudWidth*j + laserCloudWidth*laserCloudHeight*k] = s;
c->clear();
s->clear();
}
centerCubeK--;
laserCloudCenDepth--;
}
// 局部地图提取 源代码512行
/*x,y方向：±2个网格（共5×5）
z方向：±1个网格（共3层）
总网格数：5×5×3=75个（最大125个，边界时更少）*/
int laserCloudValidNum = 0, laserCloudSurroundNum = 0;
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)
{
laserCloudValidInd[laserCloudValidNum++] = i + laserCloudWidth*j + laserCloudWidth*laserCloudHeight*k;
laserCloudSurroundInd[laserCloudSurroundNum++] = i + laserCloudWidth*j + laserCloudWidth*laserCloudHeight*k;
}
}
// 清空之前的局部地图
laserCloudCornerFromMap->clear();
laserCloudSurfFromMap->clear();
// 收集有效区域的地图点
/*laserCloudValidInd：之前计算的75个有效体素索引
将机器人周围的有效体素中的点云合并到局部地图
局部地图范围：±2网格（xy方向），±1网格（z方向）*/
for (int i=0; i<laserCloudValidNum; i++)
{
*laserCloudCornerFromMap += *laserCloudCornerArray[laserCloudValidInd[i]];
*laserCloudSurfFromMap += *laserCloudSurfArray[laserCloudValidInd[i]];
}
//角点特征下采样
pcl::PointCloud<PointType>::Ptr laserCloudCornerStack(new pcl::PointCloud<PointType>());
downSizeFilterCorner.setInputCloud(laserCloudCornerLast);
downSizeFilterCorner.filter(*laserCloudCornerStack);
//平面点特征下采样
pcl::PointCloud<PointType>::Ptr laserCloudSurfStack(new pcl::PointCloud<PointType>());
downSizeFilterSurf.setInputCloud(laserCloudSurfLast);
downSizeFilterSurf.filter(*laserCloudSurfStack);
printf("map prepare time %f ms\n", t_shift.toc());
printf("map corner num %d surf num %d \n", laserCloudCornerFromMap->size(), laserCloudSurfFromMap->size());
/*需要足够的地图特征点才能进行优化
角点至少10个，平面点至少50个*/
if (laserCloudCornerFromMap->size() > 10 && laserCloudSurfFromMap->size() > 50)
{
TicToc t_opt;
TicToc t_tree;
kdtreeCornerFromMap->setInputCloud(laserCloudCornerFromMap);
kdtreeSurfFromMap->setInputCloud(laserCloudSurfFromMap);
for (int iter=0; iter<2; iter++)
{
ceres::LossFunction *loss = new ceres::HuberLoss(0.1);
ceres::LocalParameterization *q_param = new ceres::EigenQuaternionParameterization();
ceres::Problem problem;
problem.AddParameterBlock(parameters, 4, q_param);
problem.AddParameterBlock(parameters+4, 3);
TicToc t_data;
for (auto &p : laserCloudCornerStack->points) {
pointOri = p;
// 转换到地图坐标系（使用当前估计的位姿）
pointAssociateToMap(&p, &pointSel);
// 搜索最近5个地图角点
kdtreeCornerFromMap->nearestKSearch(pointSel,5,pointSearchInd,pointSearchSqDis);
// 线特征判断 // 最大距离小于1米 确保匹配点足够近
if (pointSearchSqDis[4] < 1.0)
{
std::vector<Eigen::Vector3d> ns;
Eigen::Vector3d cen(0,0,0);
// 计算5个点的中心
for (int j=0;j<5;j++)
{
auto &pp = laserCloudCornerFromMap->points[pointSearchInd[j]];
Eigen::Vector3d tmp(pp.x,pp.y,pp.z);
cen += tmp; ns.push_back(tmp);
}
cen /=5;
// 计算协方差矩阵
Eigen::Matrix3d cov = Eigen::Matrix3d::Zero();
for (auto &n : ns)
{
auto t = n - cen;
cov += t*t.transpose();
}
// 特征值分解
Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> saes(cov);
// 线特征判断条件（构建点到线的距离残差）
if (saes.eigenvalues()[2] > 3*saes.eigenvalues()[1])
{
// 线的方向向量
auto dir = saes.eigenvectors().col(2);
// 线上两点
Eigen::Vector3d curr(p.x,p.y,p.z);
Eigen::Vector3d a = cen + 0.1*dir;
Eigen::Vector3d b = cen - 0.1*dir;
// 构建残差
auto cost = LidarEdgeFactor::Create(curr,a,b,1.0);
problem.AddResidualBlock(cost, loss, parameters, parameters+4);
}
}
}
for (auto &p : laserCloudSurfStack->points) {
pointOri = p;
pointAssociateToMap(&p, &pointSel);
kdtreeSurfFromMap->nearestKSearch(pointSel,5,pointSearchInd,pointSearchSqDis);
if (pointSearchSqDis[4] <1.0) {
// 拟合平面（ 构建线性方程组：n·x + d = 0）
Eigen::Matrix<double,5,3> A;
Eigen::Matrix<double,5,1> b = -Eigen::Matrix<double,5,1>::Ones();
for (int j=0;j<5;j++)
{
auto &pp = laserCloudSurfFromMap->points[pointSearchInd[j]];
A(j,0)=pp.x;
A(j,1)=pp.y;
A(j,2)=pp.z;
}
// 求解平面法向量 n
Eigen::Vector3d n = A.colPivHouseholderQr().solve(b);
double inv = 1/n.norm(); // d = -1/|n|
n.normalize(); // 单位化法向量
// 平面拟合质量检查：
bool ok = true;
for (int j=0;j<5;j++) {
auto &pp = laserCloudSurfFromMap->points[pointSearchInd[j]];
if (fabs(n.x()*pp.x + n.y()*pp.y + n.z()*pp.z + inv) >0.2) {
ok=false; break;
}
}
// 构建点到面的距离残差：
if (ok) {
Eigen::Vector3d curr(p.x,p.y,p.z);
auto cost = LidarPlaneNormFactor::Create(curr,n,inv);
problem.AddResidualBlock(cost, loss, parameters, parameters+4);
}
}
}
printf("mapping data assosiation time %f ms \n", t_data.toc());
//优化求解
TicToc t_solver;
ceres::Solver::Options opt;
opt.linear_solver_type=ceres::DENSE_QR;
opt.max_num_iterations=4;
ceres::Solver::Summary sum;
ceres::Solve(opt,&problem,&sum);
printf("mapping solver time %f ms \n", t_solver.toc());
}
printf("mapping optimization time %f \n", t_opt.toc());
}
transformUpdate();
TicToc t_add;
// 将当前帧加入地图
// 将优化后的角点加入地图
for (auto &p : laserCloudCornerStack->points) {
pointAssociateToMap(&p, &pointSel);// 用优化后的位姿转换
// 计算体素索引并加入对应体素
int ci = int((pointSel.x+25)/50) + laserCloudCenWidth;
int cj = int((pointSel.y+25)/50) + laserCloudCenHeight;
int ck = int((pointSel.z+25)/50) + laserCloudCenDepth;
if (pointSel.x+25<0) ci--;
if (pointSel.y+25<0) cj--;
if (pointSel.z+25<0) ck--;
// 索引计算和边界检查
if (ci>=0&&ci<laserCloudWidth &&
cj>=0&&cj<laserCloudHeight &&
ck>=0&&ck<laserCloudDepth)
{
int idx = ci + laserCloudWidth*cj + laserCloudWidth*laserCloudHeight*ck;
laserCloudCornerArray[idx]->push_back(pointSel);
}
}
for (auto &p : laserCloudSurfStack->points) {
pointAssociateToMap(&p, &pointSel);
int ci = int((pointSel.x+25)/50) + laserCloudCenWidth;
int cj = int((pointSel.y+25)/50) + laserCloudCenHeight;
int ck = int((pointSel.z+25)/50) + laserCloudCenDepth;
if (pointSel.x+25<0) ci--;
if (pointSel.y+25<0) cj--;
if (pointSel.z+25<0) ck--;
if (ci>=0&&ci<laserCloudWidth &&
cj>=0&&cj<laserCloudHeight &&
ck>=0&&ck<laserCloudDepth) {
int idx = ci + laserCloudWidth*cj + laserCloudWidth*laserCloudHeight*ck;
laserCloudSurfArray[idx]->push_back(pointSel);
}
}
printf("add points time %f ms\n", t_add.toc());
TicToc t_filter;
// 地图维护与下采样
for (int i=0; i<laserCloudValidNum; i++)
{
int idx = laserCloudValidInd[i];
pcl::PointCloud<PointType>::Ptr tmp(new pcl::PointCloud<PointType>());
downSizeFilterCorner.setInputCloud(laserCloudCornerArray[idx]);
downSizeFilterCorner.filter(*tmp);
laserCloudCornerArray[idx] = tmp;
tmp.reset(new pcl::PointCloud<PointType>());
downSizeFilterSurf.setInputCloud(laserCloudSurfArray[idx]);
downSizeFilterSurf.filter(*tmp);
laserCloudSurfArray[idx] = tmp;
}
printf("filter time %f ms \n", t_filter.toc());
//局部地图发布（每5帧）
TicToc t_pub;
if (frameCount %5 ==0) {
laserCloudSurround->clear();
for (int i=0; i<laserCloudSurroundNum; i++)
{
int idx = laserCloudSurroundInd[i];
*laserCloudSurround += *laserCloudCornerArray[idx];
*laserCloudSurround += *laserCloudSurfArray[idx];
}
sensor_msgs::msg::PointCloud2 msg;
pcl::toROSMsg(*laserCloudSurround, msg);
msg.header.stamp = rclcpp::Time(static_cast<int64_t>(timeLaserOdometry*1e9));
msg.header.frame_id = "camera_init";
pubLaserCloudSurround->publish(msg);
}
//全局地图发布（每20帧）
if (frameCount%20 ==0) {
pcl::PointCloud<PointType> map;
for (int i=0; i<laserCloudNum; i++)
{
map += *laserCloudCornerArray[i];
map += *laserCloudSurfArray[i];
}
sensor_msgs::msg::PointCloud2 msg;
pcl::toROSMsg(map, msg);
msg.header.stamp = rclcpp::Time(static_cast<int64_t>(timeLaserOdometry*1e9));
msg.header.frame_id = "camera_init";
pubLaserCloudMap->publish(msg);
}
for (auto &p : laserCloudFullRes->points) pointAssociateToMap(&p, &p);
sensor_msgs::msg::PointCloud2 msg_full;
pcl::toROSMsg(*laserCloudFullRes, msg_full);
msg_full.header.stamp = rclcpp::Time(static_cast<int64_t>(timeLaserOdometry*1e9));
msg_full.header.frame_id = "camera_init";
pubLaserCloudFullRes->publish(msg_full);
printf("mapping pub time %f ms \n", t_pub.toc());
printf("whole mapping time %f ms +++++\n", t_whole.toc());
nav_msgs::msg::Odometry odom;
odom.header.frame_id = "camera_init";
odom.child_frame_id = "aft_mapped";
odom.header.stamp = rclcpp::Time(static_cast<int64_t>(timeLaserOdometry*1e9));
odom.pose.pose.orientation.x = q_w_curr.x();
odom.pose.pose.orientation.y = q_w_curr.y();
odom.pose.pose.orientation.z = q_w_curr.z();
odom.pose.pose.orientation.w = q_w_curr.w();
odom.pose.pose.position.x = t_w_curr.x();
odom.pose.pose.position.y = t_w_curr.y();
odom.pose.pose.position.z = t_w_curr.z();
pubOdomAftMapped->publish(odom);
geometry_msgs::msg::PoseStamped pose;
pose.header = odom.header;
pose.pose = odom.pose.pose;
laserAfterMappedPath.header.stamp = odom.header.stamp;
laserAfterMappedPath.header.frame_id = "camera_init";
laserAfterMappedPath.poses.push_back(pose);
pubLaserAfterMappedPath->publish(laserAfterMappedPath);
geometry_msgs::msg::TransformStamped tf;
tf.header.stamp = odom.header.stamp;
tf.header.frame_id = "camera_init";
tf.child_frame_id = "aft_mapped";
tf.transform.translation.x = t_w_curr.x();
tf.transform.translation.y = t_w_curr.y();
tf.transform.translation.z = t_w_curr.z();
tf.transform.rotation.x = q_w_curr.x();
tf.transform.rotation.y = q_w_curr.y();
tf.transform.rotation.z = q_w_curr.z();
tf.transform.rotation.w = q_w_curr.w();
tf_broadcaster_->sendTransform(tf);
frameCount++;
}
std::this_thread::sleep_for(std::chrono::milliseconds(2));
}
}
int main(int argc, char **argv)
{
rclcpp::init(argc, argv);
node_ = std::make_shared<rclcpp::Node>("laserMapping");
tf_broadcaster_ = std::make_unique<tf2_ros::TransformBroadcaster>(*node_);
//增加下采样参数（去launch文件修改）
float lineRes = 0.4;
float planeRes = 1.2;
node_->declare_parameter<float>("mapping_line_resolution", lineRes);
node_->declare_parameter<float>("mapping_plane_resolution", planeRes);
node_->get_parameter("mapping_line_resolution", lineRes);
node_->get_parameter("mapping_plane_resolution", planeRes);
RCLCPP_INFO(node_->get_logger(), "line %.2f plane %.2f", lineRes, planeRes);
downSizeFilterCorner.setLeafSize(lineRes,lineRes,lineRes);
downSizeFilterSurf.setLeafSize(planeRes,planeRes,planeRes);
auto sub1 = node_->create_subscription<sensor_msgs::msg::PointCloud2>("/laser_cloud_corner_last",100,laserCloudCornerLastHandler);
auto sub2 = node_->create_subscription<sensor_msgs::msg::PointCloud2>("/laser_cloud_surf_last",100,laserCloudSurfLastHandler);
auto sub3 = node_->create_subscription<nav_msgs::msg::Odometry>("/laser_odom_to_init",100,laserOdometryHandler);
auto sub4 = node_->create_subscription<sensor_msgs::msg::PointCloud2>("/velodyne_cloud_3",100,laserCloudFullResHandler);
pubLaserCloudSurround = node_->create_publisher<sensor_msgs::msg::PointCloud2>("/laser_cloud_surround",100);
pubLaserCloudMap = node_->create_publisher<sensor_msgs::msg::PointCloud2>("/laser_cloud_map",100);
pubLaserCloudFullRes = node_->create_publisher<sensor_msgs::msg::PointCloud2>("/velodyne_cloud_registered",100);
pubOdomAftMapped = node_->create_publisher<nav_msgs::msg::Odometry>("/aft_mapped_to_init",100);
pubOdomAftMappedHighFrec = node_->create_publisher<nav_msgs::msg::Odometry>("/aft_mapped_to_init_high_frec",100);//直接从里程计获取位姿
pubLaserAfterMappedPath = node_->create_publisher<nav_msgs::msg::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};
rclcpp::spin(node_);
rclcpp::shutdown();
return 0;
}
lidarFactor.hpp
// Author: Tong Qin qintonguav@gmail.com
// Shaozu Cao saozu.cao@connect.ust.hk
#pragma once
#include <ceres/ceres.h>
#include <ceres/rotation.h>
#include <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
{
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], 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;
Eigen::Matrix<T, 3, 1> nu = (lp - lpa).cross(lp - lpb);
Eigen::Matrix<T, 3, 1> de = lpa - lpb;
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>(
new LidarEdgeFactor(curr_point_, last_point_a_, last_point_b_, s_)));
}
Eigen::Vector3d curr_point, last_point_a, last_point_b;
double s;
};
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_)
{
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::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_norm.cast<T>());
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;
};
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;
};
CMakeList.txt
cmake_minimum_required(VERSION 3.8)
project(aloam_velodyne_ros2)
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
# 设置 C++ 标准
set(CMAKE_CXX_STANDARD 14)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
# ROS2 依赖
find_package(ament_cmake REQUIRED)
find_package(glog REQUIRED)
find_package(rclcpp REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(nav_msgs REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(tf2_ros REQUIRED)
find_package(tf2_geometry_msgs REQUIRED)
find_package(pcl_conversions REQUIRED)
find_package(pcl_ros REQUIRED)
find_package(cv_bridge REQUIRED)
find_package(image_transport REQUIRED)
find_package(rosbag2_cpp REQUIRED)
# 第三方库
find_package(Eigen3 REQUIRED)
find_package(OpenCV REQUIRED)
find_package(Ceres REQUIRED)
find_package(PCL REQUIRED)
# 包含目录
include_directories(
include
${EIGEN3_INCLUDE_DIR}
${OpenCV_INCLUDE_DIRS}
${CERES_INCLUDE_DIRS}
${PCL_INCLUDE_DIRS}
)
# ==============================================
# A-LOAM 核心节点
# ==============================================
# 1. 特征提取
add_executable(ascanRegistration src/scanRegistration.cpp)
ament_target_dependencies(ascanRegistration
rclcpp
sensor_msgs
nav_msgs
geometry_msgs
tf2_ros
tf2_geometry_msgs
pcl_conversions
pcl_ros
Eigen3
)
target_link_libraries(ascanRegistration
${PCL_LIBRARIES}
${OpenCV_LIBRARIES}
)
# 2. 激光里程计
add_executable(alaserOdometry src/laserOdometry.cpp)
ament_target_dependencies(alaserOdometry
rclcpp
sensor_msgs
nav_msgs
geometry_msgs
tf2_ros
tf2_geometry_msgs
pcl_conversions
pcl_ros
Eigen3
)
target_link_libraries(alaserOdometry
${PCL_LIBRARIES}
${OpenCV_LIBRARIES}
ceres
glog::glog
)
# 3. 激光建图
add_executable(alaserMapping src/laserMapping.cpp)
ament_target_dependencies(alaserMapping
rclcpp
sensor_msgs
nav_msgs
geometry_msgs
tf2_ros
tf2_geometry_msgs
pcl_conversions
pcl_ros
Eigen3
)
target_link_libraries(alaserMapping
${PCL_LIBRARIES}
${OpenCV_LIBRARIES}
ceres
glog::glog
)
# ==============================================
# 安装目标
# ==============================================
install(TARGETS
ascanRegistration
alaserOdometry
alaserMapping
# atransformIntegration
DESTINATION lib/${PROJECT_NAME}
)
# 安装头文件
install(DIRECTORY include/${PROJECT_NAME}/
DESTINATION include/${PROJECT_NAME}
)
# 安装 launch 与 rviz 配置
install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})
install(DIRECTORY rviz_cfg DESTINATION share/${PROJECT_NAME})
# 导出包含目录
ament_export_include_directories(include)
ament_export_dependencies(
rclcpp
sensor_msgs
nav_msgs
geometry_msgs
tf2_ros
tf2_geometry_msgs
pcl_conversions
pcl_ros
cv_bridge
image_transport
rosbag2_cpp
Eigen3
OpenCV
Ceres
PCL
glog
)
# 测试
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
ament_lint_auto_find_test_dependencies()
endif()
ament_package()
package.xml
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>aloam_velodyne_ros2</name>
<version>0.0.0</version>
<description>ROS2 Humble port of A-LOAM</description>
<maintainer email="user@todo.todo">user</maintainer>
<license>GPLv3</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>rclcpp</depend>
<depend>sensor_msgs</depend>
<depend>nav_msgs</depend>
<depend>geometry_msgs</depend>
<depend>tf2_ros</depend>
<depend>tf2_geometry_msgs</depend>
<depend>pcl_conversions</depend>
<depend>pcl_ros</depend>
<depend>eigen3</depend>
<depend>opencv</depend>
<depend>libpcl-dev</depend>
<depend>libeigen3-dev</depend>
<depend>libopencv-dev</depend>
<depend>libceres-dev</depend>
<depend>libgoogle-glog-dev</depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>
启动文件
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.conditions import IfCondition
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
def generate_launch_description():
# 启动参数：是否打开 rviz
rviz_arg = DeclareLaunchArgument(
'rviz', default_value='true',
description='Open RVIZ if true'
)
# 包路径
package_name = 'aloam_velodyne_ros2'
rviz_config_path = PathJoinSubstitution([
FindPackageShare(package_name),
'rviz_cfg',
'aloam_velodyne_ros2.rviz'
])
# ===================== 全局参数 =====================
scan_line = 16
mapping_skip_frame = 1 #2
minimum_range = 1.0
mapping_line_res = 0.8 #0.2
mapping_plane_res = 1.2 #0.4
# ===================== 三个 A-LOAM 核心节点 =====================
ascanRegistration_node = Node(
package='aloam_velodyne_ros2',
executable='ascanRegistration',
name='ascanRegistration',
output='screen',
parameters=[
{'scan_line': scan_line},
{'minimum_range': minimum_range}
]
)
alaserOdometry_node = Node(
package='aloam_velodyne_ros2',
executable='alaserOdometry',
name='alaserOdometry',
output='screen'
)
alaserMapping_node = Node(
package='aloam_velodyne_ros2',
executable='alaserMapping',
name='alaserMapping',
output='screen',
parameters=[
{'mapping_skip_frame': mapping_skip_frame},
{'mapping_line_resolution': mapping_line_res},
{'mapping_plane_resolution': mapping_plane_res}
]
)
transform_map = Node(
package='tf2_ros',
executable='static_transform_publisher',
name='camera_init_to_map',
# arguments=['0', '0', '0', '1.570795', '0', '1.570795', 'map', 'camera_init'],
arguments=['0', '0', '0', '0', '0', '0', 'map', 'camera_init'],
)
# ===================== RVIZ =====================
rviz_node = Node(
package='rviz2',
executable='rviz2',
name='rviz2',
arguments=['-d', rviz_config_path],
condition=IfCondition(LaunchConfiguration('rviz'))
)
return LaunchDescription([
rviz_arg,
ascanRegistration_node,
alaserOdometry_node,
alaserMapping_node,
rviz_node,
transform_map
])
立足具身智能前沿赛道，致力于搭建全球化、开源化、全栈式技术交流与实践共创平台。
更多推荐
6
0- 0
已为社区贡献3条内容
所有评论(0)