お掃除ロボットルンバの掃除経路を撮影するプログラムをつくってみた

お掃除ロボットルンバを買って半年以上たちました。外出時にルンバを起動して、帰宅したら掃除が終わっている、という感じで使っています。ただ、確かに部屋はきれいになっているのですが、本当に部屋の隅々まで掃除してくれているのかが分からない。というわけで、ルンバの移動経路を連続撮影して、ちゃんと部屋の隅々まで掃除しているか確認するプログラムをRubyとCocoa(Objective-C)で作りました。

概要

今回は以下の二つのプログラムを作りました。

• camera: iMacとMacBookの内蔵カメラ(isight)で１秒ごとに部屋内を撮影するCocoaアプリ
• mono.rb: cameraで撮影した写真を合成するRubyプログラム

実験

• ルンバを起動して５分間掃除してもらう
• ２台のMac(iMac,MacBook)を室内の２カ所に設置
• 上述のプログラムで撮影＆解析
• その間に俺は風呂に入る

結果

この実験により以下の２つの画像を生成できました。

20090727追加

合成スクリプトをちょっとかえてみた。上のとどっちがいいかな？

結論

• ５分間で結構あちこち掃除してくれていることが分かる
• ２枚目の写真では奥のほう（台所がある）には、なぜか一度も侵入していない。５分以上掃除すればちゃんと侵入してくれるはずだが。
• ２枚目をみると、充電ベースにはほとんど近づいていないことがわかる。これはルンバの仕様っぽい。なので、充電ベース周りは結構ホコリがたまりがち。ときどき充電ベースの場所を変えてあげるとよい。
• 今回のプログラムである程度ルンバの軌跡を調査できることが確認できた。今度はフルで掃除させて結果をみてみたい。
• ルンバは便利なので、みんなも買えばいいと思う。

おまけ

初期バージョンの合成スクリプトによる合成画像。ピクセル値の差分をそのまま出力してみた。ルンバを認識しやすいけど、ちょっと画像として見辛いので、今回はピクセル値の平均値を出力して、透明っぽく見えるようにした。

実験中に偶然撮影された画像。人影のようなものが写り込んでいる。プログラマーの呪縛霊か何かかもしれない。

プログラム解説(camara.app)

ソース

まずは写真撮影部のソースから。Objective-Cで書いたCocoaアプリです。

//
//  MainController.m
//  camera
//
//  Created by 松前 健太郎 on 09/07/25.
//  Copyright 2009 __MyCompanyName__. All rights reserved.
//
#import "MainController.h"

@implementation MainController

- (IBAction)startRecording:(id)sender
{
	recording = TRUE;
}
- (IBAction)stopRecording:(id)sender
{
	recording = FALSE;
}

- (void)exportImageToURL:(NSURL*)inURL
{
    CGColorSpaceRef colorSpace = CGColorSpaceCreateWithName(kCGColorSpaceGenericRGB);
    CIContext *exportContext = [CIContext contextWithCGContext:[[NSGraphicsContext currentContext] graphicsPort] options:nil];
    CGColorSpaceRelease(colorSpace);
    CGImageDestinationRef imageDestination = CGImageDestinationCreateWithURL((CFURLRef)inURL, (CFStringRef)@"public.jpeg", 1, nil);
    if (imageDestination == NULL)
    {
        NSLog(@"problems creating image destination\n");
        CFRelease(imageDestination);
        return;
    }
    CGImageRef renderedImage = [exportContext createCGImage:image fromRect:[image extent]];
    CGImageDestinationAddImage(imageDestination, renderedImage, NULL);
    if (!CGImageDestinationFinalize(imageDestination))
    {
        NSLog(@"problems writing JPEG file\n");
    }
    CFRelease(imageDestination);
    CGImageRelease(renderedImage);	
}

- (void)captureOutput:(QTCaptureOutput *)captureOutput 
    didOutputVideoFrame:(CVImageBufferRef)videoFrame 
    withSampleBuffer:(QTSampleBuffer *)sampleBuffer 
    fromConnection:(QTCaptureConnection *)connection {
	
    @synchronized (self) {
        if(recording && canSave){
            CVBufferRetain(videoFrame);
            image = [CIImage imageWithCVImageBuffer:videoFrame];
            CVBufferRelease(videoFrame);
            NSLog([image description]);
            
            imgCnt++;
            NSString *dir = @"/Users/ken/work/kenmaz-sandbox/projects/roomba/imgs/cam";
            NSString *filepath = [NSString stringWithFormat:@"%@/%d.jpg", dir, imgCnt];
            [self exportImageToURL:[NSURL fileURLWithPath:filepath]];
            canSave = FALSE;
        }
    }	
}

-(void)saveImage:(NSTimer*)timer
{
    canSave = TRUE;
}

