【VINS-Mono】RealsenseD435i运行VINS-Mono，在ubuntu18.04和opencv3和cv_bridge的报错记录

VINS-Mono（A Robust and Versatile Monocular Visual-Inertial State Estimator）
https://github.com/HKUST-Aerial-Robotics/VINS-Mono
VINS-Mono的官方配置环境Ubuntu 16.04, ROS Kinetic, OpenCV 3.3.1, Eigen 3.3.3,cereo1.14.0
我的环境Ubuntu 18.04, ROS Melodic, OpenCV 4.5.1&&3.3.1, Eigen 3.3.3,cereo1.14.0
踩坑总结：因为原来电脑就安装了opencv4.5.1，所以打算改vins-mono的opencv代码，但是ubuntu18.04的cv_bridge是和opencv3.2.0连接，导致调用opencv版本冲突，后来重新安装opencv4版本的cv_bridge，编译通过；但是在打开realsense会报错，因为realsense也用到了cv_bridge的但是用的opencv3.2.0，而改完后的vins-mono用的是opencv4.5.1，所以还是会有版本冲突。最终的解决方案是又安装了opencv3.3.1，不卸载原来版本的cv_bridge，最终成功运行。在debug的过程还发现要保持深度相机和vins-mono的图像大小一致(640x480)，否则也会报错，最后是重新安装realsense-ros解决；开始使用时注意先把摄像头前后左右晃一下，这个是给IMU初始化用的。

参考教程
从零开始使用Realsense D435i运行VINS-Mono
Realsense D435i 相机与VINS-Mono连接时右侧不显示轨迹问题的解决
关于ros Melodic与opencv 4.5.5版本冲突cv_bridge报错的解决办法
Ubuntu下多版本OpenCV共存和切换

1.安装测试VINS-Mono

先用数据集测试VINS-Mono

cd ~/catkin_ws/src
git clone https://github.com/HKUST-Aerial-Robotics/VINS-Mono.git
cd ..
catkin_make
source ~/catkin_ws/devel/setup.bash

数据集地址https://projects.asl.ethz.ch/datasets/doku.php?id=kmavvisualinertialdatasets
然后打开三个terminal，分别运行

roslaunch vins_estimator euroc.launch 
roslaunch vins_estimator vins_rviz.launch
rosbag play MH_01_easy.bag 

2.在RealsenseD435i上运行VINS-Mono

前面都测试好之后就可以在RealsenseD435i上运行VINS-Mone了

修改realsense包里的rs_camera.launch文件
第一处，修改unite_imu_method如下，这里是让IMU的角速度和加速度作为一个topic输出

 <arg name="unite_imu_method"      default="copy"/>

第二处，修改enable_sync参数为true，这里是开机相机和IMU的同步

  <arg name="enable_gyro"         default="true"/>
  <arg name="enable_accel"        default="true"/>
  <arg name="enable_sync"           default="true"/>

修改VINS-Mono包里的realsense_color_config.yaml文件
第一处，修改订阅的topic

imu_topic: "/camera/imu"
image_topic: "/camera/color/image_raw"

第二处，修改相机内参，这里先再次打开运行realsesne包，然后可以通过如下命令获取相机内参

rostopic echo /camera/color/camera_info

话题打印信息中D就是畸变系数，数值都为0；而K就是相机内参，将其替换掉配置文件中的默认值：

#camera calibration 
model_type: PINHOLE
camera_name: camera
image_width: 640
image_height: 480
distortion_parameters:
   k1: 0 #9.2615504465028850e-02
   k2: 0 #-1.8082438825995681e-01
   p1: 0 #-6.5484100374765971e-04
   p2: 0 #-3.5829351558557421e-04
projection_parameters:
   fx: 9.05654296875e+02 #6.0970550296798035e+02
   fy: 9.05483398438e+02 #6.0909579671294716e+02
   cx: 6.50563598633e+02  #3.1916667152289227e+02
   cy: 3.54959960938e+02  #2.3558360480225772e+02

第三处，IMU到相机的变换矩阵，这里我根据注释的提示修改成2

# Extrinsic parameter between IMU and Camera.
estimate_extrinsic: 2   # 0  Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it.
                        # 1  Have an initial guess about extrinsic parameters. We will optimize around your initial guess.
                        # 2  Don't know anything about extrinsic parameters. You don't need to give R,T. We will try to calibrate it. Do some rotation movement at beginning.                        
#If you choose 0 or 1, you should write down the following matrix.

第四处，IMU参数，这里我全部修改注释给的参数

#imu parameters       The more accurate parameters you provide, the better performance
acc_n: 0.2          # accelerometer measurement noise standard deviation. #0.2
gyr_n: 0.05         # gyroscope measurement noise standard deviation.     #0.05
acc_w: 0.02         # accelerometer bias random work noise standard deviation.  #0.02
gyr_w: 4.0e-5       # gyroscope bias random work noise standard deviation.     #4.0e-5
g_norm: 9.80       # gravity magnitude

