MAVSDKでSITLのドローンからテレメトリーを取得してCesiumJSにアニメーション表示してみた 〜Part 3 座標系解決編〜
こんにちは。
Part1、2でMAVSDKで取得したテレメトリーをCesiumJSに表示するところまで試してみました。
tomofiles.hatenablog.com
tomofiles.hatenablog.com
その時、3Dモデルの表示のされ方が少し変だったと思いますが、以下の記事でその原因を究明しました。
今回は、この座標系の知識を利用して、Part1で紹介したmavsdk_tryを改修してみたいと思います。
まず、前回ネットからコピペして利用したロジックを以下に再掲します。
// mavsdk_try/static/App.js #31-47 var quatlocal = new Cesium.Quaternion( czml.orientation.unitQuaternion[1], czml.orientation.unitQuaternion[2], czml.orientation.unitQuaternion[3], czml.orientation.unitQuaternion[4]) var pos = Cesium.Cartesian3.fromDegrees( czml.position.cartographicDegrees[1], czml.position.cartographicDegrees[2], czml.position.cartographicDegrees[3]); var mtx4 = Cesium.Transforms.eastNorthUpToFixedFrame(pos); // ★ var mtx3 = Cesium.Matrix4.getMatrix3(mtx4, new Cesium.Matrix3()); var base = Cesium.Quaternion.fromRotationMatrix(mtx3) var quat = Cesium.Quaternion.multiply(base, quatlocal, new Cesium.Quaternion()) // ★ czml.orientation.unitQuaternion[1] = quat.x; czml.orientation.unitQuaternion[2] = quat.y; czml.orientation.unitQuaternion[3] = quat.z; czml.orientation.unitQuaternion[4] = quat.w;
★を新たに付与しました。
★ ENUとNED
前回の"座標系とCesiumJS"の記事で説明しましたが、ENUフレームとNEDフレームの考慮がこのロジックに不足しています。
当該座標(測地座標の緯度、経度、高度)におけるグローバルな回転を★で計算していますが、ENUフレームを利用しています。しかし、MAVSDKから取得できるテレメトリーがNEDフレームのため、フレームが合っていません。その辺の考慮がないため、3Dモデルの表示が変なのです。
解決策としては、原点のフレームをCesium.Transforms.northEastDownToFixedFrameを使用してNEDフレームとして計算するか、NED→ENUのフレーム変換をしてあげる必要があります。
ただ、前回の記事で書きましたが、NEDフレームは3Dモデルの表示に影響を与えます。そのため、CesiumJSでの表示ではENUフレームで統一しておいたほうが、色々問題が少なそうなので、"表示のために"ENUフレームに変換する方式を取りましょう。
フレーム変換についても、前回の記事で紹介しました。
ENUとNEDは、xとyが逆となり、zが正負逆になり、90度回転させることで、変換出来ます。
https://stackoverflow.com/questions/53169107/quaternion-frame-conversions-ned-enu-eun
そのため、mavsdk_tryにもそのロジックを追加し、フレーム変換します。
上記、もろもろの考慮を追加した、新しいロジックは以下となります。
// mavsdk_cesium/static/App.js #43-69 // 地球固定座標での回転を計算 var pos = Cesium.Cartesian3.fromDegrees( telemetry.position.cartographicDegrees[1], telemetry.position.cartographicDegrees[2], telemetry.position.cartographicDegrees[3]); var mtx4 = Cesium.Transforms.eastNorthUpToFixedFrame(pos); var mtx3 = Cesium.Matrix4.getMatrix3(mtx4, new Cesium.Matrix3()); var base = Cesium.Quaternion.fromRotationMatrix(mtx3); // ローカル座標での回転を計算(NED→ENU) var quatlocal = new Cesium.Quaternion( telemetry.orientation.unitQuaternion[2], // ★ telemetry.orientation.unitQuaternion[1], // ★ -telemetry.orientation.unitQuaternion[3], // ★ telemetry.orientation.unitQuaternion[4]); var quat90 = Cesium.Quaternion.fromAxisAngle( new Cesium.Cartesian3(0, 0, 1), Cesium.Math.toRadians(90) ); var quatlocal = Cesium.Quaternion.multiply(quatlocal, quat90, new Cesium.Quaternion()); // ★ // 回転を掛け合わせる var quat = Cesium.Quaternion.multiply(base, quatlocal, new Cesium.Quaternion());
★の辺りが、x、y、zの軸の入れ替えと90度の回転を行っている箇所です。
x、y、zの軸の入れ替えは、クォータニオンの要素を入れ替えるだけなので、非常に簡単ですね。90度の回転も、z軸に対する90度の回転を行ったクォータニオンを生成して、ローカルなクォータニオンと掛けることで、実現できます。
以下に、上記修正を加えたmavsdk_cesiumというリポジトリーをGithubに公開しました。
最後に
比較動画を撮りました。
左が改修後、右が改修前です。
明らかにドローンの3Dモデルの姿勢が変だったのがわかると思います。
※後半、動画がカクカクしてますが、キャプチャソフトのせいなので無視してください。
◆Previous Part
tomofiles.hatenablog.com