基于reslsense d435相机的ORBSLAM2/3非ros下实时建图

orbslam2/3非ros下用d435数据建图

因需求需要在ubuntu系统下不使用ros进行orbslam2的使用，官方源码在非ros下只能使用数据集输入。

源码改动

只改动了/home/xxx/orbslam_ws/src/ORB_SLAM2/CMakeLists.txt和/home/dong/orbslam_ws/src/ORB_SLAM2/Examples/RGB-D/rgbd_tum.cc

CMakeLists.txt

在其中find_package(XXXXXXX REQUIRED)下加入以下代码，其中相关兼容库请自行下载，相关路径也自行更改

FIND_PACKAGE(realsense2 REQUIRED)
find_package(GLUT REQUIRED)
include_directories(${GLUT_INCLUDE_DIRS})
link_directories(${GLUT_LIBRARY_DIRS})
add_definitions(${GLUT_DEFINITIONS})
if(NOT GLUT_FOUND)
    message(ERROR " GLUT not found!")
endif(NOT GLUT_FOUND)
#########################################################
# FIND OPENGL
#########################################################
find_package(OpenGL REQUIRED)
find_package(glfw3 REQUIRED)
find_package(PCL REQUIRED)
find_package(Pangolin REQUIRED)
include_directories(${OpenGL_INCLUDE_DIRS})
link_directories(${OpenGL_LIBRARY_DIRS})
add_definitions(${OpenGL_DEFINITIONS})
if(NOT OPENGL_FOUND)
    message(ERROR " OPENGL not found!")
endif(NOT OPENGL_FOUND)

include_directories(/home/xxx/librealsense-2.36.0/include)
set(DEPENDENCIES realsense2 )

include_directories(/home/xxx/realsense/include)

在target_link_libraries(rgbd_tum ${PROJECT_NAME} ${OPENGL_LIBRARIES} ${realsense2_LIBRARY})加入 gl和rs2库

rgbd_tum.cc

源码整个替换

#include<iostream>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/opencv.hpp>
#include "opencv2/imgproc/imgproc.hpp"
#include<librealsense2/rsutil.h>
#include <librealsense2/rs.hpp>
//#include "/home/dong/librealsense-2.36.0/examples/example.hpp"         
// include OpenCV header file
// Helper functions
 #include <unistd.h>
#include<opencv2/core/core.hpp>
#include <opencv2/core/mat.hpp>
#include <sys/time.h>
#include<algorithm>
#include<fstream>
#include<chrono>
#include<System.h>
#define Width 640 
#define Height 480 
#define fps 30
using namespace std;
using namespace cv;
using namespace rs2;

void LoadImages(const string &strAssociationFilename, vector<string> &vstrImageFilenamesRGB,
                vector<string> &vstrImageFilenamesD, vector<double> &vTimestamps);
float get_depth_scale(device dev)
{
    for (sensor& sensor : dev.query_sensors()) //检查设备的传感器
    {
        if (depth_sensor dpt = sensor.as<depth_sensor>()) //检查传感器是否为深度传感器
        {
            return dpt.get_depth_scale();
        }
    }
    throw runtime_error("Device does not have a depth sensor");
}

 
//检查摄像头数据管道设置是否改变
bool profile_changed(const std::vector<rs2::stream_profile>& current, const std::vector<rs2::stream_profile>& prev)
{
    for (auto&& sp : prev)
    {
        //If previous profile is in current (maybe just added another)
        auto itr = std::find_if(std::begin(current), std::end(current), [&sp](const rs2::stream_profile& current_sp) { return sp.unique_id() == current_sp.unique_id(); });
        if (itr == std::end(current)) //If it previous stream wasn't found in current
        {
            return true;
        }
    }
    return false;
}

