用ArUco定位代替GPS

原文：Using Vision or Motion Capture Systems

普通单目相机调用 Aruco Library，源代码如下：

#include <opencv2/highgui.hpp>
#include "aruco.h"
using namespace std;

int main(int argc,char **argv)
{
    aruco::CameraParameters camera;
    camera.readFromXMLFile(argv[1]);
    aruco::MarkerDetector Detector;
    Detector.setDictionary("ARUCO_MIP_36h12");

    
    cv::namedWindow( "ArUco-Pose-Estimation", cv::WINDOW_AUTOSIZE );
    cv::VideoCapture cap;
    cap.open(0); // open the first camera
    if( !cap.isOpened() ) 
    { // check if we succeeded
        std::cerr << "Couldn't open capture." << std::endl;
        return -1;
    }

    cv::Mat im;
    for (;;)
    {
        cap >> im;
        if ( im.empty() )
            break; // Ran out of film

        auto markers=Detector.detect(im,camera,0.04);//0.102 is the marker size
        for ( auto m:markers )
        {
            aruco::CvDrawingUtils::draw3dAxis(im,m,camera);
            // cout<<m.Rvec<<" "<<m.Tvec<<endl;
            cout<<m.Tvec<<endl;
        }
        cv::imshow("ArUco-Pose-Estimation",im);
        if ( (char) cv::waitKey(33) >= 0 )
            break;
    }
}

MYNTEYE 相机调用 Aruco Library，源代码如下：

#include <opencv2/highgui.hpp>
#include "aruco.h"
#include <ctime>
#include <iomanip>
#include <iostream>
#include "camera.h"
#include "utility.h"

using namespace std;
using namespace mynteye;

int main(int argc,char **argv)
{
    aruco::CameraParameters camera;
    camera.readFromXMLFile(argv[1]);
    aruco::MarkerDetector Detector;
    Detector.setDictionary("ARUCO_MIP_36h12");
    cv::namedWindow( "ArUco-Pose-Estimation", cv::WINDOW_AUTOSIZE );

    std::string name;
    name = "MYNTEYE";
    cout << "Open Camera: " << name << endl;
    Camera cam;
    InitParameters params(name);
    cam.Open(params);
    if (!cam.IsOpened()) {
        std::cerr << "Error: Open camera failed" << std::endl;
        return 1;
    }

    ErrorCode code;
    cv::Mat img_left;

    for (;;)
    {
        code = cam.Grab();
        if (code != ErrorCode::SUCCESS) continue;
        if (cam.RetrieveImage(img_left, View::VIEW_LEFT_UNRECTIFIED) == ErrorCode::SUCCESS) 
        {
            auto markers=Detector.detect(img_left,camera,0.102);//0.102 is the marker size
            for ( auto m:markers )
            {
                aruco::CvDrawingUtils::draw3dAxis(img_left,m,camera);
                // cout<<m.Rvec<<" "<<m.Tvec<<endl;
                cout<<m.Tvec<<"========================="<<endl;
            }
            cv::imshow("ArUco-Pose-Estimation",img_left);
            if ( (char) cv::waitKey(33) >= 0 ) break;
        }
    }
}

全局快门效果优于卷帘快门。但是实验中发现 ArUco 还是不够稳定，相机快速抖动时会导致检测 ArUco 失败。How to Solve it?

本文旨在使用来自GPS以外的其他来源的位置数据。位置估计既可以从机载（onboard）计算机也可以从机外（offboard）计算机发送。该数据用于更新相对于本地原点的本地位置估计。该系统可用于诸如室内定位或基于视觉的航点导航等应用。对于视觉，用于发送姿势数据的MAVLink消息是vision_pose_estimate。mavros ROS-MAVLink接口具有发送这些消息的默认实现。它们也可以使用纯C/C++代码发送，并直接使用MAVLink库。ROS主题是：mocap_pose_estimate用于mocap系统，vision_pose_estimate用于视觉。

此功能仅经过测试才能与LPE估算器配合使用。

用于视觉的LPE调整

启用外部姿势输入

禁用气压计融合

调整噪声参数

NED和ENU参考系

NED和ENU

使用Mavros

使用MAVROS，此操作非常简单。ROS使用ENU框架作为惯例，因此必须在ENU中提供位置反馈。如果您有Optitrack系统，则可以使用mocap_optitrack节点，该节点将对象姿势流传输到ENU中已有的ROS主题上。通过重映射，您可以直接在mocap_pose_estimate发布，而不进行任何转换，并且mavros将负责NED转换。