GAAS 无人机自动驾驶学习（02-通过机载摄像头构建建筑物 3D 模型）

原文网址：https://gaas.gitbook.io/guide/wu-ren-ji-zi-dong-jia-shi-xi-lie-tong-guo-wu-ren-ji-ji-zai-she-xiang-tou-gou-jian-jian-zhu-wu-3d-mo

无人机自动驾驶系列 Part 2 : 通过无人机机载摄像头构建建筑物 3D 模型

上一讲(01)里介绍了如何通过 MAVROS 以及 OFFBOARD 模式在 Gazebo 模拟器中控制无人机飞行。

本讲中，我们将会介绍一个真实世界的无人机使用案例: 通过无人机机载摄像头构建建筑物3D模型。在接下来的部分里，会讲解如何基于上一讲的内容实现这一目的。以及如何通过上一课中用到的 API 以及本课提供的建模网站(完全免费) 来实现这一任务。

再次提醒：本教程在虚拟机环境下（VMWare，VirtualBox等）无法完成。

实现这一任务包括一下5个步骤：

1. 启动 Gazebo 模拟器；

2. 起飞无人机并开始录制一个 rosbag;

3. 控制无人机围绕建筑物绕圆周飞行一圈；

4. 停止 rosbag 录制并使用提供的脚本提取图片；

5. 使用提供的网站构建模型（构建模型所需环境配置过于复杂，之后的教程会详细讲解）。

前两步比较直接也很简单，对于第三步和第四步，我们会提供一个 python 脚本来辅助你来完成，但是实现并不是最好的。对于第五步，我们会在最后一部分讲解如何使用提供的网站来实现模型构建。

如果你想控制实机飞行并且构造一个实际环境的 3D 模型:

1. 使用遥控器控制无人机起飞 或者通过提供的 Python API 控制无人机起飞（因安全问题，暂时不推荐）;

2. 开始录制 Rosbag;

3. 控制无人机围绕建筑物飞行;

4. 将无人机降落然后处理 Rosbag;

5. 将图片上传到提供的网站上建模。

 

接下来我们将从头开始讲解在 Gazebo 模拟器中构建模型，最终得到的结果如下图所示。

                                                                                              Fig 1, 3D 模型

 

001. 更新环境

开始前，记得更新 GAAS:

# 改变路径到 GAAS 所在路径，使用 git pull 更新 GAAS
cd (GAAS_PATH)
git pull origin master

接下来，将 Gazebo 仿真用到的文件拷贝到 PX4 文件所在目录中:

