[:ja]カメラ姿勢が得られれば単純なARを表示するのは非常に簡単です．基本的には表示したい3次元物体を，推定されたカメラ姿勢を基にカメラ座標系へ変換し，カメラ内部パラメータを使って画像上へ投影するだけです．

ただ，一つ問題になるのはスケールの推定です．原理的に単眼のvisual SLAM/odometryにおいては画像情報だけから環境のスケールを推定することができません．例えば，大きな箱を遠くから見ているのと，小さな箱を近く見ている場合には同じような絵が得られてしまうため大きさの区別がつきません．そのため，ARにおいて物体の大きさを指定して特定の場所に置くというのは少し難しい話になります（大きさ10cmの立方体をカメラから100cm前に置く！みたいなことを正確に行うのは難しい）．今回はカメラの動きに非常に単純なアドホックな過程を置いてスケールの推定を行ってみます．

ひとまず，visual odometryを使ったARプログラムの全体像を見てから各部を説明しましょう．

#!/usr/bin/python
import cv2
import numpy
import itertools

import rospy
import cv_bridge
import tf_conversions
import message_filters
from sensor_msgs.msg import *
from geometry_msgs.msg import *


# 立方体表示クラス
class Cube:
	def __init__(self, pos, size):
		self.pos = numpy.float32(pos)

		# 立方体の3次元頂点を生成し，隣接する頂点(距離==1)を結ぶインデックスを作成する．
		self.vertices = numpy.int32(list(itertools.product((0, 1), (0, 1), (0, 1))))
		ivertices = enumerate(self.vertices)
		self.indices = [(i1, i2) for (i1, x1), (i2, x2) in itertools.combinations(ivertices, 2) if sum(abs(x1 - x2)) == 1]
		self.vertices = (self.vertices.astype(numpy.float32) - (0.5, 0.5, 0.5)) * size + self.pos

	def draw(self, canvas, camera_matrix, distortion, rvec, tvec, scale):
		# カメラ姿勢，内部パラメータを基に頂点を投影して描画するだけ
		projected, jacob = cv2.projectPoints(self.vertices * scale, rvec, tvec, camera_matrix, distortion)

		for i1, i2 in self.indices:
			pt1 = tuple(projected[i1].flatten().astype(numpy.int32))
			pt2 = tuple(projected[i2].flatten().astype(numpy.int32))
			cv2.line(canvas, pt1, pt2, (0, 255, 0), 2)

# スケール推定クラス
# はじめの100フレームにカメラが動いた領域の大きさをスケールの単位とする
class ScaleEstimater:
	def __init__(self):
		self.tvecs = []

	# 十分な数のカメラ姿勢が得られていなければ(False, None)を，そうでなければ(True, scale)を返す
	def estimate(self, tvec):
		if len(self.tvecs) < 100:
			self.tvecs.append(tvec)
			return False, None

		# カメラが動いた領域の大きさをスケールの単位とする
		tmin = numpy.min(self.tvecs[20:], axis=0)
		tmax = numpy.max(self.tvecs[20:], axis=0)
		return True, numpy.linalg.norm(tmax - tmin)


