cvl-robot's diary

研究ノート　メモメモ https://github.com/dotchang/

Ubuntu16.04LTSのopenFrameworksでPS3EyeCameraを入力にして、ArUco3.0.4を動かす。(その１)カメラキャリブレーション編

ArUcoがVer.3でかなり高速に動くようになったらしい、ということはちょっと前に調べました。ところが、実験環境に使ったIntelEuclidのRGBカメラではフレームレートが上がらず、その実力はよくわかりませんでした。そこで、今回は部屋の隅に転がっていたPS3EyeCameraが、320x240の画像サイズならば170ｆｐｓを稼げて、Linuxならドライバーも用意されていることを思い出し、これを使ってArUco3を動かしてみたいと思います。

openFrameworksでArUco

もうすでに誰かが環境を整えているんじゃないの？と思い調べてみると、ご本家arturocさんのofxArUcoがしっかりあります。つい最近のタイムスタンプもあり、しっかりメンテナンスもされているのですが、ArUcoのバージョンは1.3.0のママのようです。
github.com

でも、多少話題になってるぐらいだから、ofxArUco3もあるんじゃないの？と思い調べてみると、elliotwoodsさんのフォークっぽいものが見つかります。
github.com
ところが、中を見ると様子が多少おかしくて

  • this addon doesn't do anything but provide libs to use directly

なんてことが書かれています。どういうことか考えてみると、ArUcoはもともとopenCVをベースにしていますので、openFrameworks用のインターフェースがあった方が便利なのかもしれないが、別になくても困らないから、openFrameworksでライブラリをリンクするためだけのaddonを用意しておけば十分じゃね？と言っているんだと解釈しました。なので、これに倣います。

ofxAruco3の準備

1. ArUco3のソースのダウンロード
https://sourceforge.net/projects/aruco/files/
から最新版をダウンロードして解凍します。

2. ofxAruco3ディレクトリの作成
まず、openFrameworks/addonsの下に移動して、ofxAruco3という名前の空のディレクトリを作ります。次に、作法に則って、その下にsrcとlibsフォルダを作ります。

cd ~/workspace/openFrameworks/addons
mkdir ofxAruco3
cd ofxAruco3
mkdir src
mkdir libs

3. ソースのコピー
libsの中にArUcoのSrcをまるまるコピーします。

cd libs
cp -r ~/workspace/aruco304/src/* ./

これだけで完了です。あとは、自分のappsのaddons.makeにofxAruco3を追加すれば、openFrameworksと一緒にビルドされるようになります。

exampleとしてaruco_calibration.cppをopenFrameworksで動かしてみる

ofxPS3EyeGrabberの準備

ここでは、手軽なハイスピードカメラとしてPS3Eyeを使ってみます。画像取り込みインターフェースは,ofxVideoGrabberと同等なので、ちょっと書き換えるだけで普通のUSBカメラでも問題なく動きます。
github.com

cd ~/workspace/openFramewroks/addons
git clone https://github.com/bakercp/ofxPS3EyeGrabber

addons.makeに依存ライブラリを追加します。適当なプロジェクトフォルダをコピーして、プロジェクト環境を準備しておきます。addons.makeを編集して、

cd ~/workspace/openFrameworks/apps/vscode_of/myAruco3-calibration
nano addons.make
ofxKinect
ofxOpenCv
ofxPS3EyeGrabber
ofxAruco3

のようにofxAruco3も含めて4つ追加しておきます。

ofxAruco3-calibrationのソース

main.cpp, ofApp.h, ofApp.cppを用意します。また、calibrator.cppとcalibrator.hをArUco3のutils_calibrationからコピーして持ってきます。

main.cpp

#include "ofApp.h"

int main(int argc, char* argv[])
{
    ofGLWindowSettings settings;
    settings.width = 640;
    settings.height = 480;
    ofCreateWindow(settings);
    return ofRunApp(std::make_shared<ofApp>(argc, argv));
}

ofApp.h

#pragma once

#include "ofMain.h"
#include "aruco.h"
#include "calibrator.h"

class ofApp: public ofBaseApp
{
public:
    ofApp(int argc, char* argv[]){}

    void setup();
    void update();
    void draw();
    void keyPressed(int key);

    ofVideoGrabber grabber;
    
    cv::Mat TheInputImage, TheInputImageCopy;
    aruco::CameraParameters TheCameraParameters;
    aruco::MarkerDetector TheMarkerDetector;
    aruco::Calibrator calibrator;
    std::vector<aruco::Marker> detected_markers;
    aruco::CameraParameters camp;  // camera parameters estimated

    int width, height, fps;
    bool saveImages;
    string saveFile;
    float realsize;
};

ofApp.cpp

//
// Copyright (c) 2014 Christopher Baker <https://christopherbaker.net>
//
// SPDX-License-Identifier:	MIT
//

/**
Copyright 2017 Rafael Muñoz Salinas. All rights reserved.

Redistribution and use in source and binary forms, with or without modification$
permitted provided that the following conditions are met:

   1. Redistributions of source code must retain the above copyright notice, th$
      conditions and the following disclaimer.

   2. Redistributions in binary form must reproduce the above copyright notice,$
      of conditions and the following disclaimer in the documentation and/or ot$
      provided with the distribution.

THIS SOFTWARE IS PROVIDED BY Rafael Muñoz Salinas ''AS IS'' AND ANY EXPRESS OR $
WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTAB$
FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL Rafael Muñoz$
CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY$
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE$
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUS$
ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUD$
NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EV$
ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

The views and conclusions contained in the software and documentation are those$
authors and should not be interpreted as representing official policies, either$
or implied, of Rafael Muñoz Salinas.
*/

