用ArUco定位代替GPS

Jul 9, 2018原文：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转换。