# SimpleAR ROSノード
# スペースを押すとカメラの前方に立方体を設置する
class SimpleARNode:
	def __init__(self):
		self.cubes = []
		self.scale_estimater = ScaleEstimater()
		self.frame = numpy.zeros((480, 640, 3), dtype=numpy.uint8)

		self.cv_bridge = cv_bridge.CvBridge()

		subs = [
			message_filters.Subscriber('/usb_cam_node/camera_info', CameraInfo),
			message_filters.Subscriber('/usb_cam_node/image_raw', Image),
			message_filters.Subscriber('/vodom', PoseStamped)
		]
		self.sync = message_filters.ApproximateTimeSynchronizer(subs, 10, 0.1)
		self.sync.registerCallback(self.callback)

	# 立方体をカメラ前方に配置する
	def put_cube(self, cam2world):
		# カメラ前方の位置をワールド座標系へ変換
		pos = cam2world.dot(numpy.float32((0, 0, 0.5, 1)))
		self.cubes.append(Cube(pos[:3], 0.15))

	# 画像，カメラ姿勢コールバック
	def callback(self, camera_info_msg, image_msg, camera_pose_msg):
		# カメラパラメータと画像
		camera_matrix = numpy.float32(camera_info_msg.K).reshape(3, 3)
		distortion = numpy.float32(camera_info_msg.D).flatten()
		frame = self.cv_bridge.imgmsg_to_cv2(image_msg, 'bgr8')

		# カメラ姿勢
		cam2world = tf_conversions.toMatrix(tf_conversions.fromMsg(camera_pose_msg.pose))
		world2cam = numpy.linalg.inv(cam2world)

		tvec = world2cam[:3, 3]
		rvec, jacob = cv2.Rodrigues(world2cam[:3, :3])

		# estimates the scale of the environment
		ret, scale = self.scale_estimater.estimate(tvec)
		if not ret:
			cv2.putText(frame, 'estimating scale...', (10, 30), cv2.FONT_HERSHEY_PLAIN, 2.0, (255, 255, 255), 2)
		else:
			# はじめの100フレームでカメラが動いた領域の大きさを20cmと仮定する
			scale = scale / 0.2
			for cube in self.cubes:
				cube.draw(frame, camera_matrix, distortion, rvec, tvec, scale)

		self.frame = frame
		self.cam2world = cam2world

	# AR画像の表示と立方体の設置
	def show(self):
		cv2.imshow('frame', self.frame)
		if cv2.waitKey(10) == ord(' '):
			self.put_cube(self.cam2world)


def main():
	rospy.init_node('simple_ar')
	node = SimpleARNode()
	while not rospy.is_shutdown():
		node.show()

if __name__ == '__main__':
	main()

各クラスを詳細に見ていきましょう．まずは，一番コアになるSimpleARNodeです．callbackで画像とカメラ姿勢を受け取りますがこれはカメラ→ワールド座標系への変換になるので，ワールド座標系に存在する3次元物体を画像上に表示するために反対のワールド→カメラ変換を求めておきます．その後，3次元点の投影にOpenCVのprojectPoints関数を使用するので，これに渡すように変換行列を平行移動・回転ベクトル表現へと変換します．あとは得られた平行移動・回転ベクトルを使って，表示したい3次元物体の頂点を画像上に投影するだけです．

class SimpleARNode:
	def __init__(self):
		# スケール推定器とROS関係の初期化
		...

	# 立方体をカメラ前方に配置する
	def put_cube(self, cam2world):
		# カメラ前方の位置(0.5m先)をワールド座標系へ変換
		pos = cam2world.dot(numpy.float32((0, 0, 0.5, 1)))
		# 変換後のワールド座標に立方体を設置する
		self.cubes.append(Cube(pos[:3], 0.15))

	# 画像，カメラ姿勢コールバック
	def callback(self, camera_info_msg, image_msg, camera_pose_msg):
		...

		# DSOが出力するのはカメラ座標からワールド座標への変換 (つまりワールド座標におけるカメラの姿勢)
		# ワールド座標における物体を画像上に投影するため，ワールド→カメラの変換(=カメラ→ワールドの逆行列)を求める
		cam2world = tf_conversions.toMatrix(tf_conversions.fromMsg(camera_pose_msg.pose))
		world2cam = numpy.linalg.inv(cam2world)

		# OpenCVのために平行移動，回転ベクトル表現に変換
		tvec = world2cam[:3, 3]
		rvec, jacob = cv2.Rodrigues(world2cam[:3, :3])

		# スケール推定
		ret, scale = self.scale_estimater.estimate(tvec)

		# 立方体表示
		... 

	...

