使用ZED Mini 相机运行ORB-SLAM2

     一直想使用双目相机跑ORB-SLAM2，奈何没有双目相机，拿两个单目相机又觉得有点low。刚好另一个项目用到了zed mini双目相机，项目昨天也交差了，终于有时间可以去试试zed mini跑ORB-SLAM2。

直接上步骤：

一、先用zed mini 读取双目的图像。

1.按照zed mini官网上说的下载SDK 配置什么的。遇到的问题有：ttf-mscorefonts-installer 安装的时候有个times32.exe下载不了，我想这就是几个字体，也就是在gui上有用，我不使用gui就没理继续往下安装了。

2.之后对于各种提示，一路yes过去，最后SDK被安装在了/usr/local/zed，我顺手编译了一下depth sensing，没有让我失望果然不能运行。凡是用到gui界面的应该都不能运行，可是，我又不用gui，我只要能同时读左右相机的图像就可以了。

3.编译一下interfaces里的opencv，RGB图和深度图都可以显示，我不用深度图。

4.自己照着CMakeLists.txt和main.cpp里的东西，写了一段读RGB的代码。

SET(execName ZED_with_OpenCV)
CMAKE_MINIMUM_REQUIRED(VERSION 2.4)
PROJECT(${execName})

option(LINK_SHARED_ZED "Link with the ZED SDK shared executable" ON)

if (NOT LINK_SHARED_ZED AND MSVC)
    message(FATAL_ERROR "LINK_SHARED_ZED OFF : ZED SDK static libraries not available on Windows")
endif()

if(COMMAND cmake_policy)
	cmake_policy(SET CMP0003 OLD)
	cmake_policy(SET CMP0015 OLD)
endif(COMMAND cmake_policy)

SET(EXECUTABLE_OUTPUT_PATH ".")

find_package(ZED 2 REQUIRED)
find_package(OpenCV REQUIRED)
find_package(CUDA ${ZED_CUDA_VERSION} EXACT REQUIRED)

IF(NOT WIN32)
    add_definitions(-Wno-format-extra-args)
    SET(SPECIAL_OS_LIBS "pthread" "X11")
ENDIF()

include_directories(${CUDA_INCLUDE_DIRS})
include_directories(${ZED_INCLUDE_DIRS})
include_directories(${OPENCV_INCLUDE_DIRS})
#include_directories(${CMAKE_CURRENT_SOURCE_DIR}/include)

link_directories(${ZED_LIBRARY_DIR})
link_directories(${OpenCV_LIBRARY_DIRS})
link_directories(${CUDA_LIBRARY_DIRS})

#FILE(GLOB_RECURSE SRC_FILES src/*.cpp)
#FILE(GLOB_RECURSE HDR_FILES include/*.hpp)

ADD_EXECUTABLE(${execName} ./src/main.cc)
add_definitions(-std=c++11 -g -O3)

if (LINK_SHARED_ZED)
    SET(ZED_LIBS ${ZED_LIBRARIES} ${CUDA_CUDA_LIBRARY} ${CUDA_CUDART_LIBRARY} ${CUDA_NPP_LIBRARIES_ZED})
else()
    SET(ZED_LIBS ${ZED_STATIC_LIBRARIES} ${CUDA_CUDA_LIBRARY} ${CUDA_LIBRARY})
endif()

TARGET_LINK_LIBRARIES(${execName} ${ZED_LIBS} ${OpenCV_LIBRARIES})

这个CMakeLists.txt就把和深度图相关的都注释掉了。

main.cpp

///
//
// Copyright (c) 2019, STEREOLABS.
//
// All rights reserved.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
///

/***********************************************************************************************
 ** This sample demonstrates how to use the ZED SDK with OpenCV. 					  	      **
 ** Depth and images are captured with the ZED SDK, converted to OpenCV format and displayed. **
 ***********************************************************************************************/

 // ZED includes
#include <sl_zed/Camera.hpp>

// OpenCV includes
#include <opencv2/opencv.hpp>

using namespace sl;

