INDEMIND运行VINS-Mono

前提：VINS-Mono 已经可以正常运行数据集

1.安装SDK(INDEMIND)

官方文档
除OpenCV以外主要安装另外两个包protobuf和MNN，依赖库装好后，选择官方链接中的Ros Wrapper安装继续，完成即可
INDEMIND相机一定需要opencv3.4.2以上版本！！！否则无法运行！！！

• 源码安装protobuf
  protobuf文件夹下

$ make
$ sudo make install
$ sudo ldconfig
$ protoc --version  #安装成功则显示版本

• 源码安装MNN
  MNN文件夹下

$ mkdir build
$ cd build
$ camke ..
$ make
$ sudo make install

• 编译样例
  IMSEE文件夹下

$ make demo
$ ./demo/output/bin/get_image #电脑连接相机运行，可以运行则上面的依赖安装成功

• 编译相机ros包
  IMSEE文件夹下

$ make ros
>>进入ros文件夹
$ sudo su  #和realsense相机不同，indemind相机启动需要权限
$ source devel/setup.bash
$ roslaunch imsee_ros_wrapper start.launch  #启动相机
或者
$ roslaunch imsee_ros_wrapper display.launch  #启动相机的同时启动rviz,可以在rviz直接看相机传回的图像

TX2运行INDEMIND报错 libopencv_calib3d3.so.3.3.

Failed to load nodelet [/imsee/imsee_wrapper_node] of type [imsee/ROSWrapperNodelet] even after refreshing the cache: Failed to load library ../indemind/IMSEE-SDK/ros/devel/lib//libimsee_wrapper.so. 
Make sure that you are calling the PLUGINLIB_EXPORT_CLASS macro in the library code, and that names are consistent between this macro and your XML. 
Error string: Could not load library (Poco exception = libopencv_calib3d3.so.3.3.so.3.3: cannot open shared object file: No such file or directory)

解决方法：
将lib/others/tx2-opencv3.4.3中的libindemind.so文件放到lib/aarch64中，重新编译。

2.在VINS-Mono中添加配置文件

• config文件：在VINS-Mono/config文件夹中新建文件夹，在文件夹中建立文件，命名为indemind_config.yaml，文件内容如下：

%YAML:1.0
#common parameters
imu_topic: "/imsee/imu"
image_topic: "/imsee/image/rectified/left"
output_path: "/home/shaozu/output/"

#camera calibration 
model_type: PINHOLE
camera_name: camera
image_width: 640
image_height: 400
distortion_parameters:  //此处参数需要修改为自己的相机参数
   k1: 0.0
   k2: 0.0
   p1: 0.0
   p2: 0.0
projection_parameters:  //此处参数需要修改
   fx: 259.214
   fy: 259.214
   cx: 326.583
   cy: 173.762

#Extrinsic parameter between IMU and Camera.
estimate_extrinsic: 1   # 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.
#Rotation from camera frame to imu frame, imu^R_cam
extrinsicRotation: !!opencv-matrix  //此处参数需要修改
   rows: 3
   cols: 3
   dt: d
   data: [-0.999931, 0.010294, 0.005654,
           -0.010296, -0.999947, -0.000368, 
           0.005649, -0.000426, 0.999984]
#Translation from camera frame to imu frame, imu^T_cam
extrinsicTranslation: !!opencv-matrix  //此处参数需要修改
   rows: 3
   cols: 1
   dt: d
   data: [0.0599903, 0.000129698, -0.002]

#feature traker paprameters
max_cnt: 150            # max feature number in feature tracking
min_dist: 30            # min distance between two features 
freq: 10                # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image 
F_threshold: 1.0        # ransac threshold (pixel)
show_track: 1           # publish tracking image as topic
equalize: 1             # if image is too dark or light, trun on equalize to find enough features
fisheye: 0              # if using fisheye, trun on it. A circle mask will be loaded to remove edge noisy points

#optimization parameters
max_solver_time: 0.04  # max solver itration time (ms), to guarantee real time
max_num_iterations: 8   # max solver itrations, to guarantee real time
keyframe_parallax: 10.0 # keyframe selection threshold (pixel)

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

#loop closure parameters
loop_closure: 1                    # start loop closure
load_previous_pose_graph: 0        # load and reuse previous pose graph; load from 'pose_graph_save_path'
fast_relocalization: 0             # useful in real-time and large project
pose_graph_save_path: "/home/shaozu/output/pose_graph/" # save and load path

#unsynchronization parameters
estimate_td: 1                      # online estimate time offset between camera and imu
td: -0.0011101                             # 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). 

#visualization parameters
save_image: 1                   # save image in pose graph for visualization prupose; you can close this function by setting 0 
visualize_imu_forward: 0        # output imu forward propogation to achieve low latency and high frequence results
visualize_camera_size: 0.4      # size of camera marker in RVIZ`

其中，distortion_parameters，projection_parameters， extrinsicRotation，extrinsicTranslation 矩阵的参数需要根据自己的相机做一些改动

• launch文件：在VINS-Mono/vins_estimator/launch/下创建indemind.launch文件，内容如下：

<launch>
  <arg name="config_path" default = "$(find feature_tracker)/../config/imsee/indemind_config.yaml" />  //路径为上面配置文件的路径
    <arg name="vins_path" default = "$(find feature_tracker)/../config/../" />
  <node name="feature_tracker" pkg="feature_tracker" type="feature_tracker" output="log">
    <param name="config_file" type="string" value="$(arg config_path)" />
    <param name="vins_folder" type="string" value="$(arg vins_path)" />
  </node>
  <node name="vins_estimator" pkg="vins_estimator" type="vins_estimator" output="screen">
    <param name="config_file" type="string" value="$(arg config_path)" />
    <param name="vins_folder" type="string" value="$(arg vins_path)" />
  </node>
  <node name="pose_graph" pkg="pose_graph" type="pose_graph" output="screen">
    <param name="config_file" type="string" value="$(arg config_path)" />
    <param name="visualization_shift_x" type="int" value="0" />
    <param name="visualization_shift_y" type="int" value="0" />
    <param name="skip_cnt" type="int" value="0" />
    <param name="skip_dis" type="double" value="0" />
  </node>
</launch> 

3.启动相机和VINS-Mono

(相信大家既然已经可以运行VINS-Mono，即对ROS有了一定了解

• 启动相机：在IMSEE-SDK目录下启动相机，启动时需要管理员下运行命令

sudo su
source ros/devel/setup.bash //ros空间初始化
//下面两句选一句即可
roslaunch imsee_ros_wrapper start.launch //启动相机
roslaunch imsee_ros_wrapper display.launch  //启动相机并在rviz下可视

• 启动VINS-Mono：

source devel/setup.bash
roslaunch vins_estimator indemind.launch

• 查看VINS-Mono下的rviz:

source devel/setup.bash
roslaunch vins_estimator rviz.launch