Forked from:: rokubou's FLARToolKit Sample Simple_PV3D

FLARToolKitでカメラ・マーカー間の距離測定

カメラ・マーカー間の距離測定を行うサンプル。
実験的に表示オブジェクトの安定化コードも入れてみたけれど微妙かもしれない。
また、かなり適当に書いてあるので参考にはならんかも。

forked:0
favorite:1
lines:295
license : GPLv3 License
modified : 2011-07-27 17:58:33

Embed: Tweet

/**
 * FLARToolKit Sample - 距離計測 簡易サンプル
 *  forked from rokubou's FLARToolKit Sample Simple_PV3D
 * --------------------------------------------------------------------------------
 * Copyright (C)2011 rokubou
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 * 
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 * 
 * For further information please contact.
 *  http://www.libspark.org/wiki/saqoosha/FLARToolKit
 * 
 * Contributors
 *  rokubou
 * --------------------------------------------------------------------------------
 * readme
 *  マーカーサイズは、実際に使用するマーカーの１辺の長さ 80mm = 80 とセットする事。
 *  どうあがいても誤差が出ます。誤差を小さくする場合は、ARToolKit v2 のカメラキャリブレーションを
 *  行って、カメラパラメータを差し替える必要があります。
 */
package
{
    import flash.display.Bitmap;
    import flash.display.BitmapData;
    import flash.display.PixelSnapping;
    import flash.display.Shape;
    import flash.display.Sprite;
    import flash.events.Event;
    import flash.events.IOErrorEvent;
    import flash.events.SecurityErrorEvent;
    import flash.media.Camera;
    import flash.media.Video;
    import flash.net.URLLoader;
    import flash.net.URLLoaderDataFormat;
    import flash.net.URLRequest;
    import flash.text.TextField;
    import flash.utils.ByteArray;
        
    import org.libspark.flartoolkit.core.FLARCode;
    import org.libspark.flartoolkit.core.param.FLARParam;
    import org.libspark.flartoolkit.core.raster.rgb.FLARRgbRaster_BitmapData;
    import org.libspark.flartoolkit.core.transmat.FLARTransMatResult;
    import org.libspark.flartoolkit.detector.FLARSingleMarkerDetector;
    import org.libspark.flartoolkit.support.pv3d.FLARBaseNode;
    import org.libspark.flartoolkit.support.pv3d.FLARCamera3D;
    import org.papervision3d.core.math.Matrix3D;
    import org.papervision3d.core.math.Number3D;
    import org.papervision3d.materials.WireframeMaterial;
    import org.papervision3d.objects.DisplayObject3D;
    import org.papervision3d.objects.primitives.Plane;
    import org.papervision3d.render.LazyRenderEngine;
    import org.papervision3d.scenes.Scene3D;
    import org.papervision3d.view.Viewport3D;
    
    [SWF(width=640, height=480, backgroundColor=0x808080, frameRate=30)]
    
    public class FLARToolKit_Sample_Distance extends Sprite
    {
        /**
         * 画面の幅と高さ
         */
        protected var canvasWidth:int;
        protected var canvasHeight:int;
        
        /**
         * 画面の幅と高さ
         */
        protected var captureWidth:int;
        protected var captureHeight:int;
        
        /**
         * マーカーの一辺の長さ(px)
         */
        protected var codeWidth:int;
        
        /**
         * カメラパラメータのファイル名
         * 内部的に初期化される処理が含まれるので読み込む必要は無い。
         * 例外的に 16:9 で使う場合は、それようのパラメータファイルを読み込むこと。
         */
        protected var cameraParamFile:String;
        
        /**
         * マーカーパターンのファイル名
         */
        protected var markerPatternFile:String;
        
        /**
         * パラメータファイル、マーカーパターンファイルの読込み用
         * @see flash.net.URLLoader
         */
        private var urlLoader:URLLoader;
        
        /**
         * カメラパラメータデータ
         * アスペクト比や歪みなどの補正のための情報が含まれる
         * @see org.libspark.flartoolkit.core.param.FLARParam
         */
        protected var cameraParam:FLARParam;
        
        /**
         * マーカーパターン
         * マーカーを複数パターン使う場合はVectorなどで管理する
         * @see org.libspark.flartoolkit.core.FLARCode
         */
        protected var markerPatternCode:FLARCode;
        
        /**
         * @see flash.media.Camera
         */
        protected var webCamera:Camera;
        
        /**
         * flash.media.Video
         */
        protected var video:Video;
        
        /**
         * Webカメラからの入力をBitmapに確保する
         * @see flash.display.Bitmap
         */
        private var capture:Bitmap;
        
        /**
         * ラスタイメージ
         * @see org.libspark.flartoolkit.core.raster.rgb.FLARRgbRaster_BitmapData
         */
        private var raster:FLARRgbRaster_BitmapData;
        
        /**
         * Marker detector
         * @see org.libspark.flartoolkit.detector.FLARSingleMarkerDetector
         */
        private var detector:FLARSingleMarkerDetector;
        
        /**
         * 認識したマーカーの情報を格納
         * 複数マーカー対応しているけれど、面倒なので 1つ分
         */
        protected var markerData:MarkerData = new MarkerData(0);
        
        /**
         * 3Dモデル表示用
         */
        protected var scene:Scene3D;
        
        /**
         * 3Dモデル表示用
         */
        protected var viewport:Viewport3D;
        
        /**
         * 3Dモデル表示時の視点
         */
        protected var camera3D:FLARCamera3D;
        
        /**
         * Marker base node
         */
        protected var markerNode:FLARBaseNode;
        
        /**
         * 3D Renderer
         */
        protected var renderer:LazyRenderEngine;
        
        /**
         * 表示モデルを一括して押し込めるコンテナ
         */
        protected var container:DisplayObject3D;
        
        /**
         * マーカー認識中を示す枠
         */
        private var plane:Plane;
        
        /**
         * 距離表示用
         */
        public var text:TextField;
        
        /**
         * マーカーとカメラ間の距離をバーで表示するためのキャンバス
         */
        private var screen:Shape;
        
        /**
         * Constructor
         * ここから初期化処理を呼び出して処理をスタート
         */
        public function FLARToolKit_Sample_Distance()
        {
            
            this.initialize();
        }
        
        /**
         * initialize
         *  各種サイズの初期化
         */
        protected function initialize():void
        {
            // 各種サイズの初期化
            captureWidth = 320;
            captureHeight = 240;
            canvasWidth = 640
            canvasHeight = 480;
            codeWidth = 80;
            
            //
            markerPatternFile = 'http://assets.wonderfl.net/static/flar/flarlogo.pat';
            
            // パラメータファイルの読込み
            // 今回は省略して初期値を用いる
            this.cameraParam = new FLARParam();
            this.cameraParam.changeScreenSize(captureWidth, captureHeight);
            
            // マーカーパターンファイルの読込み
            this.urlLoader = new URLLoader();
            this.urlLoader.dataFormat = URLLoaderDataFormat.TEXT;
            this.urlLoader.addEventListener(Event.COMPLETE, this.onLoadCode);
            this.urlLoader.addEventListener(IOErrorEvent.IO_ERROR, dispatchEvent);
            this.urlLoader.addEventListener(SecurityErrorEvent.SECURITY_ERROR, dispatchEvent);
            this.urlLoader.load(new URLRequest(markerPatternFile));
        }
        
        /**
         * マーカーパターンを読み込む
         * @param e Event
         */
        protected function onLoadCode(e:Event):void
        {
            // URL Loader関連のイベントを削除
            this.urlLoader.removeEventListener(Event.COMPLETE, this.onLoadCode);
            
            // 分割数(縦・横)、黒枠の幅(縦・横)
            this.markerPatternCode = new FLARCode(16, 16, 50, 50);
            this.markerPatternCode.loadARPatt(this.urlLoader.data);
            // loaderがgc対象になるようにnullを突っ込む
            this.urlLoader = null;
            
            // 初期化
            dispatchEvent(new Event(Event.INIT));
            this.onInit();
        }
        
        /**
         * Webカメラの設定と、ARToolKitの準備
         */
        protected function onInit():void
        {
            // setup webcam
            this.webCamera = Camera.getCamera();
            if (!this.webCamera) {
                throw new Error('No webcamera!');
            }
            this.webCamera.setMode(this.captureWidth, this.captureHeight, 30);
            
            this.video = new Video( this.captureWidth, this.captureHeight);
            this.video.attachCamera(this.webCamera);
            
            // setup ARToolKit
            this.capture = new Bitmap(new BitmapData(this.captureWidth, this.captureHeight, false, 0),
                                       PixelSnapping.AUTO,
                                       true);
            // ウェブカメラの解像度と表示サイズが異なる場合は拡大する
            this.capture.width = this.canvasWidth;
            this.capture.height= this.canvasHeight;
            this.addChild(this.capture);
            
            this.raster = new FLARRgbRaster_BitmapData(this.capture.bitmapData);
            // setup Single marker detector
            this.detector = new FLARSingleMarkerDetector(this.cameraParam,
                                                          this.markerPatternCode,
                                                          this.codeWidth);
            this.detector.setContinueMode(true);
            
            // 表示関係の設定(使用するライブラリによって変化します)
            this.viewport = this.addChild(new Viewport3D(this.captureWidth,
                                                          this.captureHeight)) as Viewport3D;
            this.viewport.scaleX = this.canvasWidth / this.captureWidth;
            this.viewport.scaleY = this.canvasHeight / this.captureHeight;
            this.viewport.x = -4; // なぜかずれるので補正
            
            // 
            this.scene = new Scene3D();
            this.markerNode = this.scene.addChild(new FLARBaseNode()) as FLARBaseNode;
            
            // 3Dモデル表示時の視点を設定
            this.camera3D = new FLARCamera3D(this.cameraParam);
            
            // setup renderer
            this.renderer = new LazyRenderEngine(this.scene, this.camera3D, this.viewport);
            
            // モデル格納用のコンテナ作成
            this.container = new DisplayObject3D();
            // モデルデータ
            this.setModelData();
            // モデルデータを登録
            this.markerNode.addChild(this.container);
            
            // start
            this.start();
        }
        
        /**
         * モデルデータを書く場所
         */
        protected function setModelData():void
        {
            // ワイヤーフレームで、マーカーと同じサイズのPlaneを作成
            var wmat:WireframeMaterial = new WireframeMaterial(0x0000ff, 1, 2);
            this.plane = new Plane(wmat, 80, 80);
            this.plane.rotationX = 180;
            this.container.addChild(this.plane);
        }
        
        /**
         * マーカーの認識と3次元モデルの描写を開始する
         */
        public function start():void
        {
            // 距離イメージ表示用
            this.screen = new Shape();
            this.addChild(this.screen);
            
            // マーカー認識・非認識時用のイベントを登録
            this.addEventListener(MarkerEvent.MARKER_ADDED, this.onMarkerAdded);
            this.addEventListener(MarkerEvent.MARKER_UPDATED, this.onMarkerUpdated);
            this.addEventListener(MarkerEvent.MARKER_REMOVED, this.onMarkerRemoved);
            
            // 数値表示用
            this.text = new TextField(); text.y = 400;
            this.text.background = true;
            this.text.backgroundColor = 0xffffff;
            this.addChild(this.text);
            
            // 処理開始
            this.addEventListener(Event.ENTER_FRAME, this.run);
        }
        
        public function onMarkerAdded(e:Event=null):void
        {
            // 変換行列取得
            var resultMat:FLARTransMatResult = markerData.getAveragingTransMat(true);
            this.detector.getTransformMatrix(resultMat);
            
            // 3D枠表示用
            this.markerNode.setTransformMatrix(resultMat);
            this.markerNode.visible = true;
            
            // カメラ - マーカー間の距離を取得。単位は mm 
            var distance:Number = markerData.getDistance();
            
            with(this.screen.graphics){
                clear();
                lineStyle( 10, 255, 0.5);
                moveTo( 0, 20);
                lineTo( distance/10, 20);
            }
            text.text = distance.toString()+'[mm]';
        }
        
        public function onMarkerUpdated(e:Event=null):void
        {
            // 今回は実装していない
        }

public function onMarkerRemoved(e:Event=null):void
        {
            this.markerNode.visible = false;
//          this.renderer.render();
        }
        
        /**
         * ここで処理振り分けを行っている
         */
        public function run(e:Event):void
        {
            this.capture.bitmapData.draw(this.video);
            // マーカー情報を全て非表示にする
            markerData.setVisible(false, true);
            
            // Marker detect
            var detected:Boolean = false;
            try {
                detected = this.detector.detectMarkerLite(this.raster, 80) && this.detector.getConfidence() > 0.5;
            } catch (e:Error) {}
            
            // マーカーの認識判別フラグ更新
            // この内部で勝手に切り替えられてしまうので注意
            markerData.setVisible( detected, true);
            
            // tansmatをセットしないとね☆
            if (detected) {
                var transmat_result:FLARTransMatResult = new FLARTransMatResult();
                detector.getTransformMatrix(transmat_result);
                markerData.setTransMat(transmat_result);
            }
            
            // 認識時の処理
            if (markerData.isDetect) {
                this.dispatchEvent(new MarkerEvent(MarkerEvent.MARKER_ADDED));
            // 非認識時
            } else {
                this.dispatchEvent(new MarkerEvent(MarkerEvent.MARKER_REMOVED));
            }
            this.renderer.render();
        }
        
    }
}
import flash.display.AVM1Movie;

