基于ROS下的安卓手机图像和IMU跑ORB-SLAM3

接之前文章，手上没有现成的单目+IMU硬件，但是安卓手机很普及。因此，本文讲下如何用手机跑ORB-SLAM3的Mono和Mono_Inertial。

1. 手机与PC通信

基于ROS下的信息发布和订阅，手机和PC在一个局域网下进行信息（image和IMU）传输。

手机APK参考了github上的2个开源Android_Camera-IMU和android_ros_sensors，基于ros_java生成安卓APP，下载安装任一个APP到安卓手机，界面分别是这样：

手机和PC连一个无线网，如果没有无线网，用手机开一个热点，PC连热点也可以。

1）Ubuntu下roscore，启动ros。

2）ubuntu下ifconfig查看自己的IP，填入APP的localhost处（http://localhost:11311），第1张图是第一个APP，连无线网的IP，第2张图是第2个APP，连手机热点的IP。

3）rostopic list查看主题，发现图像是compressed，接入SLAM必须是raw才行，故进行republish：

rosrun image_transport republish compressed in:=/android/image_raw raw out:=/camera/image_raw

之后再看下rostopic list：

有了/camera/image_raw，此时OK了，你可以终端rostopic echo 对应主题 和rostopic hz 对应主题 查看信息是否OK和频率。

2 参数标定

跑单目+IMU效果好不好，最重要的是参数标定的准不准，参数包括：相机参数，IMU参数，相机和IMU的外参数，相机和IMU时间对齐的时间差。

相机参数可以通过ROS、opencv、matlab进行标定，IMU参数可以通过imu_utils进行标定，相机和IMU外参数可以通过kalibr或vins_mono在线标定，相机和IMU时间对齐的时间差可以通过vins_mono讲的或自己的代码标定，本文篇幅所限，后续如有时间再讲。

3 修改源码

在ros_mono_inertial.cc里修改。

1）修改订阅主题，在main函数里

ros::Subscriber sub_imu = n.subscribe("/android/imu", 2000, &ImuGrabber::GrabImu, &imugb);  //imu
ros::Subscriber sub_img0 = n.subscribe("/camera/image_raw", 100, &ImageGrabber::GrabImage,&igb);
std::thread sync_thread(&ImageGrabber::SyncWithImu,&igb);

2）如果是用第一个APP，IMU和相机的时间戳时间相差非常大，需要代码同步对齐，ImageGrabber::SyncWithImu函数里

if(!mpImuGb->imuBuf.empty())
            {
                // Load imu measurements from buffer
                vImuMeas.clear();
                static bool time_flag = true;
                static auto TIME_OFFSET = 0.0;
                if(time_flag){
                    TIME_OFFSET = mpImuGb->imuBuf.front()->header.stamp.toSec()-tIm;
                    time_flag = false;
                }
                //cout<<"imu_time: "<<setprecision(11)<<mpImuGb->imuBuf.front()->header.stamp.toSec()-TIME_OFFSET<<endl;
                //cout<<"image_time: "<<setprecision(11)<<tIm<<endl;
                while(!mpImuGb->imuBuf.empty() && mpImuGb->imuBuf.front()->header.stamp.toSec()-TIME_OFFSET<=tIm)
                {
                    double t = mpImuGb->imuBuf.front()->header.stamp.toSec() - TIME_OFFSET;
                    cv::Point3f acc(mpImuGb->imuBuf.front()->linear_acceleration.x, mpImuGb->imuBuf.front()->linear_acceleration.y, mpImuGb->imuBuf.front()->linear_acceleration.z);
                    cv::Point3f gyr(mpImuGb->imuBuf.front()->angular_velocity.x, mpImuGb->imuBuf.front()->angular_velocity.y, mpImuGb->imuBuf.front()->angular_velocity.z);
                    //cout<<"imudata: "<< t<<" "<<acc.x<<" "<<acc.y<<" "<<acc.z<<" "<<gyr.x<<" "<<gyr.y<<" "<<gyr.z<<endl;
                    vImuMeas.push_back(ORB_SLAM3::IMU::Point(acc,gyr,t));

                    mpImuGb->imuBuf.pop();
                }
            }

如果是用第二个APP，图像大小是1920*1080，需要进行resize和对应size下的图像参数标定。

4 跑ORB-SLAM3

按照ORB-SLAM3里的ros exsample的方法进行。

继续第一节的操作，首先/android/imu和/camera/image_raw数据都是OK的，之后

rosrun ORB_SLAM3 Mono_Inertial ORBvoc.bin ../../Monocular-Inertial/Mate10.yaml true

如果只跑单目，就是

rosrun ORB_SLAM3 Mono ORBvoc.bin ../../Monocular/Mate10.yaml

此时，就出来画面了，自己的参数一定要标好和微调，然后就根据轨迹效果实时的测试场景、优化算法吧。