cv::Mat slMat2cvMat(Mat& input);
void printHelp();

int main(int argc, char **argv) {

    // Create a ZED camera object
    Camera zed;

    // Set configuration parameters
    InitParameters init_params;
    init_params.camera_resolution = RESOLUTION_HD720;
    if (argc > 1) init_params.svo_input_filename.set(argv[1]);
        
    // Open the camera
    ERROR_CODE err = zed.open(init_params);
    if (err != SUCCESS) {
        printf("%s\n", toString(err).c_str());
        zed.close();
        return 1; // Quit if an error occurred
    }

    // Display help in console
    printHelp();

//     Set runtime parameters after opening the camera
    RuntimeParameters runtime_parameters;
    runtime_parameters.sensing_mode = SENSING_MODE_STANDARD;

    // Prepare new image size to retrieve half-resolution images
    Resolution image_size = zed.getResolution();
    int new_width = image_size.width ;
    int new_height = image_size.height;

    std::cout<<image_size.width<<" "<<image_size.height<<std::endl;
    // To share data between sl::Mat and cv::Mat, use slMat2cvMat()
    // Only the headers and pointer to the sl::Mat are copied, not the data itself
    Mat image_zed_left(new_width, new_height, MAT_TYPE_8U_C4);
    Mat image_zed_right(new_width, new_height, MAT_TYPE_8U_C4);

    // Loop until 'q' is pressed
    char key = ' ';
    while (key != 'q') {

        if (zed.grab(runtime_parameters) == SUCCESS) {

            // Retrieve the left image, depth image in half-resolution
            zed.retrieveImage(image_zed_left, VIEW_LEFT, MEM_CPU, new_width, new_height);
            cv::Mat image_ocv_left = slMat2cvMat(image_zed_left);

             zed.retrieveImage(image_zed_right, VIEW_RIGHT, MEM_CPU, new_width, new_height);
             cv::Mat image_ocv_right = slMat2cvMat(image_zed_right);
            // Display image and depth using cv:Mat which share sl:Mat data
            cv::imshow("ImageLeft", image_ocv_left);
            cv::imshow("ImageRight", image_ocv_right);
            // Handle key event
            key = cv::waitKey(10);
        }
    }
    zed.close();
    return 0;
}

/**
* Conversion function between sl::Mat and cv::Mat
**/
cv::Mat slMat2cvMat(Mat& input) {
    // Mapping between MAT_TYPE and CV_TYPE
    int cv_type = -1;
    switch (input.getDataType()) {
        case MAT_TYPE_32F_C1: cv_type = CV_32FC1; break;
        case MAT_TYPE_32F_C2: cv_type = CV_32FC2; break;
        case MAT_TYPE_32F_C3: cv_type = CV_32FC3; break;
        case MAT_TYPE_32F_C4: cv_type = CV_32FC4; break;
        case MAT_TYPE_8U_C1: cv_type = CV_8UC1; break;
        case MAT_TYPE_8U_C2: cv_type = CV_8UC2; break;
        case MAT_TYPE_8U_C3: cv_type = CV_8UC3; break;
        case MAT_TYPE_8U_C4: cv_type = CV_8UC4; break;
        default: break;
    }

    // Since cv::Mat data requires a uchar* pointer, we get the uchar1 pointer from sl::Mat (getPtr<T>())
    // cv::Mat and sl::Mat will share a single memory structure
    return cv::Mat(input.getHeight(), input.getWidth(), cv_type, input.getPtr<sl::uchar1>(MEM_CPU));
}

/**
* This function displays help in console
**/
void printHelp() {
    std::cout << " Press 's' to save Side by side images" << std::endl;
    std::cout << " Press 'p' to save Point Cloud" << std::endl;
    std::cout << " Press 'd' to save Depth image" << std::endl;
    std::cout << " Press 'm' to switch Point Cloud format" << std::endl;
    std::cout << " Press 'n' to switch Depth format" << std::endl;
}

这个代码很简单一看就都明白。