import flash.events.Event;

/**
 * イベント制御用の簡易クラス
 */
class MarkerEvent extends Event
{
    /**
     * Markerを認識した時
     */
    public static const MARKER_ADDED:String = "markerAdded";
    
    /**
     * Marker更新時
     */
    public static const MARKER_UPDATED:String = "markerUpdated";
    
    /**
     * Markerが認識しなくなった時
     */
    public static const MARKER_REMOVED:String = "markerRemoved";
    
    public function MarkerEvent(type:String, bubbles:Boolean=false, cancelable:Boolean=false)
    {
        super(type, bubbles, cancelable);
    }
}

import org.libspark.flartoolkit.core.transmat.FLARTransMatResult;

/**
 * 識別中のマーカー情報を管理するクラス
 * 
 * マーカーに関する便利機能を実装。
 */
class MarkerData
{
    /**
     * 最後に認識した時の一致率
     */
    public var confidence:Number;
    
    /**
     * マーカーのインデックス
     */
    public var markerIndex:int;
    
    /**
     * 現在の transMat
     */
    protected var current_resultMat:FLARTransMatResult;
    
    /**
     * 一つ前の transMat
     */
    protected var previous_resultMat:FLARTransMatResult;
    
    /**
     * 認識中かどうかのフラグ
     */
    public var isDetect:Boolean = false;
    
