某位置情報プログラミング技術本に、磁気センサ+加速度センサの値から、カメラの向きの極座標を得るサンプルが載ってたのですが、
カメラを特定の方向、特定の仰角に向けたまま、カメラの軸を中心に回転させると、本来ならAzimuth(Yah)、Pitchは固定でRollだけ変わって欲しいのに、回転の結果デバイスの上端が異なる方角を向くとAzimuthも変化してしまったりと全然納得のいく結果を得られませんでした。
そこで自前計算して、納得の行く結果の出るロジックを自作しましたので、共有します。
元の本のコードをかなり参考にしてますが、Objective-C => c#への変更をしてますし、また納得の行く動きをするようかなり手を入れてますので、著作権的には大丈夫…だと思います…。
また、このコードを導くためにかなりゴリゴリ座標計算しましたが、最後の最後でどうしても位相が半分ズレてしまったり、回転の向きが逆になってしまうところが出てしまい、いくら考えても判らないので半位相を加えたり回転の向きを負の値にしてよし、としてしまったので、計算式の共有は省略します…。
なお、カメラはiPhoneと同じ位置についているものとし、カメラ座標系での上面方向(Z軸正方向)は縦持ちの時のデバイス上面方向とします。
カメラを回転させてもAzimuth/Pitchは変化しないような計算結果にしてますので、もし横持ちカメラで利用したい時は、Rollだけ1/4位相ズレで読んでもらえば大丈夫です。
Xamarin.iOSと書きましたが、Androidでも、磁気センサや加速度センサの座標系が同じならば、そのまま使えるはず。
角度の値は、全てラジアンです。
public void CalcurateOrientation () {
//クラスのプロパティとして、以下が定義済みとします。
//MagnetX, MagnetY, MagnetZ: 磁気センサの値
//AccelX, AccelY, AccelZ: 加速度センサの値
//AzimuthOffset: 磁北と真方位での北の間の角度差
//AzimuthOffset = (heading.TrueHeading - heading.MagneticHeading) * Math.PI / 180.0;
double a, b, c, d, e, f, g, h, i;
//磁気センサ、加速度センサの絶対値を出す
double accelAbsolute = GetAbsoluteValue3D (AccelX, AccelY, AccelZ);
double magnetAbsolute = GetAbsoluteValue3D (MagnetX, MagnetY, MagnetZ);
//世界座標系<=>デバイス座標系間の回転行列の値を出す。詳細は省略。
a = -(MagnetY * AccelZ - MagnetZ * AccelY) / (accelAbsolute * magnetAbsolute);
b = -(MagnetZ * AccelX - MagnetX * AccelZ) / (accelAbsolute * magnetAbsolute);
c = -(MagnetX * AccelY - MagnetY * AccelX) / (accelAbsolute * magnetAbsolute);
d = (MagnetX * (AccelY * AccelY + AccelZ * AccelZ) -
MagnetY * AccelX * AccelY - MagnetZ * AccelZ * AccelX)
/ (accelAbsolute * accelAbsolute * magnetAbsolute);
e = (MagnetY * (AccelZ * AccelZ + AccelX * AccelX) -
MagnetZ * AccelY * AccelZ - MagnetX * AccelX * AccelY)
/ (accelAbsolute * accelAbsolute * magnetAbsolute);
f = (MagnetZ * (AccelX * AccelX + AccelY * AccelY) -
MagnetX * AccelZ * AccelX - MagnetY * AccelY * AccelZ)
/ (accelAbsolute * accelAbsolute * magnetAbsolute);
g = -AccelX / accelAbsolute;
h = -AccelY / accelAbsolute;
i = -AccelZ / accelAbsolute;
//もしカメラ座標ではなく、デバイス座標での極座標値を取りたい場合は、
//ここのコメントアウトされてる部分のロジックを使う
/*Azimuth = Math.Atan2 (b, e) + AzimuthOffset;
while (Azimuth < 0)
Azimuth += Math.PI * 2.0;
Pitch = Math.Asin (h);
Roll = Math.Atan2 (-g, i);*/
//ここはカメラ座標(縦持ち)
//どうしても半位相ズレたのでPI引いている
Azimuth = Math.Atan2 (c, f) - Math.PI + AzimuthOffset;
while (Azimuth < 0)
Azimuth += Math.PI * 2.0;
Pitch = Math.Asin (-i);
Roll = Math.Atan2 (-g, h);
}