趣味で作っている ATOTO X10 用カーナビで、フロントカメラの映像にこれから走るルートを重ねて描く

XFacebookはてブ
趣味で作っている ATOTO X10 用カーナビで、フロントカメラの映像にこれから走るルートを重ねて描く

個人的な趣味として、ATOTO X10 という車載 Android 機で動かす自分専用のカーナビアプリを作っています。会社の業務とは独立したプロジェクトであり、自分の車 1 台で使うためだけのアプリです。

作り始めた動機は地図の視点でした。Google マップは目的地を設定しないと、北を上に固定した見下ろし表示のままになります。目的地を決めずに走るときも、進行方向を上にし、斜めに見下ろして自車に追従する視点にしたかったため、Mapbox の Navigation SDK にある Free Drive（目的地なしで走行するモード）を利用して、その視点を常に保つ画面から作り始めました。

そこへルート案内やリアカメラの表示を順次追加し、現在は X10 に付属するフロントカメラの映像へ、これから走行するルートを帯と矢印で重ねて描画しています。AR 専用のライブラリは使用していません。地面を平面とみなした幾何学的な投影計算と、実車での位置合わせだけで構築しました。この記事では、その投影計算と実装コードを紹介します。

アプリの構成とフロントカメラの出し方

アプリは Kotlin で書いており、地図描画と案内には Mapbox の Navigation SDK（3.32）、目的地の検索には Search SDK を採用しています。動作環境である X10 は Android 13 で動作し、画面解像度は 1280×800 の横画面固定です。自分専用の X10 に導入する前提であるため、対応する Android のバージョンも CPU アーキテクチャも実機に合わせて絞り込んでいます。全体のコード規模は 2,400 行ほどです。

フロントカメラの映像は、画面の右側に 16:9 の表示領域を確保して配置し、地図画面は左側へ寄せています。案内中はカメラ映像を常時表示する設定を既定としていますが、この設定を無効化すると、次の案内地点（曲がり角）の 300m 手前（設定により 100〜500m で調整可能）に接近したときだけ映像が自動で現れるように切り替わります。

カメラ映像にルートを重ねる処理は、次の 3 段階に分かれています。

  • ルートの線（緯度経度の列）を、車から見た地面の点（右へ何 m、前へ何 m）に変換する
  • 地面の点を、カメラ映像上のピクセル座標に投影する
  • 投影した座標をつなぎ合わせ、帯と矢印のポリゴンとして描画する

ルートを「車から見た地面の点」に直す

Mapbox から受け取るルート情報は緯度経度の座標列です。まず、始点からの距離を指定して任意の位置を取り出せる RoutePath を構築し、現在地から先にあるルートを、自車を原点としたローカル座標として抽出します。

/** 車から見た地面の点 (m)。x は右、z は前 */
data class Ground(val x: Double, val z: Double)

/** 数百 m の範囲なので平面で近似する。東 x / 北 y (m) */
private fun local(origin: Point, p: Point): Pair<Double, Double> {
    val east = Math.toRadians(p.longitude() - origin.longitude()) * cos(Math.toRadians(origin.latitude())) * EARTH_RADIUS_M
    val north = Math.toRadians(p.latitude() - origin.latitude()) * EARTH_RADIUS_M
    return east to north
}

private fun toGround(origin: Point, heading: Double, p: Point): Ground {
    val (e, n) = local(origin, p)
    return Ground(x = e * cos(heading) - n * sin(heading), z = e * sin(heading) + n * cos(heading))
}

描画対象として扱うのは前方 200m までの範囲にとどまるため、地球の曲率は無視し、緯度経度の差分を東西・南北方向のメートル単位へ平面近似で変換しています。得られた平面座標を現在の進行方向角の分だけ逆回転させることで、車から見た左右方向（x）と前方方向（z）の距離が得られます。

進行方向の角度は、現在地の前後 5m に位置する 2 点を結んだ直線から算出しています。ルートの折れ線を構成する頂点ごとに向きを計算すると、頂点を通過する瞬間に角度が不連続に急変してしまうためです。

抽出する座標の間隔は 2m 刻みとしていますが、それとは別に元のルートの折れ線が持つ頂点も必ず配列に保持しています。等間隔のサンプリング点だけに頼ると交差点の角にあたる頂点が欠落し、角の形状が丸まって崩れてしまうからです。

帯は左の車線にずらす

Navigation SDK から得られるルートの線は道路の中央を通っています。そのまま描画すると帯がセンターラインの上に重なってしまうため、進行方向に対して左へ 1.5m オフセットさせ、幅 2.2m の帯として生成しています。日本の道路は左側通行であるため、この補正によって自車が走行している車線の上に帯が重なります。