    /**
     * 一つ前の段階で認識していたかどうかのフラグ
     */
    public var isPrevDetect:Boolean = false;
    /**
     * 表示ディレイカウンター
     * フレーム数を設定する。
     */
    protected var delayCountor:int = 2;
    
    /**
     * 
     */
    public function MarkerData(_markerIndex:int)
    {
        this.markerIndex = _markerIndex;
        previous_resultMat = current_resultMat = new FLARTransMatResult();
    }
    
    /**
     * 一致率を確保する
     * @param confidence
     */
    public function setConfidence(confidence:Number):void
    {
        this.confidence = confidence;
    }
    
    /**
     * 一致率を取得する
     * @return
     */
    public function getConfidence():Number
    {
        return confidence;
    }

public function getMarkerIndex():int
    {
        return markerIndex;
    }
    
    /**
     * 表示・非表示を切り替えるフラグを管理
     * 表示ディレイカウントはこの内部で加減算している。
     * 
     * @param isVisible
     */
    public function setVisible( _isVisible:Boolean, _isUpdate:Boolean):void
    {
        if (_isUpdate) {
            isPrevDetect = isDetect;
        }
        if (_isVisible) {
            isDetect = true;
            delayCountor = 3;
        } else if (delayCountor>0) {
            isDetect = true;
            if (delayCountor!=-1) {
                delayCountor--;
            }
            confidence = 0.0;
        } else {
            isDetect = false;
            confidence = 0.0;
        }
    }
        
