ardupilot开发 --- 机载(边缘)计算机-VISP 篇

1. 一些概念

• 什么是VISP
  VISP即Visual servoing platform.
  Allows to control a robot equipped with a camera from measures extracted from the images.
• 实现无人机机载端的飞行控制，机器人运动控制。
• 实现实时目标检测。
• 实现实时位姿估计。
• 特点
  • 集成其他算法很便捷
  • 跨平台，源码为C++
  • 完备的文档
• 模块化的软件架构
  下图突出显示了模块依赖项和每个模块可能使用的第三方库。中央模块是核心模块core。所有其他模块都依赖于core。
  
• 模块 概述
  • core
    定义基本数据结构（图像、矩阵、相机）的模块。此模块由所有其他模块使用。为了启用某些功能，此模块可能会使用可选的第三方库。
  • io
    管理图像或视频的 I/O、命令行解析。
  • gui
    在屏幕窗口上显示图像，实时绘制变量的演变过程。
  • ar
    ar 即 augmented reality ，增强现实。
  • detection
    条形码和人脸检测。
    目标检测。
  • robot
    真实机器人或仿真机器人接口。
    真实：Afma4, Afma6, Viper, Pioneer, Panda from Franka Emika
    仿真：free flying camera, Afma6, Viper, Pioneer
  • imgproc
    提供图像处理算法。
  • sensor
    提供摄像头、RGB-D传感器、激光传感器接口。
    如：usb相机，Kinect相机，Sick雷达。
    需要调用第三方库。
  • vision
    相机标定、图像特征点检测、特征点匹配、位姿估计等。
    基本通过调用opencv实现。
  • visual_feat
    定义视觉特征，用于视觉伺服。
  • vs
    实现视觉伺服功能。
  • 下面介绍的模块都用于跟踪功能：
  • blob
    实现blob跟踪功能
  • me
    moving-edges跟踪
  • klt
    Kanade-Lucas-Tomasi特征跟踪
  • mbt
    markerless model-based跟踪
  • tt
    基于SSD 和 ZNCC 的模版跟踪
  • tt_mi
    tt模块的扩展

2. 支持的硬件

机器人、相机、雷达、力传感器、触觉传感器、动作捕捉器、mavlink系统 …
参考：Supported Hardware

3. 支持的第三方库

如OpenCV、PCL、MavSDK/MavLink …
参考：Supported Third-Party Libraries

4. 安装

参考：Installation
环境：Ubuntu-20.04

4.1 快速安装VISP

以编译源码的方式安装visp；
仅安装部分必要的第三方库，可选的第三方库可在后续使用过程中再安装；

• 1）创建工作目录

echo "export VISP_WS=$HOME/visp-ws" >> ~/.bashrc
source ~/.bashrc
mkdir -p $VISP_WS

• 2）安装部分必要的第三方库
  openCV 和 mavsdk 对于ardupilot无人机来说是必须的，注意mavsdk要安装v1.4的版本，否则VISP编译会报错。
  安装mavsdk，请参考 MAVSDK篇 或 pixhawk_prereq_software。
  安装openCV ，命令如下：

# Ubuntu 20.04 ：
sudo apt-get install libopencv-dev libx11-dev liblapack-dev libeigen3-dev libv4l-dev libzbar-dev libpthread-stubs0-dev libdc1394-dev nlohmann-json3-dev
# older Ubuntu distros ：
sudo apt-get install libopencv-dev libx11-dev liblapack-dev libeigen3-dev libv4l-dev libzbar-dev libpthread-stubs0-dev libdc1394-22-dev

• 3）获取VISP源码

cd ~/visp-ws
git clone https://github.com/lagadic/visp.git

• 4）编译

mkdir -p ~/visp-ws/visp-build
cd ~/visp-ws/visp-build
cmake ../visp
make -j$(nproc)

• 5）设置环境变量 VISP_DIR，即设置一个visp安装路径的环境变量

echo "export VISP_DIR=$VISP_WS/visp-build" >> ~/.bashrc
source ~/.bashrc

4.2 第三方库的(可选)安装

如果发现缺少第三方库，安装它，然后重新配置VISP并重新编译。

• 1）安装第三方库
  参考1：Supported Third-Party Libraries
  参考2：Advanced ViSP installation
  参考3：pixhawk_prereq_software

• 2）重新配置VISP

cd ~/visp-ws/visp-build
cmake ../visp

查看新添加的第三方库是否被配置进~/visp-ws/ViSP-third-party.txt中：请参考