描画の開始位置は車の 3m 先に設定しています。それより手前の地面はフロントガラス下部やボンネットに遮られてカメラ映像に映らないためです。描画の終了位置は次の曲がり角の 25m 先とし、その先端に矢印の頭を配置します。交差点を曲がった先まで少し伸ばして描くことで、どちらの方向へ曲がるのかが帯の形状から一目で分かります。

地面の点を映像の上に映す

地面上の相対座標が求まったら、次にそれがカメラ映像のどのピクセルに映るかを計算します。カメラが地面から一定の高さに設置され、わずかに下を向いていると仮定した、ピンホールカメラモデルに基づく投影計算です。

fun project(g: Ground): Screen? {
    val x = g.x * cos(yaw) - g.z * sin(yaw)
    val forward = g.x * sin(yaw) + g.z * cos(yaw)
    val depth = heightM * sin(pitch) + forward * cos(pitch)
    if (depth < NEAR_M) return null
    val down = heightM * cos(pitch) - forward * sin(pitch)
    return Screen(cx + fx * x / depth, cy + fy * down / depth)
}

ここで depth はカメラの光軸方向（見通し方向）の距離であり、down はその光軸から垂直に下へ離れた距離です。この down を depth で除算して焦点距離（px）を乗じると、映像の中心からのピクセル単位のズレが求まります。

具体的な数値で確認してみます。横 1280px の映像で水平画角が 90 度の場合、焦点距離は 640px となります。カメラが水平を向いており地上高が 1m だとすると、10m 先の地面は、画面中央から 640 × 1 ÷ 10 = 64px 下の位置に映ります。遠方の点になるほど除数が大きくなるため、投影位置は次第に水平線へと収束します。この計算モデルの妥当性を確認するため、次のようなユニットテストを用意しています。

// 1280x720 に同じ比率の映像、画角 90 度、水平線は真ん中 (= 水平に向いている)
val p = GroundProjector.create(1280, 720, 1280, 720, 90.0, horizon = 0.5, heightM = 1.0, yawDeg = 0.0)
// 焦点距離 640px: 10m 先の地面は真ん中から 64px 下
val near = p.project(Ground(0.0, 10.0))!!
assertEquals(360.0 + 64.0, near.v, 1e-6)

カメラの下向きの角度は水平線の位置から求める

投影計算を行うには、カメラの俯角（下向きの傾き角度 pitch）が必要です。しかし、車載カメラの物理的な取り付け角度を直接測定するのは困難です。そこで、角度を直接指定する代わりに、プレビュー映像の中で水平線がどの垂直位置に見えるかを指定する方式を採りました。

// 水平線が真ん中より上に見えるほど、カメラは下を向いている
val pitch = atan((cy - horizon * height) / fy)

無限遠にある地表面の点は、すべて水平線上に収束します。水平線が画面の中心よりも上方に見えている場合、その変位量に応じてカメラが下を向いていると判断できます。この幾何学的な関係を利用すれば、実際のカメラ映像を見ながら画面上の水平線を合わせるだけで、適切な俯角を逆算できます。

画角はカメラの情報から求める

焦点距離を算出するには、カメラの水平画角を取得しなければなりません。設定画面で「自動」を選択した場合、Android の Camera2 API 経由でレンズの焦点距離とイメージセンサーの物理幅を取得し、次のように計算します。

val focal = c[CameraCharacteristics.LENS_INFO_AVAILABLE_FOCAL_LENGTHS]?.firstOrNull() ?: return null
val sensorWidth = c[CameraCharacteristics.SENSOR_INFO_PHYSICAL_SIZE]?.width ?: return null
return Math.toDegrees(2 * kotlin.math.atan(sensorWidth / 2.0 / focal))

安価な車載カメラなど、ハードウェアがこれらのメタデータを返さない場合は既定値として 100 度を適用し、実車と合わないときは設定画面から 5 度刻みで手動調整できるようにしています。

実車での位置合わせ

投影の計算式が正しくても、カメラの取り付け位置や向きは車種や個体によってばらつきます。そのため、実車上でキャリブレーションできるよう、次の項目を設定画面から変更可能にしました。

  • 水平線の高さ（映像の上から何 % か）
  • カメラの画角
  • カメラの高さ（地面から何 cm か）
  • カメラの左右の向き
  • 映像の傾き
  • 映像の左右反転

設定画面で「位置合わせモード」を有効にすると、案内中でなくてもフロントカメラの映像が常時表示され、その上にキャリブレーション用のガイド線が重なります。ガイド線として表示するのは、水平線を示す点線、自車線の左右境界を表す 2 本の直線、そして 10m・20m・30m・50m 先の距離を示す横線です。これらのガイド線も、ルートの帯と全く同じ投影計算を用いて描画しています。したがって、映像内の実際の白線や遠方の地平線にガイド線が重なるようパラメータを調整すれば、ルートの帯も道路上へ正確に投影されます。

