一、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的視覺定位（四）