OSOYOO PI CARキット ROS2化 #11 – ORB_SLAM3 姿勢推定の改良 その2

OSOYOO PI CAR

前回は、カメラのIMU情報をROS2標準メッセージに変換して配信しました。
今回は、その情報を取得してORB_SLAM3に入力します。
ORB_SLAM3は、撮影時間、IMUの取得時間をキーにして撮影時のIMU情報を近似、生成させているものと想像します。
今回は、そのあたりの仕組みを「picar_map_mono」ノードに追加します。
ついでにORB_SLAM3の姿勢情報を元にウェブ画面にroll/pitch情報を反映しょうと思います。

■ IMU情報の付加

機能追加で実施する処理は大きく２つです。
１つ目は、IMU情報のsubscribeを行い、ORB_SLAM3が必要なIMU情報を生成すること。
２つ目は、ORB_SLAMにIMU情報を設定すること。

〇 IMU情報のsubscribe

PicarMapMonoNode()のコンストラクターで、下記緑部のようにIMU情報をsubscribeする処理を付加します。「/picar_cam/imu」は、前回設計したカメラ IMU配信用のpublisherです。

using Imu = sensor_msgs::msg::Imu;

class PicarMapMonoNode : public rclcpp::Node
{
public:
    PicarMapMonoNode(ORB_SLAM3::System *pslam) : Node("picar_map_mono")
    {
:
:
        imu_subscriber_ = this->create_subscription<Imu>(
            "/picar_cam/imu", 10,
            std::bind(&PicarMapMonoNode::trackImu, this, _1));
:
:

実体のメソッドは、最新のIMU情報を20個ほど保持するキューを作成するメソッドです。
赤字部は、サイズがCNF_IMU_ARRAY_SZより大きくなった場合、最も古いIMU情報を破棄します。
青字部は、最新のIMU情報を一番後ろに追加します。
ORB_SLAM3::IMU::Pointは、ORB_SLAM3のIMU情報の形式となります。
ポイントは、メッセージ生成時間よりキーとなる時間を算出します。
前回設計したカメラ IMUの配信ノードは、取得時間を調整後、ヘッダーにセットしてありますので、これを浮動小数点形式時間（１秒未満は、小数点以下となる）に変換してセットします。

    /*
     *  purpose :   entry to track imu to adjust imu.
     */
    void trackImu(const Imu::SharedPtr msg)
    {
        if (vimu_meas_.size() > CNF_IMU_ARRAY_SZ) {
            vimu_meas_.erase(vimu_meas_.begin());
        }
        vimu_meas_.push_back(
            ORB_SLAM3::IMU::Point(
                msg->linear_acceleration.x,
                msg->linear_acceleration.y,
                msg->linear_acceleration.z,
                msg->angular_velocity.x,
                msg->angular_velocity.y,
                msg->angular_velocity.z,
                (double)msg->header.stamp.sec +
                (double)msg->header.stamp.nanosec / 1000000000.0)
        );
    }

〇 ORB_SLAM3へのIMU情報設定

イメージトラッキング IFに下記のようにIMUキューをセットします。
緑部分が、以前からの設計変更部です。
IMUキューが存在すれば、TrackMonocular()メソッドにORB_SLAM３形式の最新IMU情報をセットします。IMUが未装着等の理由で情報がない場合には、画像だけで動作するようにしています。

/*
     *  purpose :   entry to track image to build map.
     */
    void trackImage(const Image::SharedPtr msg)
    {
        Sophus::SE3f    pose;

        /* this converts ros raw image to cv:Mat. to display cloud points.  */
        try
        {
            cvimg_ = cv_bridge::toCvCopy(msg);
        }
        catch (cv_bridge::Exception &e)
        {
            RCLCPP_ERROR(this->get_logger(), "cv_bridge error %s", e.what());
            return;
        }
        if (vimu_meas_.size() > 0) {
            pose = slam_->TrackMonocular(cvimg_->image,
                (double)msg->header.stamp.sec +
                (double)msg->header.stamp.nanosec / 1000000000.0,
                vimu_meas_);
        } else {
            pose = slam_->TrackMonocular(cvimg_->image,
                (double)msg->header.stamp.sec +
                (double)msg->header.stamp.nanosec / 1000000000.0);
        }
        updateInfo(pose, msg->header.stamp);
    }

■ Web Roll/Pitch情報表示

Roll/Pitch情報は、以前より表示機能は作ってあったのですが機能してませんでしたが、ORB_SLAM3の姿勢情報（推測）を元に、表示しようと思います。
姿勢情報の表示は、「/picar_map_mono/pose」をsubscribeして、得た姿勢情報(クォータニオン）をroll/pitchに変換するだけです。ここでは計算式を書かないですが、「/picar_map_mono/pose」は、ZXY系の姿勢情報ですので、下記のような計算を行えば、roll/pitchに変換できます。
元の計算式は、色々なサイトで説明されていますので、そちらを参照していただけると助かります。
青部は、JavaでROS2のトピックをsubscribeする手続きとなります。
緑部は、クォータニオン（ZXY系）をroll/pitchに変換する計算式になります。
gorymeter()は、imu_roll/imu_pitchの値を元にメーターを表示する独自関数です。

    var pose_listener = new ROSLIB.Topic({
        ros : ros,
        name : '/picar_map_mono/pose',
        messageType : 'geometry_msgs/msg/PoseStamped'
    });

    pose_listener.subscribe(function(message) {
        var q = message.pose.orientation;
        var thx = Math.asin(2 * q.y * q.z + 2 * q.x * q.w);
        if (Math.cos(thx) != 0) {
            thy = Math.atan(-(2 * q.x * q.z - 2 * q.y * q.w) /
                    (2 * q.w * q.w + 2 * q.z * q.z - 1));
            thz = Math.atan(-(2 * q.x * q.y - 2 * q.z * q.w) / 
                    (2 * q.w * q.w + 2 * q.y * q.y - 1));
        } else {
            thy = 0.0;
            thz = Math.atan((2 * q.x * q.y + 2 * q.z * q.w) /
                (2 * q.w * q.w + 2 * q.x * q.x - 1));
        }
        imu_roll = thx * 180.0 / cnf_pi;
        imu_pitch = thz * 180.0 / cnf_pi;
        gyrometer();
    });

〇 テスト

上記を反映し、Webで動作確認を行いました。
結果としては、概ね動きに問題はないように見えました。
傾きの方向も画像にあっているように思います。

少々カメラマウントの水平化機能で、rollもpictchも一瞬しかでないですが、ちゃんと動いているようでした。

■ 次回

今回で大体必要な機能はそろったように思います。
Raspberry PI 3で継続するのであれば、分散コンピューティングは必須となりますし、その場合、ネットワークスピードが問題となります。
個人的には、Raspberry PI 3に限界を感じています。
これ以降の制御はRaspberry PI 5へ換装し、電源的に問題なければ、乗り換えてやってみようと思います。

タイトルとURLをコピーしました