From ddb3c890a68f2ab01a0327e9ea8d90cc14af7cca Mon Sep 17 00:00:00 2001
From: Holger Rapp <HolgerRapp@gmx.net>
Date: Thu, 26 Jan 2017 16:49:25 +0100
Subject: [PATCH] Actually check that all points are colored or none. (#192)

---
 cartographer/io/ply_writing_points_processor.cc | 9 ++++++++-
 1 file changed, 8 insertions(+), 1 deletion(-)

diff --git a/cartographer/io/ply_writing_points_processor.cc b/cartographer/io/ply_writing_points_processor.cc
index 05f56c2..0dc4195 100644
--- a/cartographer/io/ply_writing_points_processor.cc
+++ b/cartographer/io/ply_writing_points_processor.cc
@@ -109,9 +109,16 @@ void PlyWritingPointsProcessor::Process(std::unique_ptr<PointsBatch> batch) {
     has_colors_ = !batch->colors.empty();
     WriteBinaryPlyHeader(has_colors_, 0, file_.get());
   }
+  if (has_colors_) {
+    CHECK_EQ(batch->points.size(), batch->colors.size())
+        << "First PointsBatch had colors, but encountered one without. "
+           "frame_id: "
+        << batch->frame_id;
+  }
+
   for (size_t i = 0; i < batch->points.size(); ++i) {
     WriteBinaryPlyPointCoordinate(batch->points[i], file_.get());
-    if (!batch->colors.empty()) {
+    if (has_colors_) {
       WriteBinaryPlyPointColor(batch->colors[i], file_.get());
     }
     ++num_points_;