Add trajectory colors. (#363)

Generates colors for different trajectories using a scheme which
ensures even hue distribution, uniform look and high contrast
between consecutive trajectories.

cartographer_ros/cartographer_ros/color.cc

@@ -0,0 +1,69 @@
+/*
+ * Copyright 2017 The Cartographer Authors
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "cartographer_ros/color.h"
+
+#include <cmath>
+
+#include "glog/logging.h"
+
+namespace cartographer_ros {
+
+namespace {
+
+constexpr float kInitialHue = 0.69f;
+constexpr float kSaturation = 0.85f;
+constexpr float kValue = 0.77f;
+constexpr float kGoldenRatioConjugate = (std::sqrt(5) - 1) / 2.f;
+
+::cartographer_ros::ColorRgb HsvToRgb(const float h, const float s,
+                                      const float v) {
+  const float h_6 = (h == 1.f) ? 0.f : 6 * h;
+  const int h_i = std::floor(h_6);
+  const float f = h_6 - h_i;
+
+  const float p = v * (1.f - s);
+  const float q = v * (1.f - f * s);
+  const float t = v * (1.f - (1.f - f) * s);
+
+  if (h_i == 0) {
+    return {v, t, p};
+  } else if (h_i == 1) {
+    return {q, v, p};
+  } else if (h_i == 2) {
+    return {p, v, t};
+  } else if (h_i == 3) {
+    return {p, q, v};
+  } else if (h_i == 4) {
+    return {t, p, v};
+  } else if (h_i == 5) {
+    return {v, p, q};
+  } else {
+    return {0.f, 0.f, 0.f};
+  }
+}
+
+}  // namespace
+
+ColorRgb GetColor(int id) {
+  CHECK_GE(id, 0);
+  // Uniform color sampling using the golden ratio from
+  // http://martin.ankerl.com/2009/12/09/how-to-create-random-colors-programmatically/
+  const float hue = std::fmod(kInitialHue + kGoldenRatioConjugate * id, 1.f);
+  return HsvToRgb(hue, kSaturation, kValue);
+}
+
+}  // namespace cartographer_ros

cartographer_ros/cartographer_ros/color.h

@@ -0,0 +1,35 @@
+/*
+ * Copyright 2017 The Cartographer Authors
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef CARTOGRAPHER_ROS_COLOR_MANAGER_H_
+#define CARTOGRAPHER_ROS_COLOR_MANAGER_H_
+
+namespace cartographer_ros {
+
+struct ColorRgb {
+  // r, g, b are from [0,1]
+  float r;
+  float g;
+  float b;
+};
+
+// A function for on-demand generation of a colour palette, with every two
+// direct successors having large contrast. Initial hue is from [0, 1>.
+ColorRgb GetColor(int id);
+
+}  // namespace cartographer_ros
+
+#endif  // CARTOGRAPHER_ROS_COLOR_MANAGER_H_

cartographer_ros/cartographer_ros/map_builder_bridge.cc

@@ -17,6 +17,7 @@
 #include "cartographer_ros/map_builder_bridge.h"
 
 #include "cartographer_ros/assets_writer.h"
+#include "cartographer_ros/color.h"
 #include "cartographer_ros/msg_conversion.h"
 #include "cartographer_ros/occupancy_grid.h"
 #include "cartographer_ros_msgs/TrajectorySubmapList.h"
@@ -206,13 +207,19 @@ visualization_msgs::MarkerArray MapBuilderBridge::GetTrajectoryNodesList() {
   const auto trajectory_nodes =
       map_builder_.sparse_pose_graph()->GetTrajectoryNodes();
   int marker_id = 0;
-  for (const auto& single_trajectory : trajectory_nodes) {
+  for (int trajectory_id = 0;
+       trajectory_id < static_cast<int>(trajectory_nodes.size());
+       ++trajectory_id) {
+    const auto& single_trajectory = trajectory_nodes[trajectory_id];
     visualization_msgs::Marker marker;
     marker.id = marker_id++;
     marker.type = visualization_msgs::Marker::LINE_STRIP;
     marker.header.stamp = ::ros::Time::now();
     marker.header.frame_id = node_options_.map_frame;
-    marker.color.b = 1.0;
+    const ColorRgb trajectory_color = GetColor(trajectory_id);
+    marker.color.r = trajectory_color.r;
+    marker.color.g = trajectory_color.g;
+    marker.color.b = trajectory_color.b;
     marker.color.a = 1.0;
     marker.scale.x = kTrajectoryLineStripMarkerScale;
     marker.pose.orientation.w = 1.0;