From 4ad2095c927237b94d6cd20334a5831b7d6b6c81 Mon Sep 17 00:00:00 2001
From: nsubiron <nsubiron@cvc.uab.es>
Date: Sun, 30 Sep 2018 19:03:09 +0200
Subject: [PATCH] Add methods for saving point clouds in ply format

---
 Docs/python_api.md                       |  1 +
 LibCarla/cmake/client/CMakeLists.txt     |  6 +++++
 LibCarla/source/carla/pointcloud/PLY.cpp | 29 ++++++++++++++++++++
 LibCarla/source/carla/pointcloud/PLY.h   | 34 ++++++++++++++++++++++++
 PythonAPI/source/libcarla/Sensor.cpp     |  7 +++++
 5 files changed, 77 insertions(+)
 create mode 100644 LibCarla/source/carla/pointcloud/PLY.cpp
 create mode 100644 LibCarla/source/carla/pointcloud/PLY.h

diff --git a/Docs/python_api.md b/Docs/python_api.md
index fdee65e3a..d51131ade 100644
--- a/Docs/python_api.md
+++ b/Docs/python_api.md
@@ -100,6 +100,7 @@
 - `channels`
 - `raw_data`
 - `get_point_count(channel)`
+- `save_to_disk(path)`
 - `__len__()`
 - `__iter__()`
 
diff --git a/LibCarla/cmake/client/CMakeLists.txt b/LibCarla/cmake/client/CMakeLists.txt
index 9ccfdfffb..349fd398e 100644
--- a/LibCarla/cmake/client/CMakeLists.txt
+++ b/LibCarla/cmake/client/CMakeLists.txt
@@ -39,6 +39,12 @@ file(GLOB libcarla_carla_opendrive_sources
 set(libcarla_sources "${libcarla_sources};${libcarla_carla_opendrive_sources}")
 install(FILES ${libcarla_carla_opendrive_sources} DESTINATION include/carla/opendrive)
 
+file(GLOB libcarla_carla_pointcloud_sources
+    "${libcarla_source_path}/carla/pointcloud/*.cpp"
+    "${libcarla_source_path}/carla/pointcloud/*.h")
+set(libcarla_sources "${libcarla_sources};${libcarla_carla_pointcloud_sources}")
+install(FILES ${libcarla_carla_pointcloud_sources} DESTINATION include/carla/pointcloud)
+
 file(GLOB libcarla_carla_road_sources
     "${libcarla_source_path}/carla/road/*.cpp"
     "${libcarla_source_path}/carla/road/*.h")
diff --git a/LibCarla/source/carla/pointcloud/PLY.cpp b/LibCarla/source/carla/pointcloud/PLY.cpp
new file mode 100644
index 000000000..d070c8dbe
--- /dev/null
+++ b/LibCarla/source/carla/pointcloud/PLY.cpp
@@ -0,0 +1,29 @@
+// Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma
+// de Barcelona (UAB).
+//
+// This work is licensed under the terms of the MIT license.
+// For a copy, see <https://opensource.org/licenses/MIT>.
+
+#include "carla/pointcloud/PLY.h"
+
+#include <iomanip>
+
+namespace carla {
+namespace pointcloud {
+
+  void PLY::WriteHeader(OutputFile &out, size_t number_of_points) {
+    out << "ply\n"
+           "format ascii 1.0\n"
+           "element vertex " << number_of_points << "\n"
+           "property float32 x\n"
+           "property float32 y\n"
+           "property float32 z\n"
+           // "property uchar diffuse_red\n"
+           // "property uchar diffuse_green\n"
+           // "property uchar diffuse_blue\n"
+           "end_header\n";
+    out << std::fixed << std::setprecision(4u);
+  }
+
+} // namespace pointcloud
+} // namespace carla
diff --git a/LibCarla/source/carla/pointcloud/PLY.h b/LibCarla/source/carla/pointcloud/PLY.h
new file mode 100644
index 000000000..293bbf451
--- /dev/null
+++ b/LibCarla/source/carla/pointcloud/PLY.h
@@ -0,0 +1,34 @@
+// Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma
+// de Barcelona (UAB).
+//
+// This work is licensed under the terms of the MIT license.
+// For a copy, see <https://opensource.org/licenses/MIT>.
+
+#pragma once
+
+#include "carla/OutputFile.h"
+
+#include <iterator>
+
+namespace carla {
+namespace pointcloud {
+
+  class PLY {
+  public:
+
+    template <typename PointIt>
+    static void SaveToDisk(const std::string &path, PointIt begin, PointIt end) {
+      OutputFile out(path, ".ply");
+      WriteHeader(out, std::distance(begin, end));
+      for (; begin != end; ++begin) {
+        out << begin->x << ' ' << begin->y << ' ' << begin->z << '\n';
+      }
+    }
+
+  private:
+
+    static void WriteHeader(OutputFile &out, size_t number_of_points);
+  };
+
+} // namespace pointcloud
+} // namespace carla
diff --git a/PythonAPI/source/libcarla/Sensor.cpp b/PythonAPI/source/libcarla/Sensor.cpp
index bb4534f5a..0918d88f9 100644
--- a/PythonAPI/source/libcarla/Sensor.cpp
+++ b/PythonAPI/source/libcarla/Sensor.cpp
@@ -5,6 +5,7 @@
 // For a copy, see <https://opensource.org/licenses/MIT>.
 
 #include <carla/client/Sensor.h>
+#include <carla/pointcloud/PLY.h>
 #include <carla/sensor/SensorData.h>
 #include <carla/sensor/data/Image.h>
 #include <carla/sensor/data/LidarMeasurement.h>
@@ -77,6 +78,11 @@ static auto GetRawDataAsBuffer(T &self) {
   return boost::python::object(boost::python::handle<>(ptr));
 }
 
+template <typename T>
+static void SavePointCloudToDisk(T &self, const std::string &path) {
+  carla::pointcloud::PLY::SaveToDisk(path, self.begin(), self.end());
+}
+
 void export_sensor() {
   using namespace boost::python;
   namespace cc = carla::client;
@@ -105,6 +111,7 @@ void export_sensor() {
     .add_property("channels", &csd::LidarMeasurement::GetChannelCount)
     .add_property("raw_data", &GetRawDataAsBuffer<csd::LidarMeasurement>)
     .def("get_point_count", &csd::LidarMeasurement::GetPointCount, (arg("channel")))
+    .def("save_to_disk", &SavePointCloudToDisk<csd::LidarMeasurement>, (arg("path")))
     .def("__len__", &csd::LidarMeasurement::size)
     .def("__iter__", iterator<csd::LidarMeasurement>())
     .def(self_ns::str(self_ns::self))