知道aloam的朋友们知道这个代码是不包含回环检测的
而有大神们对此添加了sc回环检测
来试着改动一下这个代码
虽然大部分aloam本身的代码都没有改动
但经过博主我的精细对比发现了有如下不同
修改laserMapping.cpp文件
添加了 sensor_msgs::PointCloud2 laserCloudFullRes3Local相关的代码
在大概140多行
添加
ros::Publisher pubLaserCloudFullResLocal;
变成
PointType pointOri, pointSel;
ros::Publisher pubLaserCloudSurround, pubLaserCloudMap, pubLaserCloudFullRes, pubOdomAftMapped, pubOdomAftMappedHighFrec, pubLaserAfterMappedPath;
ros::Publisher pubLaserCloudFullResLocal;
nav_msgs::Path laserAfterMappedPath;
大概是880行左右,添加了
sensor_msgs::PointCloud2 laserCloudFullRes3Local;
pcl::toROSMsg(*laserCloudFullRes, laserCloudFullRes3Local);
laserCloudFullRes3Local.header.stamp = ros::Time().fromSec(timeLaserOdometry);
laserCloudFullRes3Local.header.frame_id = "/camera_init";
pubLaserCloudFullResLocal.publish(laserCloudFullRes3Local);
代码为:
sensor_msgs::PointCloud2 laserCloudMsg;
pcl::toROSMsg(laserCloudMap, laserCloudMsg);
laserCloudMsg.header.stamp = ros::Time().fromSec(timeLaserOdometry);
laserCloudMsg.header.frame_id = "/camera_init";
pubLaserCloudMap.publish(laserCloudMsg);
}
sensor_msgs::PointCloud2 laserCloudFullRes3Local;
pcl::toROSMsg(*laserCloudFullRes, laserCloudFullRes3Local);
laserCloudFullRes3Local.header.stamp = ros::Time().fromSec(timeLaserOdometry);
laserCloudFullRes3Local.header.frame_id = "/camera_init";
pubLaserCloudFullResLocal.publish(laserCloudFullRes3Local);
int laserCloudFullResNum = laserCloudFullRes->points.size();
for (int i = 0; i < laserCloudFullResNum; i++)
{
pointAssociateToMap(&laserCloudFullRes->points[i], &laserCloudFullRes->points[i]);
}
在main函数中别忘了继续定义它在大概980行左右:
插入
pubLaserCloudFullResLocal = nh.advertise<sensor_msgs::PointCloud2>("/velodyne_cloud_registered_local", 100);
最后变成:
pubLaserCloudMap = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_map", 100);
pubLaserCloudFullRes = nh.advertise<sensor_msgs::PointCloud2>("/velodyne_cloud_registered", 100);
pubLaserCloudFullResLocal = nh.advertise<sensor_msgs::PointCloud2>("/velodyne_cloud_registered_local", 100);
pubOdomAftMapped = nh.advertise<nav_msgs::Odometry>("/aft_mapped_to_init", 100);
并且把大部分的print输出语句注释掉了,这都是小case
对于其他的cpp文件修改的部分主要是把print语句注释。
修改laserOdometry.cpp文件
68行地方
原aloam为
int skipFrameNum = 2;
而scaloam中为
int skipFrameNum = 5;
其实影响不大
修改scanRegistration.cpp文件
在第60行的位置,添加:
std::string LIDAR_TYPE;
变成
using std::atan2;
using std::cos;
using std::sin;
std::string LIDAR_TYPE;
const double scanPeriod = 0.1;
170行左右的位置对雷达的类型做了不同的定义
if (LIDAR_TYPE == "VLP16" && N_SCANS == 16)
{
scanID = int((angle + 15) / 2 + 0.5);
if (scanID > (N_SCANS - 1) || scanID < 0)
{
count--;
continue;
}
}
else if (LIDAR_TYPE == "HDL32" && N_SCANS == 32)
{
scanID = int((angle + 92.0/3.0) * 3.0 / 4.0);
if (scanID > (N_SCANS - 1) || scanID < 0)
{
count--;
continue;
}
}
// HDL64 (e.g., KITTI)
else if (LIDAR_TYPE == "HDL64" && 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;
}
}
// Ouster OS1-64 (e.g., MulRan)
else if (LIDAR_TYPE == "OS1-64" && N_SCANS == 64)
{
scanID = int((angle + 22.5) / 2 + 0.5); // ouster os1-64 vfov is [-22.5, 22.5] see https://ouster.com/products/os1-lidar-sensor/
if (scanID > (N_SCANS - 1) || scanID < 0)
{
count--;
continue;
}
}
else
{
//printf("wrong scan number\n");
ROS_BREAK();
}
printf("angle %f scanID %d \n", angle, scanID);
但其实我运行不出来,需要修改成自己的激光雷达相关的参数
else if (N_SCANS == 32)
{
//std::cout<<"N_scnas:"<<N_SCANS<<std::endl;
scanID = int(((angle - 2.3125)/ 2.8125) + 0.5);
//std::cout<<"angle:"<<angle<<std::endl;
//std::cout<<"scanID:"<<scanID<<std::endl;
if (scanID > (N_SCANS - 1) || scanID < 0)
{
count--;
continue;
}
}
所以将32这部分进行修改
同时需要添加main中的内容
nh.param<std::string>("lidar_type", LIDAR_TYPE, "KITTI");
当然也可以完全不修改,我只需要使用32线束
同时490行左右main函数内
ros::Subscriber subLaserCloud = nh.subscribe<sensor_msgs::PointCloud2>("/velodyne_points", 100, laserCloudHandler);
需要根据自己的包内容进行修改,当然可以直接修改成包的话题名字
ros::Subscriber subLaserCloud = nh.subscribe<sensor_msgs::PointCloud2>("/horizontal_laser_3d", 100, laserCloudHandler);
或者在launch文件中进行修改
<remap from="/velodyne_points" to="horizontal_laser_3d"/>
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)