• 3）重新编译

cd ~/visp-ws/visp-build
make -j$(nproc)

nproc是操作系统级别对每个用户创建的进程数的限制。

5. 下载测试数据

文档：Install ViSP data set
一些ViSP示例和测试需要一个数据集，包括图像、视频和模型。

• 下载途径：
  • https://github.com/lagadic/visp-images
  • https://visp.inria.fr/download
• 通过下载压缩包visp-images-3.6.0.zip的方式下载数据集
  Download visp-images-3.6.0.zip from https://visp.inria.fr/download and uncompress it in your workspace
• 通过Git下载数据集

cd $VISP_WS
git clone https://github.com/lagadic/visp-images.git

设置环境变量VISP_INPUT_IMAGE_PATH以便运行example：

echo "export VISP_INPUT_IMAGE_PATH=$VISP_WS/visp-images" >> ~/.bashrc
source ~/.bashrc

• 测试

cd $VISP_WS/visp-build
./example/device/display/displayX

点击图片结束测试

6. tips

参考：Tips and tricks

包括：

• 如何卸载ViSP ？

cd $VISP_WS/visp-build
sudo make uninstall

• 如何安装VISP ？
• How to take into account a newly installed 3rd party ？
• Which are the targets that could be run with make ?

make help | grep visp

• How to build a ViSP specific module ？
• Which are the 3rd party libraries that are used in ViSP ?

7. 如何在应用程序中使用(引用)VISP？

windows应用程序、cmake、eclipse ide：请参考文档
类 unix 系统包括OSX、Fedora、Ubuntu、Debian …
以 Ubuntu 为例：

• 在项目中使用ViSP的最简单方法是使用CMake。如果不熟悉CMake，可以查看cmake篇或cmake文档。
• 本教程中描述的所有材料（源代码和图像）都是ViSP源代码的一部分，可以使用以下命令下载：$ svn export https://github.com/lagadic/visp.git/trunk/tutorial/image
• 1）编译示例程序

cd $VISP_WS/visp-build
# 编译全部
cmake ../visp
make -j$(nproc)
# 仅编译某个示例程序tutorial-viewer
cmake ../visp
make tutorial-viewer

• 2）执行示例程序
  可执行程序在：~/visp-ws/visp-build/tutorial/image/tutorial/image/tutorial-viewer
  示例程序源代码在：~/visp-ws/visp/tutorial/image/tutorial-viewer.cpp

cd $VISP_WS/visp-build/tutorial/image
./tutorial-viewer monkey.pgm

• 参考：https://visp-doc.inria.fr/doxygen/visp-daily/tutorial-getting-started.html

8. 帮助文档

源码中的文档也很赞：
visp-ws/visp/doc

8.1 基本的矩阵向量运算

• 本教程的重点是基本的线性代数运算，如向量和矩阵乘法。
• 参考文献：Tutorial: Basic linear algebra operations
• 实现在 vpMatrix 类中
• 矩阵、向量运算函数
  • 矩阵相乘
    vpMatrix::mult2Matrices(const vpMatrix &, const vpMatrix &, vpMatrix &)
    vpMatrix::operator*(const vpMatrix &) const
  • 矩阵乘以向量
    vpMatrix::multMatrixVector(const vpMatrix &, const vpColVector &, vpColVector &)
    vpMatrix::mult2Matrices(const vpMatrix &, const vpColVector &, vpColVector &)
    vpMatrix::operator*(const vpColVector &) const
  • 矩阵本身与转置相乘A * AT、AT * A
    vpMatrix::AtA()
    vpMatrix::AtA(vpMatrix &) const
    vpMatrix::AAt()
    vpMatrix::AAt(vpMatrix &) const
• 矩阵、向量运算需要调用的第三方库，如MKL、OpenBLAS 等，最好安装，可以提升计算性能。如果没有安装这些第三方，ViSP将提供一个未经优化的Lapack内置版本，矩阵运算性能远不如前者。
• 待续…

8.2 图像操作

包括：读取、滤波、渲染…
参考：Image manipulation

8.2.1 在图像显示窗口绘制图像

• visp gui模块提供图形用户界面功能，允许在窗口中显示vpImage。为此，您可以使用几个可选的第三方库，它们是：X11、GDI、OpenCV、GTK、Direct3D。我们建议在类unix系统上使用X11，即使用vpDisplayX类，在Windows上使用GDI，即使用类vpDisplayGDI。如果这些类都不可用，则可以使用vpDisplayOpenCV。
• 以tutorial-image-display.cpp为例，
  它显示了如何创建一个3840x2160的灰度图像，将所有像素值(灰度值)设置为128，并在图像中间显示一个半径为200像素的红色圆圈：

