DSOでAR ~ 2. ROSパッケージインストール

dso_ros

DSO本体に続いて,DSOをラップするdso_rosパッケージをインストールします.基本的にはDSO本体のパスを指定してパッケージをcloneするだけですが,デフォルトだとcatkinが使えないのでブランチを切り替えておきましょう.

echo "export DSO_PATH=/home/~~/dso" >> ~/.bashrc
source ~/.bashrc

cd catkin_ws/src
git clone https://github.com/JakobEngel/dso_ros.git
cd dso_ros
checkout catkin

cd ~/catkin_ws
catkin_make

無事catkin_makeに成功したら,なにはともあれ動作確認してみましょう.うちの庭で撮った小さなrosbagデータがあるのでそれでARプログラムを試していきます.(※bagファイルの最後の数秒間は動きが激しいのでDSO失敗します.)

wget http://tenteroring.luna.ddns.vc/tenteroring/workspace/ar_100.bag.tar.gz
tar xzvf ar_100.tar.gz

チュートリアルプログラムのパッケージをcloneします.このパッケージにはこれからこれから説明するARプログラムとbagファイルの撮影に使ったカメラのパラメータファイルが含まれています.

cd ~/catkin_ws/src
git clone https://github.com/koide3/dso_ar.git
cat dso_ar/params/camera.txt

# カメラパラメータファイル(dso_ar/params/camera.txt)
# RadTan 476.6422750947405 478.8132333982842 323.2965707981903 253.3289577203022 0.08013756477381188 -0.1444825496438046 -0.001205643975243196 0.002542960319388637
# 640 480
# crop
# 640 480

早速実行してみましょう.このrosbagファイルは画像をsensor_msgs/CompressedImageとして保存しているため,まずはこれをraw画像としてrepublishします.今後このrosbagを再生するときには常にこのrepublishを行います.

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

次に別のターミナルでDSOノードを実行します.通常のrosrunで画像トピックとカメラパラメータファイルを指定します.

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

最後に更に別のターミナルでrosbagを再生します.

rosbag play ar_100.bag

カメラ姿勢と点群をpublishする

さて,このdso_rosパッケージ,正しく動いていそうですがあくまで”minimum example”として作られているので,推定した姿勢や点群は一切ROS上に送り出されていません.なので,他のROSノードからこれらの情報を得たい時にはソースを若干変更する必要があります.DSOの内部データを見るためには,dso::IOWrap::Output3DWrapperクラスから派生したクラスを作ってシステムに登録します.Output3DWrapperにはDSOの様々な内部データに対応するコールバックメソッドが用意されており,必要なデータに対するメソッドをオーバーライドすることでそのデータを受け取ることができます.それでは,カメラ姿勢と点群をpublishするようにdso_ros/src/main.cppを変更していきましょう.(手っ取り早く変更後のものが使用したい人はこのリポジトリをcloneしてcatkinブランチをチェックアウトしてください.)

推定された姿勢を得るには,Output3DWrapper::publishCamPoseをオーバーライドします.メソッドの第一引数に与えられるFrameShellのcamToWorldフィールドがカメラ座標から世界座標への変換になっているので,基本的にこれをそのままpublishするだけでOKです.

class PosePublisher : public dso::IOWrap::Output3DWrapper {
public:
  PosePublisher(ros::NodeHandle& nh)
    : pose_pub(nh.advertise<geometry_msgs::PoseStamped>("vodom", 10))
  {}

  // publish estimated pose as geometry_msgs::PoseStamped
  void publishCamPose(FrameShell* frame, CalibHessian* HCalib) override {
    Eigen::Quaterniond quat(frame->camToWorld.rotationMatrix());
    Eigen::Vector3d trans = frame->camToWorld.translation();

    geometry_msgs::PoseStamped pose_msg;
    pose_msg.header.stamp = ros::Time(frame->timestamp);
    pose_msg.header.frame_id = "vodom";

    pose_msg.pose.position.x = trans.x();
    pose_msg.pose.position.y = trans.y();
    pose_msg.pose.position.z = trans.z();
    pose_msg.pose.orientation.w = quat.w();
    pose_msg.pose.orientation.x = quat.x();
    pose_msg.pose.orientation.y = quat.y();
    pose_msg.pose.orientation.z = quat.z();

    pose_pub.publish(pose_msg);
  }

private:
  ros::Publisher pose_pub;
};

