将ORB-SLAM3用图像增强的方式打开

ORB-SLAM3在复杂光照环境下，特征提取和特征匹配的性能明显下降，其准确性和鲁棒性收到很大影响，尤其是当周围环境获取的ORB特征点数量不足时，位姿的估计过程无法进行，甚至或导致初始化和跟踪失败的情况，目前工程所需在复杂光照下运行SLAM，ORB-SLAM3是工程性非常好的SLAM算法，所以打算将图像增强用于ORB-SLAM3的特征提取，增强光照环境下的鲁棒性。

一、图像增强

1、拉普拉斯算子

拉普拉斯算子，也称为拉普拉斯滤波器或拉普拉斯掩模，是一种用于图像处理的卷积核。它在图像中执行二阶导数操作，有助于检测图像中的变化率，特别是边缘。通过将拉普拉斯算子应用于图像，我们可以增强图像中的边缘，使它们更加突出。

2、自定义拉普拉斯算子

（1）加载图像

（2）定义自定义拉普拉斯卷积核

cv::Mat kernel = (Mat_<float>(3, 3) <<
		1, 1, 1,
		1, -8, 1,
		1, 1, 1);

（3）应用自定义拉普拉斯卷积核

将使用 filter2D 函数将自定义拉普拉斯卷积核应用于图像：

Mat imglap;
filter2D(image, imglap, -1, kernel);

（4）显示处理结果

Mat result;
result = image - imglap;

$$g(x,y)=f(x,y)+c[{\nabla }^{2}f(x,y)]$$

f(x,y)和g(x,y)分别是输入图像和锐化后的图像。 原图像 + c * (拉普拉斯算子卷积后的图像) = 锐化后的图像。

这里的卷积核如果采用上公式a和b，则参数c=-1；如果使用的是后面两个公式，则c=1

二、修改ORB-SLAM3的跟踪线程

1、对图像进行了灰度化

这里使用了 OpenCV 的 cvtColor 函数，将彩色图像img转换为灰度图像 gray_img。COLOR_BGR2GRAY 参数指定了转换的方式，即从 BGR 彩色空间转换到灰度空间。

Mat gray_img;
cvtColor(img, gray_img, COLOR_BGR2GRAY);

2、拉普拉斯算子锐化

使用了 OpenCV 的 Laplacian 函数来应用拉普拉斯算子。该函数接受灰度图像 gray_img 作为输入，然后将结果存储在 output_img 中。CV_8U 参数指定了输出图像的数据类型，即8位无符号整数

// Apply Laplacian operator
Mat output_img;
Laplacian(gray_img, output_img, CV_8U);

将拉普拉斯算子的处理过程放在了构造函数中。

在构造函数中，首先对图像进行了灰度化，然后应用了拉普拉斯算子，并将处理后的图像传递给了 FrameDrawer。接下来，跟踪线程的主循环 Run 函数需要添加具体的跟踪逻辑

#include "Tracking.h"
#include "ORBmatcher.h"
#include "FrameDrawer.h"
#include "Converter.h"
#include "G2oTypes.h"
#include "Optimizer.h"
#include "Pinhole.h"
#include "KannalaBrandt8.h"
#include "MLPnPsolver.h"
#include "GeometricTools.h"

#include <iostream>
#include <mutex>
#include <chrono>

using namespace std;