int main(int argc, char **argv)
{
    /*if(argc != 5)
    {
        cerr << endl << "Usage: ./rgbd_tum path_to_vocabulary path_to_settings path_to_sequence path_to_association" << endl;
        return 1;
    }*/

    // Retrieve paths to images
    vector<string> vstrImageFilenamesRGB;
    vector<string> vstrImageFilenamesD;
    vector<double> vTimestamps;
    //string strAssociationFilename = string(argv[4]);
    //LoadImages(strAssociationFilename, vstrImageFilenamesRGB, vstrImageFilenamesD, vTimestamps);

    // Check consistency in the number of images and depthmaps
    int nImages = vstrImageFilenamesRGB.size();
    /*if(vstrImageFilenamesRGB.empty())
    {
        cerr << endl << "No images found in provided path." << endl;
        return 1;
    }
    else if(vstrImageFilenamesD.size()!=vstrImageFilenamesRGB.size())
    {
        cerr << endl << "Different number of images for rgb and depth." << endl;
        return 1;
    }
*/
    // Create SLAM system. It initializes all system threads and gets ready to process frames.
    ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::RGBD,true);

    // Vector for tracking time statistics
    vector<float> vTimesTrack;
    vTimesTrack.resize(nImages);

    cout << endl << "-------" << endl;
    cout << "Start processing sequence ..." << endl;
    cout << "Images in the sequence: " << nImages << endl << endl;

    // Main loop


    struct timeval tv;
    cv::Mat dep;
    rs2::context ctx;
    auto list = ctx.query_devices(); // Get a snapshot of currently connected devices
    if (list.size() == 0) 
	throw std::runtime_error("no camera");
    rs2::device dev = list.front();
      
    rs2::frameset frames;
    //Contruct a pipeline which abstracts the device
    
    //深度图像颜色map
    rs2::colorizer c;                          // Helper to colorize depth images
    //helper用于渲染图片
    //texture renderer;                     // Helper for renderig images
 
    // Create a pipeline to easily configure and start the camera
    //创建数据管道
    rs2::pipeline pipe;
    rs2::config pipe_config;
    pipe_config.enable_stream(RS2_STREAM_DEPTH,640,480,RS2_FORMAT_Z16,30);
    pipe_config.enable_stream(RS2_STREAM_COLOR,640,480,RS2_FORMAT_BGR8,30);
    //Calling pipeline's start() without any additional parameters will start the first device
    //直接start()，不添加配置参数，则默认打开第一个设备
    // with its default streams.
    //以及以默认的配置进行流输出
    //The start function returns the pipeline profile which the pipeline used to start the device
    //start()函数返回数据管道的profile
    rs2::pipeline_profile profile = pipe.start(pipe_config);

    // Each depth camera might have different units for depth pixels, so we get it here
    //每个深度摄像头有不同单元的像素，我们这里获取
    // Using the pipeline's profile, we can retrieve the device that the pipeline uses
    //使用数据管道的profile获取深度图像像素对应于长度单位（米）的转换比例
    float depth_scale = get_depth_scale(profile.get_device());
 
    //Pipeline could choose a device that does not have a color stream
    //数据管道可以选择一个没有彩色图像数据流的设备
    //If there is no color stream, choose to align depth to another stream
    //选择彩色图像数据流来作为对齐对象
    rs2_stream align_to = RS2_STREAM_COLOR;//find_stream_to_align(profile.get_stream());
 
    /*
     @这里的对齐是改变深度图，而不改变color图
    */
    // Create a rs2::align object.
    //创建一个rs2::align的对象
    // rs2::align allows us to perform alignment of depth frames to others frames
    //rs2::align 允许我们去实现深度图像对齐其他图像
    //The "align_to" is the stream type to which we plan to align depth frames.
    // "align_to"是我们打算用深度图像对齐的图像流
    rs2::align align(align_to);
 
    // Define a variable for controlling the distance to clip
    //定义一个变量去转换深度到距离

    float depth_clipping_distance = 1.f;
    int i=0;
    while (true)
    {	
	i++;

        rs2::frameset frameset = pipe.wait_for_frames();
 
        // rs2::pipeline::wait_for_frames() can replace the device it uses in case of device error or disconnection.
        // Since rs2::align is aligning depth to some other stream, we need to make sure that the stream was not changed
        //因为rs2::align 正在对齐深度图像到其他图像流，我们要确保对齐的图像流不发生改变
        //  after the call to wait_for_frames();
        if (profile_changed(pipe.get_active_profile().get_streams(), profile.get_streams()))
        {
            //If the profile was changed, update the align object, and also get the new device's depth scale
            //如果profile发生改变，则更新align对象，重新获取深度图像像素到长度单位的转换比例
            profile = pipe.get_active_profile();
            align = rs2::align(align_to);
            depth_scale = get_depth_scale(profile.get_device());
        }
 
        //Get processed aligned frame
        //获取对齐后的帧
        auto processed = align.process(frameset);
 
        // Trying to get both other and aligned depth frames
        //尝试获取对齐后的深度图像帧和其他帧
        rs2::frame aligned_color_frame = processed.get_color_frame();//processed.first(align_to);
        rs2::frame aligned_depth_frame = processed.get_depth_frame();
 
        //获取对齐之前的color图像
        rs2::frame before_depth_frame=frameset.get_depth_frame();
        //获取宽高
        const int depth_w=aligned_depth_frame.as<rs2::video_frame>().get_width();
        const int depth_h=aligned_depth_frame.as<rs2::video_frame>().get_height();
        const int color_w=aligned_color_frame.as<rs2::video_frame>().get_width();
        const int color_h=aligned_color_frame.as<rs2::video_frame>().get_height();
        const int b_color_w=before_depth_frame.as<rs2::video_frame>().get_width();
        const int b_color_h=before_depth_frame.as<rs2::video_frame>().get_height();

        //If one of them is unavailable, continue iteration
        if (!aligned_depth_frame || !aligned_color_frame)
        {
            continue;
        }
        //创建OPENCV类型 并传入数据
        Mat imD(Size(depth_w,depth_h),CV_16U,(void*)aligned_depth_frame.get_data(),Mat::AUTO_STEP);
        Mat imRGB(Size(color_w,color_h),CV_8UC3,(void*)aligned_color_frame.get_data(),Mat::AUTO_STEP);
        Mat before_color_image(Size(b_color_w,b_color_h),CV_16U,(void*)before_depth_frame.get_data(),Mat::AUTO_STEP);
        gettimeofday(&tv, NULL);
        double tframe = tv.tv_sec%100000+tv.tv_usec*0.000001;


   /* for(int ni=0; ni<nImages; ni++)
    {
        // Read image and depthmap from file
        imRGB = cv::imread(string(argv[3])+"/"+vstrImageFilenamesRGB[ni],CV_LOAD_IMAGE_UNCHANGED);
        imD = cv::imread(string(argv[3])+"/"+vstrImageFilenamesD[ni],CV_LOAD_IMAGE_UNCHANGED);
        double tframe = vTimestamps[ni];

        if(imRGB.empty())
        {
            cerr << endl << "Failed to load image at: "
                 << string(argv[3]) << "/" << vstrImageFilenamesRGB[ni] << endl;
            return 1;
        }*/

#ifdef COMPILEDWITHC11
        std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
#else
        std::chrono::monotonic_clock::time_point t1 = std::chrono::monotonic_clock::now();
#endif

        // Pass the image to the SLAM system
        SLAM.TrackRGBD(imRGB,imD,tframe);

#ifdef COMPILEDWITHC11
        std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();
#else
        std::chrono::monotonic_clock::time_point t2 = std::chrono::monotonic_clock::now();
#endif

    /*    double ttrack= std::chrono::duration_cast<std::chrono::duration<double> >(t2 - t1).count();

        vTimesTrack[ni]=ttrack;

        // Wait to load the next frame
        double T=0;
        if(ni<nImages-1)
            T = vTimestamps[ni+1]-tframe;
        else if(ni>0)
            T = tframe-vTimestamps[ni-1];

        if(ttrack<T)
            usleep((T-ttrack)*1e6);
    }*/
    }
    // Stop all threads
    SLAM.Shutdown();

    // Tracking time statistics
    /*sort(vTimesTrack.begin(),vTimesTrack.end());
    float totaltime = 0;
    for(int ni=0; ni<nImages; ni++)
    {
        totaltime+=vTimesTrack[ni];
    }
    cout << "-------" << endl << endl;
    cout << "median tracking time: " << vTimesTrack[nImages/2] << endl;
    cout << "mean tracking time: " << totaltime/nImages << endl;
*/
    // Save camera trajectory
    SLAM.SaveTrajectoryTUM("CameraTrajectory.txt");
    SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt");   

    return 0;
}

void LoadImages(const string &strAssociationFilename, vector<string> &vstrImageFilenamesRGB,
                vector<string> &vstrImageFilenamesD, vector<double> &vTimestamps)
{
    ifstream fAssociation;
    fAssociation.open(strAssociationFilename.c_str());
    while(!fAssociation.eof())
    {
        string s;
        getline(fAssociation,s);
        if(!s.empty())
        {
            stringstream ss;
            ss << s;
            double t;
            string sRGB, sD;
            ss >> t;
            vTimestamps.push_back(t);
            ss >> sRGB;
            vstrImageFilenamesRGB.push_back(sRGB);
            ss >> t;
            ss >> sD;
            vstrImageFilenamesD.push_back(sD);

        }
    }
}

运行

./Examples/RGB-D/rgbd_tum ./Vocabulary/ORBvoc.txt ./Examples/RGB-D/TUM1.yaml