这样子Zed mini读图就没撒子问题了。

但是，不知道为啥子，重启了一下电脑驱动就坏了，应该是装什么GL之类的东西搞坏了，又装了一下驱动。

二、加到ORB-SLAM2里

1.新建了一个myslam_stereo.cc在Examples/Stereo/这个地方。

myslam_stereo.cc里这么写代码:

// 需要opencv
#include <opencv2/opencv.hpp>

// ORB-SLAM的系统接口
#include "System.h"

#include <string>
#include <chrono>   // for time stamp
#include <iostream>

#include<sl_zed/Camera.hpp>

using namespace std;
using namespace sl;

cv::Mat slMat2cvMat(Mat& input);

// 参数文件与字典文件
// 如果你系统上的路径不同，请修改它
string parameterFile = "/home/meng/ORB_SLAM2-master/Examples/Stereo/Zed_stereo.yaml";
string vocFile = "/home/meng/ORB_SLAM2-master/Vocabulary/ORBvoc.txt";

int main(int argc, char **argv) {
    cout<<"hello world";
    // 声明 ORB-SLAM2 系统
    ORB_SLAM2::System SLAM(vocFile, parameterFile, ORB_SLAM2::System::STEREO, true);

    Camera zed;

    // 记录系统时间
    auto start = chrono::system_clock::now();
    InitParameters init_params;
    init_params.camera_resolution = RESOLUTION_HD720;
    // Open the camera
    ERROR_CODE err = zed.open(init_params);
    if (err != SUCCESS) {
        printf("%s\n", toString(err).c_str());
        zed.close();
        return 1; // Quit if an error occurred
    }

    //     Set runtime parameters after opening the camera
        RuntimeParameters runtime_parameters;
        runtime_parameters.sensing_mode = SENSING_MODE_STANDARD;

        // Prepare new image size to retrieve half-resolution images
        Resolution image_size = zed.getResolution();
        int new_width = image_size.width ;
        int new_height = image_size.height;

        // To share data between sl::Mat and cv::Mat, use slMat2cvMat()
        // Only the headers and pointer to the sl::Mat are copied, not the data itself
        Mat image_zed_left(new_width, new_height, MAT_TYPE_8U_C4);
        Mat image_zed_right(new_width, new_height, MAT_TYPE_8U_C4);
    while (1) {
//        cv::Mat frame;
//        cap >> frame;   // 读取相机数据
        if(zed.grab(runtime_parameters) == SUCCESS){
              zed.retrieveImage(image_zed_left, VIEW_LEFT, MEM_CPU, new_width, new_height);
               zed.retrieveImage(image_zed_right, VIEW_RIGHT, MEM_CPU, new_width, new_height);
        }
         cv::Mat image_ocv_left = slMat2cvMat(image_zed_left);
         cv::Mat image_ocv_right = slMat2cvMat(image_zed_right);

        auto now = chrono::system_clock::now();
        auto timestamp = chrono::duration_cast<chrono::milliseconds>(now - start);
        SLAM.TrackStereo(image_ocv_left,image_ocv_right, double(timestamp.count())/1000.0);
    }

    return 0;
}

/**
* Conversion function between sl::Mat and cv::Mat
**/
cv::Mat slMat2cvMat(Mat& input) {
    // Mapping between MAT_TYPE and CV_TYPE
    int cv_type = -1;
    switch (input.getDataType()) {
        case MAT_TYPE_32F_C1: cv_type = CV_32FC1; break;
        case MAT_TYPE_32F_C2: cv_type = CV_32FC2; break;
        case MAT_TYPE_32F_C3: cv_type = CV_32FC3; break;
        case MAT_TYPE_32F_C4: cv_type = CV_32FC4; break;
        case MAT_TYPE_8U_C1: cv_type = CV_8UC1; break;
        case MAT_TYPE_8U_C2: cv_type = CV_8UC2; break;
        case MAT_TYPE_8U_C3: cv_type = CV_8UC3; break;
        case MAT_TYPE_8U_C4: cv_type = CV_8UC4; break;
        default: break;
    }

    // Since cv::Mat data requires a uchar* pointer, we get the uchar1 pointer from sl::Mat (getPtr<T>())
    // cv::Mat and sl::Mat will share a single memory structure
    return cv::Mat(input.getHeight(), input.getWidth(), cv_type, input.getPtr<sl::uchar1>(MEM_CPU));
}