- (void)awakeFromNib
{
    timer = [NSTimer scheduledTimerWithTimeInterval:2 
                                                                     target:self 
	                                                                 selector:@selector(saveImage:) 
                                                                     userInfo:nil
                                                                      repeats:YES];	
	mCaptureSession = [[QTCaptureSession alloc] init];
	BOOL success = NO;
    NSError *error;
	QTCaptureDevice *device = [QTCaptureDevice defaultInputDeviceWithMediaType:QTMediaTypeVideo];
    if (device) {
		success = [device open:&error];
		if (!success) {
		    // Handle error
		}
		mCaptureDeviceInput = [[QTCaptureDeviceInput alloc] initWithDevice:device];
		success = [mCaptureSession addInput:mCaptureDeviceInput error:&error];
		if (!success) {
		    // Handle error
		}
		mCaptureDecompressVideoOutput = [[QTCaptureDecompressedVideoOutput alloc] init];
		success = [mCaptureSession addOutput:mCaptureDecompressVideoOutput error:&error];
		if(!success){
		//Error
		}	
		// Set the controller be the movie file output delegate.
		[mCaptureDecompressVideoOutput setDelegate:self];
		
		// Associate the capture view in the UI with the session
		[mCaptureView setCaptureSession:mCaptureSession];
    }
	// Start the capture session running
	[mCaptureSession startRunning];
}

- (void)windowWillClose:(NSNotification *)notification
{
    [mCaptureSession stopRunning];
    [[mCaptureDeviceInput device] close];
}

- (void)dealloc
{
    [mCaptureSession release];
    [mCaptureDeviceInput release];
    [mCaptureDecompressVideoOutput release];
    [super dealloc];
}
@end

解説

• 上記のソース以外に、UIビルダーで、キャプチャビューと撮影ボタンをもった簡単なウィンドウを作っておきます

• アプリを起動するとawakeFromNibメソッドが実行されます。このメソッド中では、画像キャプチャのためのセッション(QTCaptureSession)をまず作成してます。次にデバイス(QTCaptureDevice)を検出して、セッションへの入力元として設定しています。また、出力先として非圧縮画像を取得するためのOutputを作成し、同じくセッションに出力先として設定しています。あとは１秒ごとに撮影を行うためのタイマー等も設定しています。
• カメラが画像をとらえるたびに、captureOutput:didOUtputVideoFrameメソッドが実行されます。メソッドでは、１秒おきにカメラが撮影した画像(CIImage)を読み込んで、exportImageToURLメソッドを呼びます。
• exportImageToURLメソッド内では受け取った画像をローカルにjpeg形式で保存します。
• Macのネイティブアプリを作るのははじめてでしたが、iPhoneアプリ開発である程度Cocoaフレームワークの開発手法を理解していたので、割とすんなり作れました。
• １秒ごとにNSTimerを実行して画像を保存しているんですが、もうちょっと良いやり方は無いかな。。Javaで言うところのSystem.currentTimeInMillis()みたいなので時間を計れればもうちょいシンプルになったのだが。。
• インデントがめちゃめちゃに表示されるなー。＞はてな記法

プログラム解説(mono.rb)

ソース

今度はcamera.appで撮影した画像を読み込んで、うまいこと合成するRubyのスクリプトです。ファイル名がmono.rbとなっていますが、この名前に特に意味はありません。

require 'rubygems'
require 'rmagick'
include Magick
include Math

def diff(base, file)
  puts "processing #{file} ..."
  img = ImageList.new(file)
  base.rows.times do |y|
    base.columns.times do |x|
      clr1 = base.pixel_color(x, y)
      clr2 = img.pixel_color(x, y)
      r1, g1, b1 = clr1.red, clr1.green, clr1.blue
      r2, g2, b2 = clr2.red, clr2.green, clr2.blue
      r = (r1 - r2).abs
      g = (g1 - g2).abs
      b = (b1 - b2).abs
      r = r < 5000 ? r1 : (r1+r1+r2)/3
      g = g < 5000 ? g1 : (g1+r1+g2)/3
      b = b < 5000 ? b1 : (b1+r1+b2)/3
      base.pixel_color(x, y, Pixel.new(r,g,b))
    end
  end
  base.write("jpeg:#{Time.now.to_i}.jpg")
end

def collect
  files = get_files
  bass = ImageList.new(files[0])
  files.each do |file|
    diff(bass, file)
  end
  bass.write("jpeg:out.jpg")
end

def get_files
  fnames = []
  Dir::foreach(ARGV[0]) do |f|
    if f =~ /.jpg/
      fnames << f
    end
  end
  fnames.sort!{|a,b| a.to_i <=> b.to_i }
  files = []
  fnames.each do |f|
    files << ARGV[0] + f
  end
  files
end

collect

解説,感想

• 実行には,ImageMagick+RMagickがインストールされている必要があります。
• やっていることは、引数で与えられたフォルダ以下の全てのファイル同士をピクセル単位で比較し、相違があれば合成して出力して、最終的に一枚の画像ファイルを生成しているだけです。
• diffメソッドでは、一枚目の画像を基準(base)として、残りの画像(img)とピクセル単位で比較し、差異がなければbaseのピクセル値を採用、差異があればbase:img = 2:1の割合でピクセル値の平均を取って採用しています。「r = r < 5000 ? r1 : (r1+r1+r2)/3」とかやってる部分なんですが、相当適当です。
• てか、すごい重いです。そのうちCで書きなおします。

ソース一式

以下に置いてあります。

http://code.google.com/p/kenmaz-sandbox/source/browse/#svn/trunk/projects/roomba