点群データを得るにはOutput3DWrapper::publishKeyframesをオーバーライドします.DSOは内部データとしては点群そのものではなく,各画像におけるピクセルのuv座標とinverse depth(1 / depth)を保持しているため,点群の形にするにはカメラパラメータを使ってこの[u, v, d]から[x, y, z]に変換してやる必要があります.ここを参考に下のPointCloudPublisherクラスを作成しましょう.pcl::PointCloudのpublishにはpcl_rosが必要なので,これの依存をpackage.xmlとCMakeLists.txtに追加しておいてください.

#include <pcl_ros/point_cloud.h>
#include <sensor_msgs/PointCloud2.h>

class PointCloudPublisher : public dso::IOWrap::Output3DWrapper {
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  PointCloudPublisher(ros::NodeHandle& nh)
    : points_pub(nh.advertise<sensor_msgs::PointCloud2>("points", 10))
  {}

  // publish point cloud
  void publishKeyframes(std::vector<FrameHessian*> &frames, bool final, CalibHessian* HCalib) {
    if(!final) {
      return;
    }

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>());
    for(const auto* frame : frames) {
      // convert [u, v, depth] to [x, y, z]
      double fxi = HCalib->fxli();
      double fyi = HCalib->fyli();
      double cxi = HCalib->cxli();
      double cyi = HCalib->cyli();
      auto const & cam2world=  frame->shell->camToWorld.matrix3x4();

      for (auto const* p : frame->pointHessiansMarginalized) {
        float depth = 1.0f / p->idepth;
        auto const x = (p->u * fxi + cxi) * depth;
        auto const y = (p->v * fyi + cyi) * depth;
        auto const z = depth * (1 + 2*fxi);

        Eigen::Vector3d world_point = cam2world * Eigen::Vector4d(x, y, z, 1.f);

        pcl::PointXYZ pt;
        pt.getVector3fMap() = world_point.cast<float>();
        cloud->push_back(pt);
      }
    }

    cloud->header.frame_id = "world";
    cloud->width = cloud->size();
    cloud->height = 1;
    cloud->is_dense = false;
    points_pub.publish(cloud);
  }

private:
  ros::Publisher points_pub;
};

デフォルトのdso_liveでは画像のタイムスタンプがキーフレームに適応されていません.ARでは画像と推定カメラ姿勢の同期が非常に重要なので,受け取ったImageメッセージをキーフレームのタイムスタンプに割り当てるようにして,後で簡単に同期ができるようにしておきます.

void vidCb(const sensor_msgs::ImageConstPtr img)
{
  ...
  ImageAndExposure* undistImg = undistorter->undistort<unsigned char>(&minImg, 1,0, 1.0f);
  undistImg->timestamp = img->header.stamp.toSec(); // これを追加!
  fullSystem->addActiveFrame(undistImg, frameID);
  ...
}

最後に,PosePublisherとPointCloudPublisherをシステムに登録して完了です!

int main( int argc, char** argv )
{
 ...
 ros::NodeHandle nh;
 ros::Subscriber imgSub = nh.subscribe("image", 1, &vidCb);
 fullSystem->outputWrapper.push_back(new PosePublisher(nh));
 fullSystem->outputWrapper.push_back(new PointCloudPublisher(nh));
 ...
}

さっきと同様にrosbagを再生してみて,rvizで点群が正しく出力されているか確認してみます.

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
rviz

rvizで”Add” -> “by Topic”から”/points”トピックを追加します.出力される点群は更新単位でのものになるので,点群を累積するために表示設定の”Decay Time”を適当な大きな値に変更します(10000くらい).無事にrviz上で点群を見ることができれば成功です!

1 thought on “DSOでAR ~ 2. ROSパッケージインストール

  1. calm

    はじめまして

    dsoslamをROS化するにあたり、本記事を参考にさせていただいてます。
    記事のおかげで、前記事「DSOでAR ~ 1. DSO本体インストール」まではうまく行きました。

    次に本記事の説明に沿って編集し、catkin_makeでビルドしてみたのですが、以下のように
    「FrameHessianにtimestampというメンバ関数はない」と指摘されました。
    エラー行をコメントアウトすればビルドは通りましたが、rvizで点群を確認することはできませんでした。
    何か対処できることがあれば教えていただきたく思います。
    よろしくお願いします。

    ~/catkin_ws/src/dso_ros/src/main.cpp: In member function ‘virtual void PointCloudPublisher::publishKeyframes(std::vector&, bool, dso::CalibHessian*)’:
    ~/catkin_ws/src/dso_ros/src/main.cpp:104:28: error: ‘const class dso::FrameHessian’ has no member named ‘timestamp’

    返信

コメントを残す

メールアドレスが公開されることはありません。 * が付いている欄は必須項目です

このサイトはスパムを低減するために Akismet を使っています。コメントデータの処理方法の詳細はこちらをご覧ください