一、ArUco之Marker Mapper
1、Marker Mapper简介
Mapping and Localization from Planar Markers是A.V.A小组基于ArUco开发的一个利用二维码建图与定位的项目。
论文地址：Mapping and Localization from Planar Markers
源码地址：https://sourceforge.net/projects/markermapper/files/?source=navbar （源码持续更新）
2、利用MarkerMapper实现建图
（1）编译源码
我们从源码地址下载当前最新的1.0.12版本，解压编译：
$ unzip marker_mapper1.0.12.zip
$ cd marker_mapper1.0.12
$ mkdir build
$ cd build
$ cmake ..
$ make
如果编译出错，打开marker_mapper1.0.12文件夹下的CMakeLists.txt，做如下修改：
修改完以后重新编译即可：$ cmake ..; make
（2）用自己的摄像头建图
先找到你之前标定相机生成的yml文件，将其拷贝到 build/utils文件夹下，然后在此文件夹下运行命令$ ./mapper_from_video，结果如下：
解释一下这几个参数：
- (live | intput-video.avi[:v2.avi[:v3.avi…]])：live表示采用摄像头设备，但这里只能打开设备号为0的相机设备，如果你用笔记本想打开USB摄像头的话需要改一下源码。具体怎么改我就不说了，相信这点小问题肯定难不倒大家。后面input-video.avi表示读取视频文件，你需要事先录取一段视频，然后最好也放到 build/utils文件夹下。
- camera_parameters.yml：相机的标定文件。
- marker_size(meters)：二维码的边长（以米为单位）。请务必保证所有二维码的边长一致，不要出现大小不一的二维码。
- [-ref reference_marker_id]：参考二维码的id。在建图过程中需要选定一个二维码作为基准，其他二维码的位置均是相对于这个二维码来说的。即以参考二维码的坐标系作为固定的世界坐标系。缺省条件下以第一个被识别的二维码为基准。
- [-fi frameIncrement]：帧增量，缺省值为1。
- [-out basename]：程序输出使用的基本名称。程序会以“basename”生成basename.pcd、basename.log、basename.yml、basename-cam.yml四个文件。缺省值为“markerset”。
- [-d maker_dictionary] [-c arucoConfig.yml]：两者二选一，指定二维码的字典或指定一个二维码的配置文件。-d的缺省值是ALL_DICTS，表示可以识别ArUco库内所有字典中的二维码。
- -noshow：这个参数没有在Usage中体现出来，它表示在程序最后不启动MapperViewer。
根据上面对每个参数的解释，我们可以根据自己的实际情况输入合适的命令行参数，以我的为例：
$ ./mapper_from_video live out_camera_calibration.yml 0.135 -ref 10 -d ARUCO -noshow
运行效果如下：
我把二维码的前三个角点按照黄、绿、蓝的顺序标了出来，你的程序中应该不会有这个效果，因为我改了ArUco的源码。程序运行时，为确保建图的精度，相机移动不宜过快，且应尽量保证在运动过程中每一帧图像至少有两个二维码。按’s’键暂停，'ESC’键退出。程序运行结束后，你会发现文件夹中多了四个文件：
- markerset.log：记录了关键帧的相机位姿，数据格式为：#frame_id tx ty tz qx qy qz qw。
- markerset.pcd：二维码分布及相机关键帧的点云图。
- markerset.yml：二维码的分布文件，这个文件描述了所有二维码相对于参考二维码的空间位姿。以参考二维码的中心为基准，以该二维码的坐标系为世界坐标系，所有二维码角点的三维空间坐标便确定了。
- markerset-cam.yml：优化以后的相机标定文件。
我们用PCL看一下生成的点云图，运行命令$ pcl_viewer markerset.pcd：
如上图所示，我在实验室墙壁上贴了20张ARUCO字典中的marker，marker的ID从0到19。点云图中的marker分布完全重现了真实物理环境中的二维码分布。
（3）用二维码分布图实现室内定位
用MarkerMapper我们得到了二维码分布图，即我们知道了所有二维码之间的相对位姿关系。近一步说，我们以参考二维码的坐标系作为世界坐标系，我们便知道了每个二维码在三维空间中精确的位姿，这是非常重要的！
由上一篇我们知道，利用一个二维码可以估计相机的位姿，而现在我们有了好多二维码，并且它们的空间位姿我们也都知道（实际我们已经知道了每个二维码每个角点的三维坐标）。那么，我们是不是可以利用这些二维码来实现相机的位姿估计，进而达到室内定位的目的呢？答案是肯定的！
我们先用ArUco源码中的aruco_test_markermap测试一下看看效果，执行命令：
$ ~/OpenCV/aruco-3.0.6/build/utils_markermap/aruco_test_markermap live:1 markerset.yml markerset-cam.yml -s 0.135
不要照抄，根据你的实际情况做适当修改，运行结果如下图所示：
从图中我们可以看出，相机准确地找到了自己的位置，即很好地实现了相机的位姿估计。接下来，我就在这个程序的基础上做一些删改，按照上一篇的做法将相机在世界坐标系中的位姿数据显示出来。
- 首先，添加源文件
cd 到上一篇创建的工程目录test/文件夹下，运行命令：
$ touch cam_localization.cpp
$ gedit cam_localization.cpp
然后粘贴以下代码：
/*****************************************************/
/* By Li Xujie */
/*****************************************************/
#include "cvdrawingutils.h"
#include "aruco.h"
#include "timers.h"
#include <fstream>
#include <iostream>
#include <opencv2/opencv.hpp>
#include <sstream>
#include <string>
#include <stdexcept>
using namespace cv;
using namespace aruco;
using namespace std;
string TheMarkerMapConfigFile;
float TheMarkerSize = -1;
VideoCapture TheVideoCapturer;
Mat TheInputImage, TheInputImageCopy;
CameraParameters TheCameraParameters;
MarkerMap TheMarkerMapConfig;
MarkerDetector TheMarkerDetector;
MarkerMapPoseTracker TheMSPoseTracker;
int waitTime = 0;
int ref_id = 0;
char camPositionStr[100];
char camDirectionStr[100];
class CmdLineParser
{
int argc;char** argv;public:
CmdLineParser(int _argc, char** _argv): argc(_argc), argv(_argv){}
//is the param?
bool operator[](string param)
{int idx = -1; for (int i = 0; i < argc && idx == -1; i++)if (string(argv[i]) == param) idx = i;return (idx != -1); }
//return the value of a param using a default value if it is not present
string operator()(string param, string defvalue = "-1"){int idx = -1;for (int i = 0; i < argc && idx == -1; i++)if (string(argv[i]) == param) idx = i;if (idx == -1) return defvalue;else return (argv[idx + 1]);}
};
/************************************
************************************/
int main(int argc, char** argv)
{
try
{
CmdLineParser cml(argc, argv);
if (argc < 4 || cml["-h"])
{
cerr << "Invalid number of arguments" << endl;
cerr << "Usage: (in.avi|live[:camera_index(e.g 0 or 1)])) marksetconfig.yml camera_intrinsics.yml "
"[-s marker_size] [-ref_id reference_marker_id] [-e use EnclosedMarker or not]\n\t" << endl;
return false;
}
TheMarkerMapConfigFile = argv[2];
TheMarkerMapConfig.readFromFile(TheMarkerMapConfigFile);
TheMarkerSize = stof(cml("-s", "1"));
ref_id = std::stoi(cml("-ref_id"));
if(-1 == ref_id){cout<<"You need indicate a referene_marker by use the parameter [-ref_id].\n"<<endl;return false;}
// read from camera or from file
string TheInputVideo=string(argv[1]);
if ( TheInputVideo.find( "live")!=std::string::npos)
{
int vIdx = 0;
// check if the :idx is here
char cad[100];
if (TheInputVideo.find(":") != string::npos)
{
std::replace(TheInputVideo.begin(), TheInputVideo.end(), ':', ' ');
sscanf(TheInputVideo.c_str(), "%s %d", cad, &vIdx);
}
cout << "Opening camera index " << vIdx << endl;
TheVideoCapturer.open(vIdx);
waitTime = 10;
}
else TheVideoCapturer.open(argv[1]); // check video is open
if (!TheVideoCapturer.isOpened())throw std::runtime_error("Could not open video");
// read first image to get the dimensions
TheVideoCapturer >> TheInputImage;
// read camera parameters if passed
TheCameraParameters.readFromXMLFile(argv[3]);
TheCameraParameters.resize(TheInputImage.size());
// prepare the detector
TheMarkerDetector.setDictionary( TheMarkerMapConfig.getDictionary());
TheMarkerDetector.getParameters().detectEnclosedMarkers(std::stoi(cml("-e", "0"))); //If use enclosed marker, set -e 1(true), or set -e 0(false). Default value is 0.
if (cml["-config"])TheMarkerDetector.loadParamsFromFile(cml("-config"));
// prepare the pose tracker if possible
// if the camera parameers are avaiable, and the markerset can be expressed in meters, then go
if (TheMarkerMapConfig.isExpressedInPixels() && TheMarkerSize > 0)
TheMarkerMapConfig = TheMarkerMapConfig.convertToMeters(TheMarkerSize);
cout << "TheCameraParameters.isValid()=" << TheCameraParameters.isValid() << " "<< TheMarkerMapConfig.isExpressedInMeters() << endl;
if (TheCameraParameters.isValid() && TheMarkerMapConfig.isExpressedInMeters()){
TheMSPoseTracker.setParams(TheCameraParameters, TheMarkerMapConfig);
TheMarkerSize=cv::norm(TheMarkerMapConfig[0][0]- TheMarkerMapConfig[0][1]);
}
char key = 0;
int index = 0;
// capture until press ESC or until the end of the video
cout << "Press 's' to start/stop video" << endl;
Timer avrgTimer;
do
{
TheVideoCapturer.retrieve(TheInputImage);
TheInputImage.copyTo(TheInputImageCopy);
index++; // number of images captured
if (index>1) avrgTimer.start();//do not consider first frame that requires initialization
// Detection of the markers
vector<aruco::Marker> detected_markers = TheMarkerDetector.detect(TheInputImage,TheCameraParameters,TheMarkerSize);
// estimate 3d camera pose if possible
if (TheMSPoseTracker.isValid())
if (TheMSPoseTracker.estimatePose(detected_markers)) {
Mat rMatrix,camPosMatrix,camVecMatrix;//定义旋转矩阵，平移矩阵，相机位置矩阵和姿态矩阵
Rodrigues(TheMSPoseTracker.getRvec(),rMatrix);//获得相机坐标系与世界坐标系之间的旋转向量并转化为旋转矩阵
camPosMatrix=rMatrix.inv()*(-TheMSPoseTracker.getTvec().t());//计算出相机在世界坐标系下的三维坐标
Mat vect=(Mat_<float>(3,1)<<0,0,1);//定义相机坐标系下的单位方向向量，将其转化到世界坐标系下便可求出相机在世界坐标系中的姿态
camVecMatrix=rMatrix.inv()*vect;//计算出相机在世界坐标系下的姿态
sprintf(camPositionStr,"Camera Position: px=%f, py=%f, pz=%f", camPosMatrix.at<float>(0,0),
camPosMatrix.at<float>(1,0), camPosMatrix.at<float>(2,0));
sprintf(camDirectionStr,"Camera Direction: dx=%f, dy=%f, dz=%f", camVecMatrix.at<float>(0,0),
camVecMatrix.at<float>(1,0), camVecMatrix.at<float>(2,0));
}
if (index>1) avrgTimer.end();
cout<<"Average computing time "<<avrgTimer.getAverage()<<" ms"<<endl;
// print the markers detected that belongs to the markerset
for (auto idx : TheMarkerMapConfig.getIndices(detected_markers))
{
detected_markers[idx].draw(TheInputImageCopy, Scalar(0, 0, 255));
if(ref_id == detected_markers[idx].id) CvDrawingUtils::draw3dAxis(TheInputImageCopy,detected_markers[idx],TheCameraParameters);
}
putText(TheInputImageCopy,camPositionStr,Point(10,13),CV_FONT_NORMAL,0.5,Scalar(0,255,255));
putText(TheInputImageCopy,camDirectionStr,Point(10,30),CV_FONT_NORMAL,0.5,Scalar(0,255,255));
imshow("out",TheInputImageCopy);
key = waitKey(waitTime);
if (key=='s') waitTime = waitTime?0:10;
} while (key != 27 && TheVideoCapturer.grab());
}
catch (std::exception& ex) { cout << "Exception :" << ex.what() << endl; }
}
- 然后，修改CMakeLists.txt
在终端运行$ gedit ../CMakeLists.txt，在文档最后粘贴下面两行代码：
add_executable( cam_localization test/cam_localization.cpp )
target_link_libraries( cam_localization ${OpenCV_LIBS} ${aruco_LIBS} )
- 最后，编译执行
依次运行以下命令：
$ cd ../build
$ cmake ..;make
$ ../bin/cam_localization live:1 ~/OpenCV/marker_mapper1.0.12/build/utils/markerset.yml ~/OpenCV/marker_mapper1.0.12/build/utils/markerset-cam.yml -s 0.135 -ref_id 10 -e 1
第三条命令请根据自己的情况适当修改，该程序的运行效果如下图：
图中黄色字符串显示的是相机在世界坐标系下的位置和姿态，因为前面我用MarkerMapper建图的时候是以ID为10的marker作为参考，所以这里的世界坐标系就是10号二维码的坐标系。
博客写到这儿，我们已经用单目相机实现了基于ArUco的室内定位。可以说，这个定位的精度还是相当令人满意的，如果后期再加上一些滤波优化算法，则可以进一步提高它的定位精度和稳定性。**相对于当前很多其他基于二维码的定位与导航方法，基于ArUco基准标识码的视觉定位不需要人工测量或预先铺设二维码路径，我们只要任意地将ArUco码贴在墙（门）面、天花板、地面或立柱上就可实时获取移动机器人的三维空间坐标和姿态。该方法非常方便、灵活，可以极大地降低人工成本。**但是，它有一个致命的弱点：在实际应用中，我们不可能在室内到处都贴上二维码，换句话说，实际环境中的二维码分布通常是很稀疏的，所以难免会出现图像中没有二维码的情况。那么问题来了，在相机检测不到二维码时如何估计它的位姿？这个问题留给各位大神来解决，如有好的方案还望不吝赐教！
二、ArUco之SPM-SLAM
SPM-SLAM: Squared Planar Marker SLAM，是一个基于ArUco的单目视觉SLAM方案。
项目地址：http://www.uco.es/investiga/grupos/ava/node/58
源码地址：https://sourceforge.net/projects/spm-slam/files/
1、编译运行
$ unzip spm-slam_1.0.1.zip
$ cd spm-slam_1.0.1/
$ mkdir build
$ cd build/
$ cmake ..
$ make
从这里下载一个名叫“4_video_office_large”的数据集，解压以后里面是一些图片序列以及相机的标定文件等。因为SPM-SLAM需要输入一个视频，所以你需要先把图片序列转成视频（或者直接将源码改为读取图片序列），然后把视频和相机标定文件拷贝到spm-slam文件夹下。为了防止混乱，我在此新建一个文件夹叫dataset，把视频和标定文件放在这里。然后运行命令：
$ ../build/spm-slam video.avi camera.yml -dic ARUCO -markersize 0.123
效果如下图：
运行完了你会发现在dataset文件夹多了一个markermap.yml文件，里面记录了所有marker的分布情况。那么接下来，我要把它的代码改一下，用它跑我们自己的摄像头。
2、SPM-SLAM跑自己的摄像头
- Step1 创建源文件
cd 到build文件夹，在终端运行命令：
$ touch ../spm_slam.cpp
$ gedit ../spm_slam.cpp
粘贴以下代码：
/********************************************************************/
/ By Li Xujie /
/********************************************************************/
#include "slam.h"
#include "mapviewer.h"
#include "stuff/timers.h"
#include <Eigen/Geometry>
class CmdLineParser {
int argc;
char **argv;
public:
CmdLineParser(int _argc, char **_argv) : argc(_argc), argv(_argv) {}
bool operator[](string param) {
int idx = -1;
for (int i = 0; i < argc && idx == -1; i++) if (string(argv[i]) == param) idx = i;
return (idx != -1);
}
string operator()(string param, string defvalue = "-1") {
int idx = -1;
for (int i = 0; i < argc && idx == -1; i++) if (string(argv[i]) == param) idx = i;
if (idx == -1) return defvalue; else return (argv[idx + 1]);
}
};
int main(int argc, char **argv) {
try {
CmdLineParser cml(argc, argv);
if (argc < 3) {
cerr << "Usage: camIndex camera_params.yml [-conf configParams.yml] [-dic DICT] [-markersize val]" << endl;
cerr << "\tcamIndex: camera index" << endl;
cerr << "\tcamera_params.yml: file with calibration parameters. Use OpenCv tool or ArUco tool." << endl;
cerr << "\t-conf <file>: specifies a configuration file for the aruco detector. Overrides [-dic and -markersize]" << endl;
cerr << "\t-markersize <float>: specifies the marker size in meters. Default value is 1." << endl;
cerr << "\t-dic <string>: indicates the dictionary to be employed. Default value is ARUCO_MIP_36h12" << endl;
cerr << "\t Possible Dictionaries: ";
for (auto dict : aruco::Dictionary::getDicTypes()) cerr << dict << " ";
cerr << endl;
return -1;
}
cv::VideoCapture vcap;
cv::VideoWriter videoout;
vcap.open(std::stod(argv[1]));
if (!vcap.isOpened()) throw std::runtime_error("Video not opened");
ucoslam::Slam Slam;
ucoslam::ImageParams image_params;
ucoslam::Params params;
cv::Mat in_image;
image_params.readFromXMLFile(argv[2]);
if (cml["-conf"]) params.readFromYMLFile(cml("-conf"));
else {
params.aruco_DetectorParams.dictionary = cml("-dic", "ARUCO_MIP_36h12");
params.aruco_markerSize = stof(cml("-markersize", "1"));
}
auto TheMap = std::make_shared<ucoslam::Map>();
//Create the viewer to see the images and the 3D
auto TViewer = ucoslam::MapViewer::create("Cv");
TViewer->set("showNumbers", "1");
TViewer->set("canLeave", "1");
TViewer->set("mode", "0");
TViewer->set("modelMatrix", "0.998437 -0.0490304 0.0268194 0 0.00535287 0.561584 0.827403 0 -0.0556289 -0.825967 0.560969 0 0 0 0 1");
TViewer->set("viewMatrix", " 1 0 0 0.01 0 4.63287e-05 -1 0.910185 0 1 4.63287e-05 9.18 0 0 0 1 ");
Slam.setParams(TheMap, params);
//Ok, let's start
char k = 0;
ucoslam::TimerAvrg Fps;
int currentFrameIndex=0;
while (vcap.grab() && k != 27) {
vcap.retrieve(in_image);
currentFrameIndex++;
//cout << "currentFrameIndex = " << currentFrameIndex << endl;
Fps.start();
Slam.process(in_image, image_params, currentFrameIndex);
Fps.stop();
k = TViewer->show(TheMap, in_image, Slam.getCurrentPose_f2g(), "#" + std::to_string(currentFrameIndex) + " fps=" + to_string(1. / Fps.getAvrg()));
}
cerr << "The markermap is saved to markermap.yml" << endl;
TheMap->saveToMarkerMap("markermap.yml");
}
catch (std::exception &ex) { cerr << ex.what() << endl; }
}
- Step2 修改CMakeLists.txt
在终端运行命令$ gedit ../CMakeLists.txt，先在文档中找到“ADD_EXECUTABLE(spm-slam spm-slam.cpp )”，然后做如下修改：
- Step3 编译运行
把相机标定文件拷贝到build文件夹下，在终端执行命令：
$ cmake ..;make
$ ./spm_slam 1 Logitech.yml -dic ARUCO -markersize 0.135
程序入口参数请根据你的实际情况适当修改，运行效果如下图：
你应该能够发现，程序最后的运行效果跟前面的MarkerMapper差不多，它也会生成一个二维码的分布文件叫markermap.yml。这个文件和MarkerMapper生成的markerset.yml是一样的，都可以用于室内定位。
这里再总结一下，SPM-SLAM相比于其他的视觉SLAM来说，它不存在明显的尺度漂移且没有其他单目SLAM的通病——尺度不确定性。原因很简单，SPM-SLAM预先知道了环境中的一个条件——二维码的边长，这就使得它对整个空间的尺度有了准确地把握；再加上每个二维码四个角点的相对位置已知，这使得相机在运动过程中可以有效地消除累积误差。
三、MarkerMapper的建图原理
本来想在这儿简单地介绍一下原理，但考虑到问题的复杂性，一句两句总也说不清楚，所以我又追加了一篇博客，详细介绍MarkerMapper的实现原理，感兴趣的请移步基于ArUco的视觉定位（四）
扫一扫
9329
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
