Method OutputPathToPointcloud

builder/trajectory.cc:71–145

}

void Trajectory::OutputPathToPointcloud(const std::string& path) {
  if (!common::FileExist(path)) {
    LOG(WARNING) << "Path does not exist. do not output any thing.";
    return;
  }
  int path_point_index = 0;
  std::ofstream path_text_file(path + "/path.csv");
  std::string path_content;
  bool write_to_text = path_text_file.is_open();
  pcl::PointCloud<pcl::_PointXYZI>::Ptr path_cloud(
      new pcl::PointCloud<pcl::_PointXYZI>);
  pcl::PointCloud<pcl::PointXYZI> enu_path_cloud;
  pcl::PointCloud<pcl::PointXYZI> odom_path_cloud;
  enu_path_cloud.points.reserve(this->size());
  for (const auto& submap : *this) {
    // enu path
    if (submap->HasGps()) {
      pcl::PointXYZI enu_point;
      const Eigen::Vector3d enu = submap->GetRelatedGpsInENU();
      enu_point.x = enu[0];
      enu_point.y = enu[1];
      enu_point.z = enu[2];
      enu_point.intensity = 0.;
      enu_path_cloud.push_back(enu_point);
    }
    // odom path
    if (submap->HasOdom()) {
      pcl::PointXYZI odom_point;
      const Eigen::Vector3d odom =
          common::Translation(submap->GetRelatedOdom());
      odom_point.x = odom[0];
      odom_point.y = odom[1];
      odom_point.z = odom[2];
      odom_point.intensity = 0.;
      odom_path_cloud.push_back(odom_point);
    }
    // map path
    for (auto& frame : submap->GetFrames()) {
      pcl::_PointXYZI path_point;
      Eigen::Matrix4d pose = frame->GlobalPose();
      Eigen::Vector6<double> global_pose_6d = common::TransformToVector6(pose);
      path_point.x = global_pose_6d[0];
      path_point.y = global_pose_6d[1];
      path_point.z = global_pose_6d[2];
      path_point.intensity = path_point_index;

      path_cloud->push_back(path_point);
      if (write_to_text) {
        path_content += (std::to_string(path_point_index) + ", ");
        path_content += (std::to_string(global_pose_6d[0]) + ", ");
        path_content += (std::to_string(global_pose_6d[1]) + ", ");
        path_content += (std::to_string(global_pose_6d[2]) + ", ");
        path_content += (std::to_string(global_pose_6d[3]) + ", ");
        path_content += (std::to_string(global_pose_6d[4]) + ", ");
        path_content += (std::to_string(global_pose_6d[5]) + " \n");
      }
      path_point_index++;
    }