代码不解释了，不难。

2.编写CMakeLists.txt 对着之前的写就可以了，路径啥的注意点，链接opencv库该注释的注释掉，可执行文件名字给上。

cmake_minimum_required(VERSION 2.8)
project(ORB_SLAM2)

IF(NOT CMAKE_BUILD_TYPE)
  SET(CMAKE_BUILD_TYPE Release)
ENDIF()

MESSAGE("Build type: " ${CMAKE_BUILD_TYPE})

set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS}  -Wall  -O3")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall   -O3")

# Check C++11 or C++0x support
include(CheckCXXCompilerFlag)
CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11)
CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X)
if(COMPILER_SUPPORTS_CXX11)
   set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
   add_definitions(-DCOMPILEDWITHC11)
   message(STATUS "Using flag -std=c++11.")
elseif(COMPILER_SUPPORTS_CXX0X)
   set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
   add_definitions(-DCOMPILEDWITHC0X)
   message(STATUS "Using flag -std=c++0x.")
else()
   message(FATAL_ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.")
endif()

LIST(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules)

find_package(OpenCV 3.0 QUIET)
if(NOT OpenCV_FOUND)
   find_package(OpenCV 2.4.3 QUIET)
   if(NOT OpenCV_FOUND)
      message(FATAL_ERROR "OpenCV > 2.4.3 not found.")
   endif()
endif()

find_package(Eigen3 3.1.0 REQUIRED)
find_package(Pangolin REQUIRED)

include_directories(
${PROJECT_SOURCE_DIR}
${PROJECT_SOURCE_DIR}/include
${EIGEN3_INCLUDE_DIR}
${Pangolin_INCLUDE_DIRS}
)

set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/lib)

add_library(${PROJECT_NAME} SHARED
src/System.cc
src/Tracking.cc
src/LocalMapping.cc
src/LoopClosing.cc
src/ORBextractor.cc
src/ORBmatcher.cc
src/FrameDrawer.cc
src/Converter.cc
src/MapPoint.cc
src/KeyFrame.cc
src/Map.cc
src/MapDrawer.cc
src/Optimizer.cc
src/PnPsolver.cc
src/Frame.cc
src/KeyFrameDatabase.cc
src/Sim3Solver.cc
src/Initializer.cc
src/Viewer.cc
)

target_link_libraries(${PROJECT_NAME}
${OpenCV_LIBS}
${EIGEN3_LIBS}
${Pangolin_LIBRARIES}
${PROJECT_SOURCE_DIR}/Thirdparty/DBoW2/lib/libDBoW2.so
${PROJECT_SOURCE_DIR}/Thirdparty/g2o/lib/libg2o.so
)


#------------------------------------------------------------------------------------------------
#Add zed camera
option(LINK_SHARED_ZED "Link with the ZED SDK shared executable" ON)

if (NOT LINK_SHARED_ZED AND MSVC)
    message(FATAL_ERROR "LINK_SHARED_ZED OFF : ZED SDK static libraries not available on Windows")
endif()

if(COMMAND cmake_policy)
	cmake_policy(SET CMP0003 OLD)
	cmake_policy(SET CMP0015 OLD)
endif(COMMAND cmake_policy)

#SET(EXECUTABLE_OUTPUT_PATH ".")

find_package(ZED 2 REQUIRED)
#find_package(OpenCV REQUIRED)
find_package(CUDA ${ZED_CUDA_VERSION} EXACT REQUIRED)

IF(NOT WIN32)
    add_definitions(-Wno-format-extra-args)
    SET(SPECIAL_OS_LIBS "pthread" "X11")
ENDIF()

