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

[:ja]

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上で点群を見ることができれば成功です！

[:]