//! \example tutorial-image-display.cpp
#include <visp3/gui/vpDisplayGDI.h>
#include <visp3/gui/vpDisplayX.h>int main()
{vpImage<unsigned char> I(2160, 3840, 128);try {
#if defined(VISP_HAVE_X11)vpDisplayX d(I);
#elif defined(VISP_HAVE_GDI)vpDisplayGDI d(I);
#endifvpDisplay::setTitle(I, "My image");vpDisplay::display(I);vpDisplay::displayCircle(I, I.getHeight() / 2, I.getWidth() / 2, 200, vpColor::red, true);vpDisplay::flush(I);std::cout << "A click to quit..." << std::endl;vpDisplay::getClick(I);} catch (const vpException &e) {std::cout << "Catch an exception: " << e.getMessage() << std::endl;}
}

• vpImage只能与一个显示窗口关联。在前面的例子中，图像I与显示d相关联。
• 图像显示的缩放
  应用于显示大于屏幕分辨率的图像
  修改代码：

5倍缩放：

#if defined(VISP_HAVE_X11)vpDisplayX d(I, vpDisplay::SCALE_5);
#elif defined(VISP_HAVE_GDI)vpDisplayGDI d(I, vpDisplay::SCALE_5);
#endif

自动缩放：

#if defined(VISP_HAVE_X11)vpDisplayX d(I, vpDisplay::SCALE_AUTO);
#elif defined(VISP_HAVE_GDI)vpDisplayGDI d(I, vpDisplay::SCALE_AUTO);
#endif

或：

#if defined(VISP_HAVE_X11)vpDisplayX d;
#elif defined(VISP_HAVE_GDI)vpDisplayGDI d;
#endifd.setDownScalingFactor(vpDisplay::SCALE_AUTO);d.init(I);
}

8.2.2 在图像显示窗口加载、显示、绘制图像

参考：Tutorial: How to display an image and basic drawings in a window
涉及到的第三方库：X11、GDI、OpenCV、GTK、Direct3D。至少使用其中之一。

• 显示本地路径的图片
  
• 显示基本图形
  
• 在图像上绘制一个点（覆盖式绘制）
• 绘制两个点之间的直线（覆盖式绘制）
• 绘制矩形、圆形、十字、文字（覆盖式绘制）
• 导出并保存绘制的内容
• 处理窗口中的键盘事件

8.2.2 无显示窗口的情况下在图像上添加基本图形

参考：Tutorial: How to modify an image to insert basic drawings
在不需要图像显示窗口的情况下通过添加基本图形来修改图像的内容。
无需使用第三方库：X11、GDI、OpenCV、GTK、Direct3D。
vpImageDraw类允许通过插入点、圆、线、矩形、多边形、框架等基本图形来修改图像。还有vpFont类，它允许修改图像以插入文本。这些类在testImageDraw.cpp中使用。

8.2.3 在视频流或相机中获取图像祯

参考：Tutorial: Image frame grabbing
只有安装了相应的第三方，才能从相机、视频流中获取图像。需要根据相机型号、品牌、视频流来源来安装对应的第三方库。

• FlyCapture SDK
  例子：tutorial-grabber-flycapture.cpp
  FlyCapture SDK适配以下相机类型：
  • Flea3 USB 3.0 cameras (FL3-U3-32S2M-CS, FL3-U3-13E4C-C)
    菲力尔品牌的usb相机
  • Flea2 firewire camera (FL2-03S2C)
  • Dragonfly2 firewire camera (DR2-COL)
  • GigE PGR cameras
• libdc1394 SDK
  firewire or USB3 camera under Unix
  tutorial-grabber-1394.cpp
• libv4l2 SDK
  usb camera under Unix
  tutorial-grabber-v4l2.cpp
• Pylon SDK
  Basler cameras
  tutorial-grabber-basler-pylon.cpp
• Realsense SDK
  Realsense RGB-D cameras
  tutorial-grabber-realsense.cpp
  tutorial-grabber-multiple-realsense.cpp
• Occipital Structure SDK
  Occipital公司的Structure Core RGB-D camera，如第二代Structure Sensor：Mark II
  tutorial-grabber-structure-core.cpp