include_directories(${CUDA_INCLUDE_DIRS})
include_directories(${ZED_INCLUDE_DIRS})
#include_directories(${OPENCV_INCLUDE_DIRS})
#include_directories(${CMAKE_CURRENT_SOURCE_DIR}/include)

link_directories(${ZED_LIBRARY_DIR})
#link_directories(${OpenCV_LIBRARY_DIRS})
link_directories(${CUDA_LIBRARY_DIRS})

#FILE(GLOB_RECURSE SRC_FILES src/*.cpp)
#FILE(GLOB_RECURSE HDR_FILES include/*.hpp)

#ADD_EXECUTABLE(${execName} ./src/main.cc)
#add_definitions(-std=c++11 -g -O3)

if (LINK_SHARED_ZED)
    SET(ZED_LIBS ${ZED_LIBRARIES} ${CUDA_CUDA_LIBRARY} ${CUDA_CUDART_LIBRARY} ${CUDA_NPP_LIBRARIES_ZED})
else()
    SET(ZED_LIBS ${ZED_STATIC_LIBRARIES} ${CUDA_CUDA_LIBRARY} ${CUDA_LIBRARY})
endif()

#TARGET_LINK_LIBRARIES(${execName} ${ZED_LIBS} ${OpenCV_LIBRARIES})

#---------------------------------------------------------------------------------------

# Build examples

set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples/RGB-D)

add_executable(rgbd_tum
Examples/RGB-D/rgbd_tum.cc)
target_link_libraries(rgbd_tum ${PROJECT_NAME})

set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples/Stereo)

add_executable(stereo_kitti
Examples/Stereo/stereo_kitti.cc)
target_link_libraries(stereo_kitti ${PROJECT_NAME})

add_executable(stereo_euroc
Examples/Stereo/stereo_euroc.cc)
target_link_libraries(stereo_euroc ${PROJECT_NAME})

add_executable(myslam_stereo
Examples/Stereo/myslam_stereo.cc)
target_link_libraries(myslam_stereo ${PROJECT_NAME} ${ZED_LIBS})


set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples/Monocular)

add_executable(mono_tum
Examples/Monocular/mono_tum.cc)
target_link_libraries(mono_tum ${PROJECT_NAME})

add_executable(mono_kitti
Examples/Monocular/mono_kitti.cc)
target_link_libraries(mono_kitti ${PROJECT_NAME})

add_executable(mono_euroc
Examples/Monocular/mono_euroc.cc)
target_link_libraries(mono_euroc ${PROJECT_NAME})

add_executable(myslam
Examples/Monocular/myslam.cc)
target_link_libraries(myslam ${PROJECT_NAME} ${ZED_LIBS})

注意，我之前用单目摄像头跑过，单目的在CMakeLists.txt最后几行，可以删掉。

三、写好yaml文件，复制Stereo里的KITT03的yaml文件，起个帅气的名字myslam_zed.yaml.

从usr/local/zed/setting找到你的相机的配置文件，SNXXXXXX.conf，简单易懂，填到myslam_zed.yaml里，HD是720P分辨率下的内参，对于Camera.bf参数 找到SNXXXXXX.conf里的Baseline参数  Camera.bf=fx*baseline/1000,为啥除以1000，因为这个conf里用的是毫米，yaml里用的是米。

想一下怎么可能如此的顺利，跑代码怎么会如此的顺利。运行不了几下子就报错崩溃，什么framebuffer with request attributes blabla...什么的错误，复制粘贴到百度，查了查应该还是GL的问题，看了对着一个解决方案试了一下:

删除掉ORBSLAM的cmakelists中的　-march=native　以及 g2o 的cmakelists中的-march=native
重新执行ORBSLAM目录下的./build.sh 后就可以正常跑了

转自：https://blog.csdn.net/weixin_42656998/article/details/99855639

试了一下，虽然还是会报错，可是程序不崩溃了，不管了，继续走。

可以完美运行了，双目比单目效果好多了，纵享丝滑。

完美，告辞。