namespace ORB_SLAM3
{

Tracking::Tracking(System *pSys, ORBVocabulary* pVoc, FrameDrawer *pFrameDrawer, MapDrawer *pMapDrawer, Atlas *pAtlas, KeyFrameDatabase* pKFDB, const string &strSettingPath, const int sensor, Settings* settings, const string &_nameSeq):
    mState(NO_IMAGES_YET), mSensor(sensor), mTrackedFr(0), mbStep(false),
    mbOnlyTracking(false), mbMapUpdated(false), mbVO(false), mpORBVocabulary(pVoc), mpKeyFrameDB(pKFDB),
    mbReadyToInitializate(false), mpSystem(pSys), mpViewer(NULL), bStepByStep(false),
    mpFrameDrawer(pFrameDrawer), mpMapDrawer(pMapDrawer), mpAtlas(pAtlas), mnLastRelocFrameId(0), time_recently_lost(5.0),
    mnInitialFrameId(0), mbCreatedMap(false), mnFirstFrameId(0), mpCamera2(nullptr), mpLastKeyFrame(static_cast<KeyFrame*>(NULL))
{
    // Load camera parameters from settings file
    if(settings){
        newParameterLoader(settings);
    }
    else{
        cv::FileStorage fSettings(strSettingPath, cv::FileStorage::READ);

        bool b_parse_cam = ParseCamParamFile(fSettings);
        if(!b_parse_cam)
        {
            std::cout << "*Error with the camera parameters in the config file*" << std::endl;
        }

        // Load ORB parameters
        bool b_parse_orb = ParseORBParamFile(fSettings);
        if(!b_parse_orb)
        {
            std::cout << "*Error with the ORB parameters in the config file*" << std::endl;
        }

        bool b_parse_imu = true;
        if(sensor==System::IMU_MONOCULAR || sensor==System::IMU_STEREO || sensor==System::IMU_RGBD)
        {
            b_parse_imu = ParseIMUParamFile(fSettings);
            if(!b_parse_imu)
            {
                std::cout << "*Error with the IMU parameters in the config file*" << std::endl;
            }

            mnFramesToResetIMU = mMaxFrames;
        }

        if(!b_parse_cam || !b_parse_orb || !b_parse_imu)
        {
            std::cerr << "**ERROR in the config file, the format is not correct**" << std::endl;
            try
            {
                throw -1;
            }
            catch(exception &e)
            {

            }
        }
    }

    initID = 0; lastID = 0;
    mbInitWith3KFs = false;
    mnNumDataset = 0;

    vector<GeometricCamera*> vpCams = mpAtlas->GetAllCameras();
    std::cout << "There are " << vpCams.size() << " cameras in the atlas" << std::endl;
    for(GeometricCamera* pCam : vpCams)
    {
        std::cout << "Camera " << pCam->GetId();
        if(pCam->GetType() == GeometricCamera::CAM_PINHOLE)
        {
            std::cout << " is pinhole" << std::endl;
        }
        else if(pCam->GetType() == GeometricCamera::CAM_FISHEYE)
        {
            std::cout << " is fisheye" << std::endl;
        }
        else
        {
            std::cout << " is unknown" << std::endl;
        }
    }

#ifdef REGISTER_TIMES
    vdRectStereo_ms.clear();
    vdResizeImage_ms.clear();
    vdORBExtract_ms.clear();
    vdStereoMatch_ms.clear();
    vdIMUInteg_ms.clear();
    vdPosePred_ms.clear();
    vdLMTrack_ms.clear();
    vdNewKF_ms.clear();
    vdTrackTotal_ms.clear();
#endif

    // Perform Laplacian operator on initial frame
    string img_path = "S:/datasets/coco2017/train/images/000000000009.jpg"; // Replace with your image path
    Mat img = imread(img_path);

    Mat gray_img;
    cvtColor(img, gray_img, COLOR_BGR2GRAY);

    // Apply Laplacian operator
    Mat output_img;
    Laplacian(gray_img, output_img, CV_8U);

    // Pass the processed image to the FrameDrawer
    mpFrameDrawer->UpdateImage(output_img);

    // Set the initial state to NO_IMAGES_YET
    mState = NO_IMAGES_YET;
}

void Tracking::Run()
{
    while (1) {
        // Here goes your tracking loop
    }
}

#ifdef REGISTER_TIMES
// Define your time statistics functions here
#endif

}

3、追踪效果演示

在ROS中运行改进后的ORB-SLAM3可以看出，在大太阳的操场上依旧有着不错的追踪效果和特征点数量

三、总结

在本次实验中更加充分的了解了ORB-SLAM3算法的各个模块以及实验原理，熟悉了拉普拉斯锐化图像。ORB-SLAM 3设置了很多固定值，这次把使用手动设置FAST角点的阈值 Θ，设置为图像增强计算后的值。虽然增加了对光照的鲁棒性，但是相对的，特征提取阶段的时间花销会更多。

如果想改进已有的算法以获得在数据集上的视觉里程计定位精度上的提升，比较困难。

如果是在实际场景中发现已有的框架（例如ORB-SLAM3)的定位精度不能达到论文中，或者预想的精度，那么这个事情是可以根据实际场景讨论的。