cp -r (GAAS_PATH)/simulator/launch/* ~/catkin_ws/src/Firmware/launch/
cp -r (GAAS_PATH)/simulator/models/* ~/catkin_ws/src/Firmware/Tools/sitl_gazebo/models/
cp -r (GAAS_PATH)/simulator/worlds/* ~/catkin_ws/src/Firmware/Tools/sitl_gazebo/worlds/
cp -r (GAAS_PATH)/simulator/posix-config/* ~/catkin_ws/src/Firmware/posix-configs/SITL/init/ekf2/

// 我的ubuntu环境是如下，一般按镜像安装的GAAS_PATH地址是一样的：

cp -r /home/gaas_folder/GAAS/simulator/launch/* ~/catkin_ws/src/Firmware/launch/
cp -r /home/gaas_folder/GAAS/simulator/models/* ~/catkin_ws/src/Firmware/Tools/sitl_gazebo/models/
cp -r /home/gaas_folder/GAAS/simulator/worlds/* ~/catkin_ws/src/Firmware/Tools/sitl_gazebo/worlds/
cp -r /home/gaas_folder/GAAS/simulator/posix-config/* ~/catkin_ws/src/Firmware/posix-configs/SITL/init/ekf2/

完成后，你可以继续到下一步.

002. 启动仿真

如果你一步一步的做完了第一讲并且更新了本地环境，你可以直接执行下列命令来启动 Gazebo 仿真环境:

roslaunch px4 sfm.launch

上述命令会一同启动 MAVROS 以及 Gazebo 仿真，启动后，你会发现你的无人机以及一栋房子出现在模拟器环境中，这栋房子将会是你建模的目标。

可能出现如下error

.....
[Err] [InsertModelWidget.cc:298] Missing model.config for model "/root/catkin_ws/src/Firmware/Tools/sitl_gazebo/cmake"
[Err] [InsertModelWidget.cc:298] Missing model.config for model "/root/catkin_ws/src/Firmware/Tools/sitl_gazebo/external"
[Err] [InsertModelWidget.cc:298] Missing model.config for model "/root/catkin_ws/src/Firmware/Tools/sitl_gazebo/include"
[Err] [InsertModelWidget.cc:298] Missing model.config for model "/root/catkin_ws/src/Firmware/Tools/sitl_gazebo/models"
[Err] [InsertModelWidget.cc:298] Missing model.config for model "/root/catkin_ws/src/Firmware/Tools/sitl_gazebo/msgs"
[Err] [InsertModelWidget.cc:298] Missing model.config for model "/root/catkin_ws/src/Firmware/Tools/sitl_gazebo/scripts"
[Err] [InsertModelWidget.cc:298] Missing model.config for model "/root/catkin_ws/src/Firmware/Tools/sitl_gazebo/src"
[Err] [InsertModelWidget.cc:298] Missing model.config for model "/root/catkin_ws/src/Firmware/Tools/sitl_gazebo/worlds"
......

如果执行命令后Gazebo窗口不能正常开启且terminal中提示 "Getting models from ...", 你可以下载 https://bitbucket.org/osrf/gazebo_models/get/e6d645674e8a.zip 并将解压后的文件复制到如下文件夹 (如果没有models文件夹需要手动新建):

/home/your-user-name/.gazebo/models

#这里我的用户名是chen，进入/home/chen，ctrl+h没有发现.gazebo,于是自己建了mkdir .gazebo/models

如果上面这个链接无法下载的话（有用户反馈 18.04 无法下载），可以通过这里下载：https://drive.google.com/open?id=1dvkt3PKjUs4pXdo-5N3IMo11tk9ODVfJ

之后，gazebo启动：

在你进行下一步之前，记得通过如下命令检查 MAVROS 的连接情况:

# 确保 "connected" 为 "True"
rostopic echo /mavros/state

当你启动仿真后，IMU，气压计，摄像头图像信息以及 GPS 等传感器信息会通过 MAVROS 发布出来; 你可以通过如下命令来查看可用的 ROS 主题:

rostopic list

通过 MAVROS 发布出来的主题包括:

/clock
/diagnostics
/gazebo/link_states
/gazebo/model_states
/gazebo/parameter_descriptions
/gazebo/parameter_updates
/gazebo/set_link_state
/gazebo/set_model_state
/gazebo_gui/parameter_descriptions
/gazebo_gui/parameter_updates
/gi/simulation/left/camera_info
/gi/simulation/left/image_raw
......

而我们用来构建 3D 模型的主题只是左(或者右)摄像头信息:

/gi/simulation/left/image_raw

你可以通过 Rviz 来查看发布出来的摄像头图片:

rviz

在 Terminal 中输入 rviz 后你可以看到一个如下的窗口:

                                                                                 Fig 2, RVIZ

在窗口左下角，选择 “Add”->“by topic”->”/gi/simulation/left/image_raw“， 之后你可以看到当前的左目摄像头信息。我们接下来会在 Gazebo 环境中构建前方建筑物的 3D 模型。

                                                                                                   Fig 3, 选择图像topic

                                                                                           Fig 4, rviz窗口中的图像展示

 

003.录制一个 Rosbag

若要生成一个 3D 模型，我们需要围绕建筑物或者环境的一组图片，而这些图片需要是从建筑物不同角度拍摄的连续图片（连续图片间需要有关于建筑物的共视区) 。Rosbag 类似于一个盒子，可以将ROS/MAVROS 发布出来的消息保存在内, 所以 Rosbag 非常适合保存摄像机发布出来的图像信息。

因为我们将使用左目摄像头，我们只需要订阅并存储左目摄像头的信息:

rosbag record /gi/simulation/left/image_raw -O sfm.bag

接下来，起飞无人机:

python  (GAAS_PATH)/demo/tutorial_2/2_Struction_from_Motion/px4_mavros_run.py

#我的环境命令是：
python /home/gaas_folder/GAAS/demo/tutorial_2/2_Struction_from_Motion/px4_mavros_run.py

无人机会起飞到 3 米高左右。接下来我们可以控制无人机围绕建筑物飞行，同时我们需要注意在控制无人机飞行的过程中，我们需要保持无人机的头部，也就是摄像头正前指向建筑物, 为此我们提供了一个脚本控制无人机围绕圆形飞行:

python (GAAS_PATH)/demo/tutorial_2/fly_circle.py

#我的环境地址是：
python /home/gaas_folder/GAAS/demo/tutorial_2/2_Struction_from_Motion/fly_circle.py

无人机将会围绕建筑物飞行一圈，最终它将会降落在起飞点的地面上。

在录制 bag 过程中，若要构建一个比较好的模型，我们需要时刻注意一下几点:

1. 围绕建筑物拍摄的连续图片需要有一定视野上的重叠;

2. 避免幅度较大的 Yaw 运动;

3. 尽量将建筑物的表面都拍摄下来。

无人机降落后，停止 Rosbag 的录制，你会得到一个名为 “sfm.bag” 的 Rosbag， 接下来，我们将把 Rosbag 处理成一系列图片。

 

003. 处理 Rosbag

为了构建 3D 模型，我们需要将 rosbag 中的图片信息提取出来，你可以使用 tutorial_2 提供的 python 脚本来实现这个功能。

python bag2image.py --bag (PATH-TO-YOUR-BAG) --output_path (IMAGE-OUTPUT-FOLDER) --image_topic /gi/simulation/left/image_raw

#output_path: place_where_you_want_to_save_images

#这里我的环境下：
python bag2image.py --bag /home/chen/sfm.bag.active --output_path /home/gaas_folder/GAAS/map1 --image_topic /gi/simulation/left/image_raw

这里需要进一步测试，因为发现在自己/home/chen目录下生成了一个sfm.bag.active文件（大约有五十多个G大小，惊呆了），然后提取图片，发现没有成功：

bag file:  /home/chen/sfm.bag.active
output image path:  /home/gaas_folder/GAAS/map1
output image path:  /gi/simulation/left/image_raw
Traceback (most recent call last):
  File "bag2image.py", line 38, in 
    bag2image()
  File "bag2image.py", line 24, in bag2image
    bag = rosbag.Bag(args.bag, "r")
  File "/opt/ros/kinetic/lib/python2.7/dist-packages/rosbag/bag.py", line 173, in __init__
    self._open(f, mode, allow_unindexed)
  File "/opt/ros/kinetic/lib/python2.7/dist-packages/rosbag/bag.py", line 1102, in _open
    if   mode == 'r': self._open_read(f, allow_unindexed)
  File "/opt/ros/kinetic/lib/python2.7/dist-packages/rosbag/bag.py", line 1138, in _open_read
    self._reader.start_reading()
  File "/opt/ros/kinetic/lib/python2.7/dist-packages/rosbag/bag.py", line 2312, in start_reading
    raise ROSBagUnindexedException()

不知道是电脑内存不足还是神码问题，但是感觉又不像。。。。。。待解决(已解决)

！！！！这里找到原因了！！！吃完饭回来，一想不对呀，保存的sfm.bag.active文件注意最后后缀，不就是表示还在采集数据过程中，也就是为什么那个文件这莫大，原来是飞机飞行一圈降落后自己没有关闭保存数据的那个命令终端，所以一直在采集数据过程中。这次，飞机飞完一圈降落，把record bag的那个终端关掉，保存的文件为sfm.bag。

I‘m too fool and too simple!

打开新终端重新运行：

python bag2image.py --bag /home/chen/sfm.bag --output_path /home/gaas_folder/GAAS/map1 --image_topic /gi/simulation/left/image_raw

#注意是在bag2image.py脚本目录下运行

转换之后大概有两千多张图片，我保存的位置是/home/gaas_folder/GAAS/map1文件夹下。

 

成功完成上一步后，你会将所有的图片从 rosbag 提取出来并且存储到本地文件夹内。

注意：请控制上传图片的数量；小场景建模的图片通常不超过 100 张（你可能需要进行图片筛选）。确保每张图片有 “重叠”，并尽量覆盖物体全部。

 

004. 构造模型

3D 建模的环境配置比较复杂，为此我们提供了一个网站来帮助建模。通过网站即可构建模型，你所需要做的只是将图片上传到如下网站：

http://aviation.giai.tech/

                                                                                          Fig 5, 3D 建模网站首页

点击 “构建自己的模型” 后，点击 “Add FIles” 选择物体的图片（不支持批量上传），之后点击提交。

下图是真实世界的一个使用案例，通过 76 张图片构建出来的的3D模型。

                                              Fig6, 吉林大学南岭校区，飞手控制无人机飞行并采集76张图片后的建模结果

 

将图片上传到网站后，因为计算能力限制，您需要等待一段时候后建模才可以完成。建模完成后，你会收到一封邮件，根据邮件提示即可得到模型。