3次元立方体の投影を行うのがCubeクラスです．表示自体は単純にcv2.projectPointsで立方体の頂点を画像上に投影して，その頂点を結ぶ直線を描画しているだけです．頂点とインデックスの生成は少し凝った書き方をしています．頂点は[(0, 1), (0, 1), (0, 1)]の直積を求めることで，[(0, 0, 0), (0, 0, 1), (0, 1, 0), (0, 1, 1), … (1, 1, 1)]となる頂点配列を生成しています．インデックスはその生成された頂点配列の中から隣接する頂点の組(距離==1となるような頂点の組)を抽出することで生成しています．多少短い代わりに読みにくい書き方だと思うので，普通にself.vertices = [(0, 0, 0), (0, 0, 1), …, (1, 1, 1)], self.indices = [(0, 1), (0, 2), … ]とベタ書きしてもいいと思います．

# 立方体表示クラス
class Cube:
	def __init__(self, pos, size):
		...
		# 立方体の3次元頂点を生成し，隣接する頂点(距離==1)を結ぶインデックスを作成する．
		self.vertices = numpy.int32(list(itertools.product((0, 1), (0, 1), (0, 1))))
		ivertices = enumerate(self.vertices)
		self.indices = [(i1, i2) for (i1, x1), (i2, x2) in itertools.combinations(ivertices, 2) if sum(abs(x1 - x2)) == 1]
		self.vertices = (self.vertices.astype(numpy.float32) - (0.5, 0.5, 0.5)) * size + self.pos

	def draw(self, canvas, camera_matrix, distortion, rvec, tvec, scale):
		# カメラ姿勢，内部パラメータを基に頂点を画像上に投影
		projected, jacob = cv2.projectPoints(self.vertices * scale, rvec, tvec, camera_matrix, distortion)

		# インデックスに従って，頂点を結ぶ線を引いていく
		for i1, i2 in self.indices:
			pt1 = tuple(projected[i1].flatten().astype(numpy.int32))
			pt2 = tuple(projected[i2].flatten().astype(numpy.int32))
			cv2.line(canvas, pt1, pt2, (0, 255, 0), 2)

先程も触れたとおり単眼カメラだけを使って環境のスケールを推定することは原理上不可能です．なので，ここでは適当にカメラがはじめの100フレームで動く領域を1単位とした大きさを基に，立方体の表示をすることにします．もちろん，推定されるスケールはカメラの動き方に大きく依存するため，カメラが動かない場合なんかにはうまく行かない方法なので，今回のデータだけに向けた方法です．

# スケール推定クラス
# はじめの100フレームにカメラが動いた領域の大きさをスケールの単位とする
class ScaleEstimater:
	# 十分な数のカメラ姿勢が得られていなければ(False, None)を，そうでなければ(True, scale)を返す
	def estimate(self, tvec):
		if len(self.tvecs) < 100:
			self.tvecs.append(tvec)
			return False, None

		# カメラが動いた領域の大きさをスケールの単位とする
		tmin = numpy.min(self.tvecs[20:], axis=0)
		tmax = numpy.max(self.tvecs[20:], axis=0)
		return True, numpy.linalg.norm(tmax - tmin)

SimpleARNodeでの描画時には，推定されたスケール(カメラの移動した領域の大きさ)を0.2mと仮定して，0.15mの大きさの立方体を表示するようにしています．スペースを押すたびにカメラ前方に立方体を置きます．それでは実行してみましょう！

rosrun image_transport republish compressed in:=/usb_cam_node/image_raw raw out:=/usb_cam_node/image_raw

roscd dso_ar
rosrun dso_ros dso_live image:=/usb_cam_node/image_raw calib=params/philips.txt

rosbag play ar_100.bag

rosrun dso_ar simple_ar.py

スケール推定がとてもプアなので，たまにうまく行かないことがあるかもしれませんが，一応簡単なARが実現できていますね．次はDirect系visual odometryの特徴である密な点群の推定を活かしたスケール推定と環境とのインタラクションを行います．

[:en]カメラ姿勢が得られれば[:]