• 对比 RealSense D435 相机 和 Structure Core 相机得到的RGB和深度图像
  tutorial-grabber-rgbd-D435-structurecore.cpp
  
• 从OpenCV获取图像祯
  参考：Frame grabbing using OpenCV
  例子：tutorial-grabber-opencv.cpp
  如何通过openCV获取https、rtsp视频流？？
  1）找到调用opencv的地方，可知_cap是opencv类的对象：
  cv::VideoCapture _cap;
  2）由 this->_cap.open(this->_camera_id, cv::CAP_V4L2); 可知打开视频的函数是open
  3）F12 进入 cv::VideoCapture 找到 open 函数的定义：
  CV_WRAP virtual bool open(const String& filename, int apiPreference = CAP_ANY);
  4）F12 进入 CAP_ANY 可知openCV函数支持以下视频源输入：

enum VideoCaptureAPIs {CAP_ANY          = 0,            //!< Auto detect == 0CAP_VFW          = 200,          //!< Video For Windows (obsolete, removed)CAP_V4L          = 200,          //!< V4L/V4L2 capturing supportCAP_V4L2         = CAP_V4L,      //!< Same as CAP_V4LCAP_FIREWIRE     = 300,          //!< IEEE 1394 driversCAP_FIREWARE     = CAP_FIREWIRE, //!< Same value as CAP_FIREWIRECAP_IEEE1394     = CAP_FIREWIRE, //!< Same value as CAP_FIREWIRECAP_DC1394       = CAP_FIREWIRE, //!< Same value as CAP_FIREWIRECAP_CMU1394      = CAP_FIREWIRE, //!< Same value as CAP_FIREWIRECAP_QT           = 500,          //!< QuickTime (obsolete, removed)CAP_UNICAP       = 600,          //!< Unicap drivers (obsolete, removed)CAP_DSHOW        = 700,          //!< DirectShow (via videoInput)CAP_PVAPI        = 800,          //!< PvAPI, Prosilica GigE SDKCAP_OPENNI       = 900,          //!< OpenNI (for Kinect)CAP_OPENNI_ASUS  = 910,          //!< OpenNI (for Asus Xtion)CAP_ANDROID      = 1000,         //!< Android - not usedCAP_XIAPI        = 1100,         //!< XIMEA Camera APICAP_AVFOUNDATION = 1200,         //!< AVFoundation framework for iOS (OS X Lion will have the same API)CAP_GIGANETIX    = 1300,         //!< Smartek Giganetix GigEVisionSDKCAP_MSMF         = 1400,         //!< Microsoft Media Foundation (via videoInput)CAP_WINRT        = 1410,         //!< Microsoft Windows Runtime using Media FoundationCAP_INTELPERC    = 1500,         //!< RealSense (former Intel Perceptual Computing SDK)CAP_REALSENSE    = 1500,         //!< Synonym for CAP_INTELPERCCAP_OPENNI2      = 1600,         //!< OpenNI2 (for Kinect)CAP_OPENNI2_ASUS = 1610,         //!< OpenNI2 (for Asus Xtion and Occipital Structure sensors)CAP_GPHOTO2      = 1700,         //!< gPhoto2 connectionCAP_GSTREAMER    = 1800,         //!< GStreamerCAP_FFMPEG       = 1900,         //!< Open and record video file or stream using the FFMPEG libraryCAP_IMAGES       = 2000,         //!< OpenCV Image Sequence (e.g. img_%02d.jpg)CAP_ARAVIS       = 2100,         //!< Aravis SDKCAP_OPENCV_MJPEG = 2200,         //!< Built-in OpenCV MotionJPEG codecCAP_INTEL_MFX    = 2300,         //!< Intel MediaSDKCAP_XINE         = 2400,         //!< XINE engine (Linux)};

综上可知，枚举中的 CAP_GSTREAMER 类型就可以打开https、rtsp类型的视频源（前提是必须安装gstreamer第三方库），或者干脆设置为 CAP_ANY 类型可实现自动检测！！

• CMU1394 SDK
  a firewire camera under Windows
  tutorial-grabber-CMU1394.cpp
• Parrot Bebop 2 drone
  tutorial-grabber-bebop2.cpp
• 从视频流获取图像祯
  只支持本地视频流，不支持https、rtsp等网络视频流，网络视频流请移步OpenCV；
  支持*.avi, *.mp4, *.mov, *.ogv, *.flv 等格式；
  要求安装第三方库OpenCV；
  例子：tutorial-video-reader.cpp
  参考：Images from a video stream