#include "ofApp.h"
#include "ofxPS3EyeGrabber.h"
#include <opencv2/highgui/highgui.hpp>

void ofApp::setup()
{
    // ofSetFrameRate(0);
    // ofSetVerticalSync(0);

    // Camera Settings
    width = 320;            // 320x240, 640x480
    height = 240;
    fps = 187;              // 187Hz, 75Hz
    
    // Aruco Settings
    realsize = 0.038;       // [m]
    saveImages = 1;
    saveFile = "calib.yml";

    // ofxPS3EyeGrabber settings.
    grabber.setGrabber(std::make_shared<ofxPS3EyeGrabber>());
    grabber.setPixelFormat(OF_PIXELS_BGR); //OF_PIXELS_RGB, BGR, GRAY, NATIVE 
    grabber.setDesiredFrameRate(fps); 
    grabber.setup(width, height); 
    grabber.getGrabber<ofxPS3EyeGrabber>()->setAutogain(true);
    grabber.getGrabber<ofxPS3EyeGrabber>()->setAutoWhiteBalance(true);

    // ofxAruco3 settings
    // set specific parameters for this configuration
    TheMarkerDetector.setDictionary( "ARUCO_MIP_36h12");
    TheMarkerDetector.setDetectionMode(aruco::DM_VIDEO_FAST,0.05);

    // read first image to get the dimensions
    grabber.update();
    ofPixels pix = grabber.getPixels();
    TheInputImage = cv::Mat(pix.getHeight(), pix.getWidth(), CV_8UC3, pix.getData());

    //configure the calibrator
    calibrator.setParams(cv::Size(width, height), realsize);
}

void ofApp::update()
{
    grabber.update();
    ofPixels pix = grabber.getPixels();
    TheInputImage = cv::Mat(pix.getHeight(), pix.getWidth(), CV_8UC3, pix.getData());
    if(!TheInputImage.empty()){
        TheInputImageCopy = TheInputImage.clone();
    }

    // detect and print
    detected_markers.clear();
    detected_markers = TheMarkerDetector.detect(TheInputImage);

    if(!TheInputImageCopy.empty()){
        for (auto m: detected_markers)
            m.draw(TheInputImageCopy, cv::Scalar(0, 0, 255), 1);
    }
}


void ofApp::draw()
{
    ofBackground(0);
    ofSetColor(255);

    // print markers from the board
    if(!TheInputImageCopy.empty()){
        cv::imshow("draw", TheInputImageCopy);
        cv::waitKey(1);
    }
    
    grabber.draw(0, 0);

    std::stringstream ss;

    ss << " App FPS: " << ofGetFrameRate() << std::endl;
    ss << " Cam FPS: " << grabber.getGrabber<ofxPS3EyeGrabber>()->getFPS() << std::endl;
    ss << "Real FPS: " << grabber.getGrabber<ofxPS3EyeGrabber>()->getActualFPS() << std::endl;
    ss << "      id: 0x" << ofToHex(grabber.getGrabber<ofxPS3EyeGrabber>()->getDeviceId());

    ss << endl;

    ss << "'a' add current image for calibration" << std::endl;
    ss << calibrator.getInfo() << std::endl;

    ofDrawBitmapStringHighlight(ss.str(), ofPoint(10, 315));
}

void ofApp::keyPressed(int key)
{
    if (key == 'a'){
        calibrator.addView(detected_markers);
        if (saveImages){
            string number=std::to_string(calibrator.getNumberOfViews()-1);
            while(number.size()!=5) number="0"+number;
                cv::imwrite("calib-"+number+".png",TheInputImage);
        }
    }

    if (key == 27) {
        aruco::CameraParameters camp;
        if ( calibrator.getCalibrationResults(camp)){
                camp.saveToFile(saveFile);
                cout<<"results saved to " << saveFile << endl;
            }
            else cerr<<"Could not obtain calibration"<<endl;
        cout<<"Final error="<<calibrator.getReprjError()<<endl;
    }
}

calibrator.cpp .hのコピーとEigenのインクルードパスの指定

cd ~/workspace/aruco304/utils_calibration
cp calibrator.* ~/workspace/openFrameworks/apps/vscode_oF/myPS3EyeGrabber/src

calibratorの中のlevmarの演算で使われているEigen3を見つけ出せるように、config.makeのPROJECT_CFLAGSのコメントアウトを外して編集します。

PROJECT_CFLAGS = -I/usr/include/eigen3/

実行結果

f:id:cvl-robot:20180215142340p:plain
こんな感じで、窓が2つ開きます。openCVのMatにマーカーを書いた結果をopenFrameworksに戻すのが面倒臭かったので、cv::imshowで表示をしているためです。ofでレンダリングする方法としては、ofxCvのtoOFを使うのが一番簡単でしょう。ただし、コピーを伴うので若干遅れます。もっとも、このexampleも60ｆｐｓでしか確認できていませんが。。。
何枚か'a'キーを押して画像を取得したあと、’esc’を押すことによってキャリブレーション結果が計算されて保存されます。ファイル名やその他のパラメータはソースコードに決め打ちしているので、適当に編集してください。