DJI TelloでOMPLを用いた自律飛行ドローンのナビゲーション - 6.6. 単眼カメラでORB_SLAM2から3D point clouodの生成

DJI Tello等ドローンの自律飛行を実現するには、3D環境でナビゲーション経路を計画としてOMPL（Open Motion Planning Library）を試してみます。

# 目次
1. [OMPLのインストール](https://qiita.com/k-koh/items/4f7e441d1225e6580f82)
2. [OMPLの基礎](https://qiita.com/k-koh/items/9933cd29599c859cac45)
3. [OMPL.appの入門](https://qiita.com/k-koh/items/fe1f05313045ed729c8b)
4. [OMPLの使用](https://qiita.com/k-koh/items/3a8b372537aa29e17863)
5. DJI Telloの使用
    1. [DJI Telloのフロントカメラのcamera_calibration](https://qiita.com/k-koh/items/094631646931ea5bc2bb)
    2. [Visual SLAM ORB_SLAM2 用のカメラキャリブレーションYamlファイルの作成](https://qiita.com/k-koh/items/b524fc175198e701bdff)
    3. [DJI Telloのカメラ でVisual-SLAMのORB-SLAM2 を動かしてみた](https://qiita.com/k-koh/items/98b5f079f784b22dcd01)
6. navigationスタックの使い方
    1. [Turtlebot3 NavigationをGazebo Simulationで動かしてみた](https://qiita.com/k-koh/items/1a4fbc175ca37be6206b)
    2. [Turtlebot3 Navigationでmap_fileの代わりにSLAMを使用](https://qiita.com/k-koh/items/1ec9097e4afedd0b43ca)
    3. [ARdroneのシミュレーターをGazeboで動かす](https://qiita.com/k-koh/items/feccbaee7c11c43c3ab4)
    4. [AirSimのシミュレーターを動かす](https://qiita.com/k-koh/items/52122fb5e96336ac8d33)
    5. [octomapのインストール](https://qiita.com/k-koh/items/c9a3a87e043934456842)
    6. [単眼カメラでORB_SLAM2から3D point clouodの生成](https://qiita.com/k-koh/items/1e069c9dda43b85e2fa0)　← いまココ
    7. [point_cloud2から3D octomapの生成](https://qiita.com/k-koh/items/01e34f353487eb8a6e26)
7. OMPLで3D 経路計画
    1. [状態空間環境のセットアップ](https://qiita.com/k-koh/items/e19ac08bb0a16975081e)

## 動作環境
- Ubuntu 18.04
- ROS Melodic
- Tello EDU

## 単眼カメラでORB_SLAM2から3D point clouodを生成

```c++
  sensor_msgs::PointCloud pc;
  pc.header.frame_id = "/first_keyframe_cam";

  const std::vector<ORB_SLAM2::MapPoint*>& point_cloud =
      mpSLAM->mpMap->GetAllMapPoints();
  pc.points.clear();
  for (size_t i = 0; i < point_cloud.size(); i++)
  {
    if (point_cloud[i]->isBad())
    {
      continue;
    }
    cv::Mat pos = point_cloud[i]->GetWorldPos();
    geometry_msgs::Point32 pp;
    pp.x = pos.at<float>(0);
    pp.y = pos.at<float>(1);
    pp.z = pos.at<float>(2);

    pc.points.push_back(pp);
  }
  ros::Publisher pc_pub =
    nodeHandler.advertise<sensor_msgs::PointCloud>("/orb/point_cloud", 2);
  pc_pub.publish(pc);
```

## 単眼カメラでORB_SLAM2から3D point clouodを生成

```c++
  // gets points from most recent frame
  const std::vector<ORB_SLAM2::MapPoint*>& map_points =
      mpSLAM->mpMap->GetAllMapPoints();

  // convertToPCL2
  const std::size_t n_map_points = map_points.size();
  // Kind of a hack, but there aren't much better ways to avoid a copy
  struct point
  {
    float x, y, z;
  };

  std::vector<uint8_t> data_buffer(n_map_points * sizeof(point));
  std::size_t vtop = 0;

  point *dataptr = (point*) data_buffer.data();

  ros::Time tStart = ros::Time::now();
  for (ORB_SLAM2::MapPoint *map_point : map_points) {
      if (map_point->isBad())
          continue;
      cv::Mat pos = map_point->GetWorldPos();
      dataptr[vtop++] = {
          pos.at<float>(0),
          pos.at<float>(1),
          pos.at<float>(2),
      };

  }

  static const char* const names[3] = { "x", "y", "z" };
  static const std::size_t offsets[3] = { offsetof(point, x), offsetof(point, y), offsetof(point, z) };
  std::vector<sensor_msgs::PointField> fields(3);
  for (int i=0; i < 3; i++) {
      fields[i].name = names[i];
      fields[i].offset = offsets[i];
      fields[i].datatype = sensor_msgs::PointField::FLOAT32;
      fields[i].count = 1;
  }

  sensor_msgs::PointCloud2 msg;
  msg.height = 1;
  msg.width = n_map_points;
  msg.fields = fields;
  msg.is_bigendian = IS_BIG_ENDIAN;
  msg.point_step = sizeof(point);
  msg.row_step = sizeof(point) * msg.width;
  msg.data = std::move(data_buffer);
  msg.is_dense = true;  // invalid points already filtered out

  ros::Publisher pc2_pub =
    nodeHandler.advertise<sensor_msgs::PointCloud2>("/orb/point_cloud2", 2);
  pc2_pub.publish(msg);
```

- Rvizでpoint clouod表示のYouTube動画

[![Visual-SLAMのORB-SLAM2を起動](http://img.youtube.com/vi/AkZTAQDK8JU/0.jpg)](http://www.youtube.com/watch?v=AkZTAQDK8JU)


Prev: 6.5. [octomapのインストール](https://qiita.com/k-koh/items/c9a3a87e043934456842)
Next: 6.7. [point_cloud2から3D octomapの生成](https://qiita.com/k-koh/items/01e34f353487eb8a6e26)