水平線の高さとカメラのヨー角（左右の向き）は、プレビュー映像を指でドラッグすることで直感的に調整できます。上下ドラッグで水平線の位置を上下させ、左右ドラッグでカメラの向きを回転させます。左右の操作では、指の移動量に合わせてガイド線が自然に追従するよう、画角から算出した 1 ピクセルあたりの角度を用いて回転量を換算しています。また、カメラがわずかに傾いて取り付けられており地平線が斜めに見える場合に備えて、0.5 度単位で映像の傾きを補正するボタンも用意しました。

実機である X10 において、外付けフロントカメラが Android システム上でどのカメラ ID として認識されるかは、事前調査の段階では確定できませんでした。そのため、利用するカメラデバイスを設定から自由に選択できる構造にしています。さらに、バックカメラ用のセンサーを流用した製品などで映像が鏡像になってしまう場合に備え、左右反転の切り替えスイッチも実装しました。

位置の更新の間をなめらかに動かす

自車がルート上のどの地点まで進んだかという情報は、Navigation SDK から進行状況のイベントとして周期的に届きます。しかし、このイベントを受信したタイミングだけで描画位置を更新すると、カメラ映像は滑らかに動いているのに対し、ルートの帯だけが断続的に停止してはジャンプするぎこちない動きになってしまいます。

そこで、直近に受信した進捗位置へ現在の車速と経過時間を掛け合わせて目標位置を予測し、画面上の描画位置を毎フレーム少しずつその目標値へ近づける線形補間を導入しました。

val elapsed = (SystemClock.uptimeMillis() - baseTime) / 1000.0
val target = baseAlong + speedMps * min(elapsed, DriveCamera.AR_EXTRAPOLATE_MAX_S)
// 進捗が来るたびに飛ばないよう、少しずつ追いつく。大きくずれたら (リルートなど) そのまま合わせる
val along = shownAlong?.takeIf { abs(target - it) < DriveCamera.AR_SNAP_M }
    ?.let { it + (target - it) * DriveCamera.AR_SMOOTHING } ?: target

外挿によって先へ進める時間は、最大 1.5 秒分までに制限しています。トンネル進入などで GPS 信号が途切れた際、帯だけが単独で前進し続ける現象を防ぐためです。また、リルートなどで目標位置と現在の描画位置との間に 30m 以上の乖離が生じた場合は、補間を行わずに目標位置へ即座に同期（スナップ）させます。

もう一つの注意点として、Navigation SDK が内部計算で返す走行距離と、アプリ側で緯度経度の配列から積算したルート全長との間には、測地計算の誤差が生じます。このズレによって終端で位置が合わなくなるのを防ぐため、SDK が通知する「走破距離 ÷ ルート全長」という進行比率を取得し、その比率をアプリ側で積算した全長に掛け合わせて正確な描画位置を決定しています。

机の上で走らせて確かめる

実車へ持ち込む前の机上デバッグにも工夫が必要でした。Android エミュレータの標準機能（geo fix コマンドや NMEA データの送信）で擬似的な GPS 位置を流し込むと、方位角（ベアリング）が 0 に固定されてしまい、進行方向を上にする視点の挙動を検証できません。そこで、Mapbox SDK に用意されているリプレイ機能（定義されたルートに沿って位置情報をシミュレーション再生する仕組み）を組み込み、開発機上で擬似走行を行いながらカメラ重畳や追従の動作を確認できるようにしました。

実際に走ってみて

完成したアプリを X10 に導入し、実車に載せて走行テストを行いました。投影した帯は道路の白線に沿って滑らかに重なり、カーブの手前でも遅延なく追従して描画されます。

安全上の観点から、運転中にナビの画面を注視するわけにはいきません。それでも、曲がり角に差し掛かる直前に周辺視野へ案内が入ってくるだけでも、曲がるべき交差点を直感的に把握しやすくなりました。同乗者からも「曲がる場所が分かりやすい」と好評を得ています。

専用の AR ライブラリを導入しなくても、カメラの設置高や画角、水平線の位置を合わせられれば、地表面を平面とみなした幾何計算だけで実用的なガイド映像を描けます。対象とする車種やハードウェアが 1 台に定まっている個人開発だからこそ、過剰な汎用性を削ぎ落とし、2,400 行というコンパクトなコードで目的の体験を実現できました。実機に特化した調整の楽しさを味わえる題材として、車載 Android 機でのアプリ開発は有意義な取り組みです。

望月 涼太 / polidog

望月 涼太 / polidog

Co-Founder / Web Engineer

関連記事