• 利用手机摄像头采集图片运行ORB-SLAM2

    一、ROS配置安装

    二、ORB_SLAM2配置安装

    可参考前文  

    ROS仿真环境安装与配置_身在江湖的郭大侠的博客-CSDN博客

    三、Android手机摄像头与ROS建立通信

    GitHub有个开源的项目，可以通过wifi将摄像头捕捉到的图片传输到ROS，地址：

    https://github.com/hitcm/Android_Camera-IMU

    作者也给出了博文，

    ROS实时采集Android的图像和IMU数据 - hitcm - 博客园

    下载代码、安装工具包

    1. git clone https://github.com/hitcm/Android_Camera-IMU.git
    2.  
    3. sudo apt-get install ros-melodic-imu-tools  # 按实际安装indigo、Kinetic 等安装对应的工具包

    三个文件

    Camera_Imu.apk安装到手机上，注意有的手机可能因为安装版本过高导致无法运行此程序，可以试着在手机上装一个虚拟软件如VMOS Pro，这个安卓版本是7.1可以运行，还有一个版本的apk

    Releases · huaibovip/android_ros_sensors · GitHub

    这个版本的apk带源码，大佬可以二次开发，但是此apk笔者并未试成功

    手机和电脑要在同一局域网下，在实验前可以在手机上安装一个ping工具，确保电脑和手机的网络是互通的，笔者是虚拟机，为了让手机与虚拟机的网互通，这是把虚拟机的网络模式设为桥接模式

     

     虚拟机打开

    Terminal1 输入：

    roscore

    Android: 打开应用，在 在 IP Port 中修改 IP 地址为 PC的 IP地址，port不需要修改（PC 的 IP 可在 PC 终端输入 ifconfig 查看），之后点击 Connect，连接成功则进入相机界面。

    Terminal 2 到下载git文件的目录打开android_cam-imu.launch：

    1. cd Android_Camera-IMU      
    2. roslaunch android_cam-imu.launch

    这是会弹出一个Rviz界面：

    如果要实时显示则

    • 如果要实时显示 image，需要 Add - By topic - 添加/camera/image_raw/image。
    • 如果要显示 imu，则需要 Add - By topic - 添加 imu，且在 Fix Frame 中 将 map 改为 imu。

     

     PS：

    可以在Ubuntu里面用

    rostopic list

    查询订阅的主题，可以看到有订阅的图像和imu

     

     

     手机摄像头标定

    为了 ORB-SLAM2 准确运行，需要对手机摄像头进行标定。标定方式为：对棋盘格标定板进行各个方向的拍照，之后基于 OpenCV 进行标定。注意这里采集的图片需要和 ORB-SLAM2 程序读取到的一致，所以不能直接使用手机自带相机 app 拍照，因为手机会自动通过算法进行校正，而上述通信传输的是 raw images。因此，首先我们需要完成的任务是：采集并保存摄像头图像。

    使用下图作为标定板（参考资料），可直接在电脑屏幕上显示，对其拍照即可。

    注意：

    • 实验发现，使用长宽格数不一样的棋盘标定板效果更好。
    • 实验发现，标定板周围要是白色的才行，黑色的提取不出角点来（在电脑屏幕上显示标定板时尤其需要注意）。
    • 摄像头需要从不同方向拍摄棋盘格，可参考 OpenCV 安装目录下 samples/data 中的 left0x.jpg 系列标定图片。

    采集并保存图片

    目前没有找到直接保存的方法，所以我们选择写一个 ROS node 来接收手机传来的图像，再通过 OpenCV 进行显示和保存。

    为了方便，我们选择直接在 ORB-SLAM2 的 ros_mono.cc 的代码基础上进行修改，在 ros_mono.cc 同一目录下写了个 ros_camera_capture.cc：

    1. /**
    2. * This file is to capture images from Android phone, for camera calibration
    3. * This file is used with Android_Camera-IMU
    4. */
    5.  
    6. #include<iostream>
    7. #include<algorithm>
    8. #include<fstream>
    9. #include<chrono>
    10.  
    11. #include<ros/ros.h>
    12. #include <cv_bridge/cv_bridge.h>
    13.  
    14. #include<opencv2/core/core.hpp>
    15.  
    16. #include"../../../include/System.h"
    17.  
    18. using namespace std;
    19.  
    20. string save_dir = "PATH"; // 修改为自己保存图片的路径
    21. int imgId = 0;
    22.  
    23. void GrabImage(const sensor_msgs::ImageConstPtr& msg);
    24.  
    25. int main(int argc, char **argv)
    26. {
    27.  
    28.     std::cout << "To save the current frame, please press 'Q' or 'q' " << std::endl;
    29.     std::cout << "The images will be saved to " <<  save_dir << std::endl;
    30.  
    31.     ros::init(argc, argv, "PClistener");
    32.     ros::start();
    33.  
    34.     ros::NodeHandle nodeHandler;
    35.     ros::Subscriber sub = nodeHandler.subscribe("/camera/image_raw", 1, GrabImage);
    36.  
    37.     ros::spin();
    38.  
    39.     ros::shutdown();
    40.  
    41.     return 0;
    42. }
    43.  
    44. void GrabImage(const sensor_msgs::ImageConstPtr& msg)
    45. {
    46.     string imgname;
    47.     cv_bridge::CvImageConstPtr cv_ptr;
    48.     try
    49.     {
    50.         cv_ptr = cv_bridge::toCvShare(msg);
    51.         cv::Mat img = cv_ptr->image;
    52.         cv::imshow("img_name", img);
    53.  
    54.         char key = cv::waitKey(1);
    55.         // press "q" to save the image 
    56.         if(key == 'q' || key == 'Q'){  
    57.             imgId++;
    58.             imgname = "img_" + to_string(imgId) + ".jpg";
    59.             cv::imwrite(save_dir + imgname, img);
    60.             std::cout << "has saved image "<< imgId << " to " << save_dir << std::endl;
    61.         }
    62.     }
    63.     catch (cv_bridge::Exception& e)
    64.     {
    65.         ROS_ERROR("cv_bridge exception: %s", e.what());
    66.         return;
    67.     }
    68. }

    注意捕捉图像存储地址换成自己的

    另外，在 ORB_SLAM2/Examples/ROS/ORB_SLAM2 目录中的 CMakeLists.txt 中添加如下内容（添加在 # Node for monocular camera 上方即可）：

    1. # Node for capture images for camera calibration
    2. rosbuild_add_executable(CameraCapture
    3. src/ros_camera_capture.cc
    4. )
    5.  
    6. target_link_libraries(CameraCapture
    7. ${LIBS}
    8. )

    之后重新编译 ORB_SLAM2 项目。

    1. cd PATH/ORB_SLAM2
    2. ./build_ros.sh

    使用方法：

    Terminal 1:

    roscore

    手机进入 app 运行

    Terminal 2: 在 Android_Camera-IMU 目录

    roslaunch android_cam-imu.launch

    （可以关掉 Rviz）

    Terminal 3:

    rosrun ORB_SLAM2 CameraCapture

    鼠标选中弹出的图像框，按下 q 键保存图像。一定要多角度多捕捉几张图片，否则很难捕捉到关键点。像这样

     

    进行标定

    使用 OpenCV samples 中的代码实现。参考资料

    标定例程

    新建一个工作目录（文件夹）camera_calibration_opencv，将 OpenCV 安装目录中的 samples/cpp/tutorial_code/calib3d/camera_calibration 文件夹内的内容拷贝至该目录。

    修改 VID5.xml

    VID5.xml 中存储着标定图像的路径，所以要在 VID.xml 中添加所有标定图像的路径，eg：图片换成自己捕捉照片存储的位置

    1. <?xml version="1.0"?>
    2. <opencv_storage>
    3. <images>
    4. /home/ghj/Desktop/slam/pictureimg_1.jpg
    5. /home/ghj/Desktop/slam/pictureimg_2.jpg
    6. /home/ghj/Desktop/slam/pictureimg_3.jpg
    7. /home/ghj/Desktop/slam/pictureimg_4.jpg
    8. /home/ghj/Desktop/slam/pictureimg_5.jpg
    9. </images>
    10. </opencv_storage>

    修改 in_VID5.xml

    表示棋盘格的宽和高，注意，这里的宽度和高度是指内部交叉点的个数，而不是方形格的个数。如上图棋盘的数据就是9和6。

    1. <BoardSize_Width> 9</BoardSize_Width>
    2. <BoardSize_Height>6</BoardSize_Height>

    修改为每格的边长 (mm)，拿尺子量。

    <Square_Size>20</Square_Size>

    修改 输入的VID5.xml 的路径（换成自己的））

    <Input>"/home/ghj/Desktop/slam/camera_calibration_opencv/VID5.xml"</Input>

    修改输出参数信息存储的地址（换成自己的）

      <Write_outputFileName>"/home/ghj/Desktop/slam/camera_calibration_opencv/out_camera_data.xml"</Write_outputFileName>

     此处原来是0，需要改为1，表示引入切向畸变参数（因为 ORB-SLAM2 中也引入了切向畸变参数），否则只有径向畸变参数。

    <Calibrate_FixPrincipalPointAtTheCenter> 1 </Calibrate_FixPrincipalPointAtTheCenter>

    其它地方应该不需要改动，想进一步了解可看其中的注释。

    1. "1.0"?>
    2. <opencv_storage>
    3. <Settings>
    4.   
    5.   <BoardSize_Width> 9BoardSize_Width>
    6.   <BoardSize_Height>6BoardSize_Height>
    7.   
    8.   
    9.   <Square_Size>20Square_Size>
    10.   
    11.   
    12.   <Calibrate_Pattern>"CHESSBOARD"Calibrate_Pattern>
    13.   
    14.   
    15.   <Input>"/home/ghj/Desktop/slam/camera_calibration_opencv/VID5.xml"Input>
    16.   
    17.   <Input_FlipAroundHorizontalAxis>0Input_FlipAroundHorizontalAxis>
    18.   
    19.   
    20.   <Input_Delay>100Input_Delay>	
    21.   
    22.   
    23.   <Calibrate_NrOfFrameToUse>25Calibrate_NrOfFrameToUse>
    24.   
    25.   <Calibrate_FixAspectRatio> 1 Calibrate_FixAspectRatio>
    26.   
    27.   <Calibrate_AssumeZeroTangentialDistortion>1Calibrate_AssumeZeroTangentialDistortion>
    28.   
    29.   <Calibrate_FixPrincipalPointAtTheCenter> 1 Calibrate_FixPrincipalPointAtTheCenter>
    30.   
    31.   
    32.   <Write_outputFileName>"/home/ghj/Desktop/slam/camera_calibration_opencv/out_camera_data.xml"Write_outputFileName>
    33.   
    34.   <Write_DetectedFeaturePoints>1Write_DetectedFeaturePoints>
    35.   
    36.   <Write_extrinsicParameters>1Write_extrinsicParameters>
    37.   
    38.   <Show_UndistortedImage>1Show_UndistortedImage>
    39.  
    40. Settings>
    41. opencv_storage>

    编译

    在工作目录 camera_calibration_opencv 中新建 CMakeLists.txt:

    1. project(Camera_Calibration)
    2. set(CMAKE_CXX_STANDARD 11)
    3.  
    4. find_package(OpenCV 3.0 QUIET)
    5. if(NOT OpenCV_FOUND)    
    6.     find_package(OpenCV 2.4.3 QUIET)
    7.     if(NOT OpenCV_FOUND)
    8.         message(FATAL_ERROR "OpenCV > 2.4.3 not found.")
    9.     endif()
    10. endif()
    11.  
    12. include_directories(${OpenCV_INCLUDE_DIR})
    13. add_executable(Camera_Calibration camera_calibration.cpp)
    14. target_link_libraries(Camera_Calibration ${OpenCV_LIBS})

    之后编译：

    1. cd camera_calibration_opencv
    2. mkdir build
    3. cd build
    4. cmake ..
    5. make

    运行，标定

    1. cd camera_calibration_opencv
    2. ./build/Camera_Calibration ../in_VID5.xml

    程序启动后会显示标定图像的角点提取情况，之后会显示校正后图像，一个一个全部关闭后才会保存标定参数至 out_camera_data.xml。

     标定结果写入camera_calibration/out_cam

     

    •  是相机内参矩阵，顺序为 fx, 0, cx; 0, fy, cy; 0, 0, 1。
    •  是畸变参数，其顺序为 k1, k2, p1, p2, k3。

    之后在 ORB_SLAM2 中新建一个配置文件 mycam.yaml（建哪儿都行），将 TUM1.yaml 的内容拷贝过来，并把其中的 Camera 参数进行修改。

    注意： 相机参数对 ORB-SLAM2 的运行效果有极大影响（尤其是初始化），所以标定过程须认真完成。

    运行 ORB-SLAM2 Mono

    Terminal 1:

    roscore

    手机进入 app 运行

    Terminal 2: 在 Android_Camera-IMU 目录

    roslaunch android_cam-imu.launch

    （可以关掉 Rviz）

    Terminal 3(加载的两个文件换成自己的）:

    rosrun ORB_SLAM2 Mono /home/ghj/ROS_SIM/src/ORB_SLAM2/Vocabulary/ORBvoc.txt /home/ghj/ROS_SIM/src/ORB_SLAM2/Examples/ROS/ORB_SLAM2/mycam.yaml

    运行效果展示：

     

    注意： ORB-SLAM2 Mono 还是比较难以初始化的（其设置的初始化条件相对苛刻），在开始时，选择特征纹理丰富的区域，多上下左右平移相机，有利于初始化。

    简化启动

    上述启动步骤需要启动3个终端，挺麻烦的，所以可以选择写一个脚本来自动启动这3个终端。参考资料

    新建 ORB_SLAM2_with_AndroidPhone.sh，在其中填入：

    1. gnome-terminal --title="roscore" -x bash -c "roscore"
    2. # 暂停 2s，保证几个不同终端的启动顺序
    3. sleep 2s;  
    4.  
    5. gnome-terminal --title="AndroidPhone" -x bash -c "cd /home/ghj/Desktop/slam/Android_Camera-IMU; roslaunch android_cam-imu.launch"
    6. sleep 2s;
    7.  
    8. gnome-terminal --title="ORB-SLAM2" -x bash -c "rosrun ORB_SLAM2 Mono /home/ghj/ROS_SIM/src/ORB_SLAM2/Vocabulary/ORBvoc.txt /home/ghj/ROS_SIM/src/ORB_SLAM2/Examples/ROS/ORB_SLAM2/mycam.yaml"

    之后赋予权限（仅需一次）：

    chmod +x ORB_SLAM2_with_AndroidPhone.sh

    运行：

    ./ORB_SLAM2_with_AndroidPhone.sh

    即可一次性打开3个终端，并运行相关命令。之后手机再打开 app 就可以了。

    注意： 此时终端运行结束后会自动退出，如果不想自动退出，可 在terminal点右键，选择Preferences然后找到Command，里面有一项When command exits，后面选择为Hold the terminal open。参考资料

     

     

  • 相关阅读:
    菁染料CY7.5标记海藻酸钠|CY7.5-海藻酸钠|alginate-Cyanine7.5|CY7.5-peg-海藻酸钠alginate
    Win11怎么调亮度？Win11调屏幕亮度的四种方法
    基于Springboot的人事管理系统 （有报告）。Javaee项目，springboot项目。
    从零开始 Spring Cloud 15：多级缓存
    找工作----C++面试题库
    二、链表（linked-list）
    django组件552
    基于JavaSwing开发记事本程序(新功能替换查找撤销状态栏) 课程设计 大作业源码
    不需要服务器，教你仅用30行代码搞定实时健康码识别
    PPT文件不能编辑可以这样解决
  • 原文地址：https://blog.csdn.net/qqGHJ/article/details/127397326