第五处，是否需要在线估计同步时差，根据上述博主的建议这里选择不需要

#unsynchronization parameters
estimate_td: 0                      # online estimate time offset between camera and imu
td: 0.000                           # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock)

第六处，相机曝光改成全局曝光

#rolling shutter parameters
rolling_shutter: 0                      # 0: global shutter camera, 1: rolling shutter camera
rolling_shutter_tr: 0               # unit: s. rolling shutter read out time per frame (from data sheet). 

打开深度相机，运行VINS-Mono

roslaunch realsense2_camera rs_camera.launch 
roslaunch vins_estimator realsense_color.launch 
roslaunch vins_estimator vins_rviz.launch

3.报错记录

问题一
编译vins-mono，opencv版本冲突

cmake_minimum_required(VERSION 2.8.3)
project(ar_demo)

set(CMAKE_BUILD_TYPE "Release")
set(CMAKE_CXX_FLAGS "-std=c++11 -DEIGEN_DONT_PARALLELIZE")
#-DEIGEN_USE_MKL_ALL")
set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g")

set(OpenCV_DIR /usr/local/include/opencv2)
include_directories(${OpenCV_INCLUDE_DIRS}) 


find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs
  image_transport
  sensor_msgs
  cv_bridge
  message_filters
  camera_model
)
find_package(OpenCV 3.3.1 REQUIRED)
message("OpenCV version  is :  ${OpenCV_VERSION}")

catkin_package(

)


include_directories(
  ${catkin_INCLUDE_DIRS}
)

set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake)
find_package(Eigen3)
include_directories(
  ${catkin_INCLUDE_DIRS}
  ${EIGEN3_INCLUDE_DIR}
)

add_executable(ar_demo_node src/ar_demo_node.cpp)

 target_link_libraries(ar_demo_node
   ${catkin_LIBRARIES} ${OpenCV_LIBS}
 )

解决方法
设置特定版本的opencv3.3.1，find_package(OpenCV 3.3.1 REQUIRED)

问题二

no previous pose graph
OpenCV Error: Assertion failed (_mask.empty() || (_mask.type() == (((0) & ((1 << 3) - 1)) + (((1)-1) << 3)) && _mask.sameSize(_image))) in goodFeaturesToTrack, file /tmp/binarydeb/ros-kinetic-opencv3-3.3.1/modules/imgproc/src/featureselect.cpp, line 366
terminate called after throwing an instance of 'cv::Exception'
  what():  /tmp/binarydeb/ros-kinetic-opencv3-3.3.1/modules/imgproc/src/featureselect.cpp:366: error: (-215) _mask.empty() || (_mask.type() == (((0) & ((1 << 3) - 1)) + (((1)-1) << 3)) && _mask.sameSize(_image)) in function goodFeaturesToTrack

[feature_tracker-1] process has died [pid 28321, exit code -6, cmd /home/k/catkin_ws/devel/lib/feature_tracker/feature_tracker __name:=feature_tracker __log:=/home/k/.ros/log/6cc297dc-da08-11eb-9403-244bfeccb575/feature_tracker-1.log].
log file: /home/k/.ros/log/6cc297dc-da08-11eb-9403-244bfeccb575/feature_tracker-1*.log

解决方法
具体原因是masksize和forw_img尺寸不一致，即相机和vins-mono设定的图像尺寸不一致，查看log文件，有报错信息提示

if (mask.size() != forw_img.size()){
    cout << "wrong size111 " << endl;
    cout<<"mask.size()"<<mask.size()<<endl;
    cout<<"forw_img.size()"<<forw_img.size()<<endl;
}

问题三
rviz 的 Global Statu 报错

解决方法
开始使用时注意先把摄像头前后左右晃一下，这个是给IMU初始化用的，Global Statu 也会恢复正常。

附录：踩坑记录

1.VINS-Mono在opencv4下的代码编写

问题一

//报错
error:  /home/ubuntu/catkin_ws/src/VINS-Mono/camera_model/src/chessboard/http://Chessboard.cc: In constructor ‘camodocal::Chessboard::Chessboard(cv::Size, cv::Mat&)’:
error:  /home/ubuntu/catkin_ws/src/VINS-Mono/camera_model/src/chessboard/Chessboard.cc:20:38: error: ‘CV_GRAY2BGR’ was not declared in this scope
cv::cvtColor(image, mSketch, CV_GRAY2BGR);

//解决方法
在报错文件上添加头文件 #include <opencv2/imgproc/imgproc_c.h>

问题二

//报错 
error: ‘CV_CALIB_CB_ADAPTIVE_THRESH’ was not declared in this scope 、
error: ‘CV_CALIB_CB_NORMALIZE_IMAGE’ was not declared in this scope、
error: ‘CV_CALIB_CB_FILTER_QUADS’ was not declared in this scope 、
error: ‘CV_CALIB_CB_FAST_CHECK’ was not declared in this scope ....