    /**
     * 必ず setVisible してから値をセットしてください。
     * setVisible で false だった場合は、呼び出す必要はありません。
     * 
     * @param _transMat
     */
    public function setTransMat( _transMat:FLARTransMatResult):void
    {
        if (!isPrevDetect && isDetect) {
            previous_resultMat = current_resultMat = _transMat;
        } else if (isDetect) {
            previous_resultMat = current_resultMat;
            current_resultMat = _transMat;
        } else if (!isDetect && delayCountor>0) {
            previous_resultMat = current_resultMat;
        }
    }
    
    /**
     * 
     */
    public function get resultMat():FLARTransMatResult
    {
        return current_resultMat;
    }
    
    /**
     * カメラ・マーカー間の距離を返す
     */
    public function getDistance():Number
    {
        // カメラ - マーカー間の距離算出。単位は mm 
        var x:Number = current_resultMat.m03;
        var y:Number = current_resultMat.m13;
        var z:Number = current_resultMat.m23;
        // 
        return Math.round(Math.sqrt(x*x+y*y+z*z)*100)/100;
    }

/**
     * 加重平均をかけてからTransMatを返却する。
     * これにより安定する可能性が高いが、動きが早い場合は追随しない可能性もある。
     * 次回以降の参考値にするために、保持しているcurrent_resultMatも更新する
     * 
     * @param isAvgT 並進ベクトル成分も平均化するなら true
     * @return NyARTransMatResult
     */
    public function getAveragingTransMat( isAvgT:Boolean):FLARTransMatResult
    {
        var _transMat:FLARTransMatResult = new FLARTransMatResult();
        _transMat = current_resultMat;
        // 一つ前のフレームで識別できていたかで処理を変える
        // できていれば並行成分にのみなんちゃって加重平均をかける
        if (isPrevDetect) {
            // ゆがみが発生する可能性があるが許容範囲として使用する
            var weightA:Number = 7;
            var weightB:Number = 3;
            // 回転成分の安定化コードはここに書く
              // 無いけどね
            // 並進ベクトルの加重平均化
            if (isAvgT) {
                weightA = 9;
                weightB = 1;
                _transMat.m03 = (previous_resultMat.m03*weightA + current_resultMat.m03*weightB)/(weightA+weightB);
                _transMat.m13 = (previous_resultMat.m13*weightA + current_resultMat.m13*weightB)/(weightA+weightB);
                _transMat.m23 = (previous_resultMat.m23*weightA + current_resultMat.m23*weightB)/(weightA+weightB);
            } else {
                _transMat.m03 = current_resultMat.m03;
                _transMat.m13 = current_resultMat.m13;
                _transMat.m23 = current_resultMat.m23;
            }
            current_resultMat = _transMat;
        // 識別できていなければ前回の値を使う
        } else {
            _transMat = current_resultMat;
            previous_resultMat = current_resultMat;
        }
        
        return _transMat;
    }
}

Code Fullscreen

Preview Fullscreen

zahir