SC-A-LOAM在aloam基础上添加了回环优化的代码运行

知道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"/>