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

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

目次

1. OMPLのインストール
2. OMPLの基礎
3. OMPL.appの入門
4. OMPLの使用
5. DJI Telloの使用
   1. DJI Telloのフロントカメラのcamera_calibration
   2. Visual SLAM ORB_SLAM2 用のカメラキャリブレーションYamlファイルの作成
   3. DJI Telloのカメラ でVisual-SLAMのORB-SLAM2 を動かしてみた
6. navigationスタックの使い方
   1. Turtlebot3 NavigationをGazebo Simulationで動かしてみた
   2. Turtlebot3 Navigationでmap_fileの代わりにSLAMを使用
   3. ARdroneのシミュレーターをGazeboで動かす
   4. AirSimのシミュレーターを動かす
   5. octomapのインストール
   6. 単眼カメラでORB_SLAM2から3D point clouodの生成　← いまココ
   7. point_cloud2から3D octomapの生成
7. OMPLで3D 経路計画
   1. 状態空間環境のセットアップ

動作環境

• Ubuntu 18.04
• ROS Melodic
• Tello EDU

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

  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を生成

  // 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動画

Prev: 6.5. octomapのインストール
Next: 6.7. point_cloud2から3D octomapの生成