//解决方法
将 CV_CALIB_CB_ADAPTIVE_THRESH 修改为 cv::CALIB_CB_ADAPTIVE_THRESH、
将 CV_CALIB_CB_NORMALIZE_IMAGE 修改为 cv::CALIB_CB_NORMALIZE_IMAGE、
将 CV_CALIB_CB_FILTER_QUADS 修改为 cv::CALIB_CB_FILTER_QUADS、
将 CV_CALIB_CB_FAST_CHECK 修改为 cv::CALIB_CB_FAST_CHECK 

问题三

//报错
 error: ‘CV_RGB2GRAY’ was not declared in this scope
/home/ubuntu/catkin_ws/src/VINS-Mono/pose_graph/src/ThirdParty/DVision/BRIEF.cpp:53:32: error: ‘CV_RGB2GRAY’ was not declared in this scope cv::cvtColor(image, aux, CV_RGB2GRAY);
^~~~~~~~~~~
/home/ubuntu/catkin_ws/src/VINS-Mono/pose_graph/src/ThirdParty/DVision/BRIEF.cpp:53:32: note: suggested alternative: ‘CV_RGB’ cv::cvtColor(image, aux, CV_RGB2GRAY);

//解决方法
在报错文件上添加头文件 #include<opencv2/imgproc/types_c.h>

问题四

//报错
 error: ‘CV_FONT_HERSHEY_SIMPLEX’ was not declared in this scope 

//解决方法
 将报错文件上的 CV_FONT_HERSHEY_SIMPLEX 参数改为 cv::FONT_HERSHEY_SIMPLEX

问题五

//报错
单独遇到CV_AA的报错
//解决方法
将 CV_AA 改为 cv::LINE_AA

2.解决cv_bridge与opencv4版本不匹配

问题描述
ubutnu18.04对应的ROS中，opencv版本是3.2.0，cv_bridge对应的opencv版本也是3.2.0，但有些情况下我们自己安装的opencv版本和ros自带的版本不一致，即编译项目时使用的opencv版本和使用的cv_bridge对应的opencv版本不一致，因此版本有冲突

解决方法

1.卸载ros自带的cv_bridge

sudo apt-get remove ros-melodic-cv-bridge

2.下载cv_bridge(opencv4版本)

mkdir -p cv_bridge/src
cd cv_bridge/src
git clone https://github.com/fizyr-forks/vision_opencv.git
gedit ~/cv_bridge/src/vision_opencv-opencv4/cv_bridge/CMakeLists.txt 
cd ..
cdtkin_make

3.修改cv_bridge包下的CMakeLists.txt ：

cmake_minimum_required(VERSION 2.8)
project(cv_bridge)
#opencv4库的路径
set(OpenCV_DIR "/usr/local/include/opencv4/opencv2")					#opencv库的路径，大家应该都一样
find_package(catkin REQUIRED COMPONENTS rosconsole sensor_msgs)

if(NOT ANDROID)
  find_package(PythonLibs)
  if(PYTHONLIBS_VERSION_STRING VERSION_LESS 3)
    find_package(Boost REQUIRED python)
  else()
    find_package(Boost REQUIRED python3)
  endif()
else()
find_package(Boost REQUIRED)
endif()

#改成4
find_package(OpenCV 4 REQUIRED						
  COMPONENTS
    opencv_core
    opencv_imgproc
    opencv_imgcodecs
  CONFIG
)

catkin_package(
  INCLUDE_DIRS include
  LIBRARIES ${PROJECT_NAME}
  CATKIN_DEPENDS rosconsole sensor_msgs
  DEPENDS OpenCV
  CFG_EXTRAS cv_bridge-extras.cmake
)

catkin_python_setup()

include_directories(include ${Boost_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS})

if(NOT ANDROID)
add_subdirectory(python)
endif()
add_subdirectory(src)
if(CATKIN_ENABLE_TESTING)
  add_subdirectory(test)
endif()

# install the include folder
install(
  DIRECTORY include/${PROJECT_NAME}/
  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
)

4.这样就得到了基于自己opencv版本的cv_bridge，将cv_bridge安装到系统路径下

cd ~/cv_bridge/src/vision_opencv-opencv4/cv_bridge
mkdir build
cd build
cmake ..
make
sudo make install

5.最后在项目使用cv_bridge中的CMakeLists.txt 前面加上

#没有安装到系统路径下
set(cv_bridge_DIR ~/cv_bridge/devel/share/cv_bridge/cmake)
#安装到系统路径下
set(cv_bridge_DIR /usr/local/share/cv_bridge/cmake) 
set(OpenCV_DIR "/usr/local/include/opencv4/opencv2")