Add and clean up viz::PointCloud::addPoint

source/RobotAPI/components/ArViz/Client/elements/PointCloud.h

@@ -49,34 +49,33 @@ namespace armarx::viz
             return *this;
         }
 
-        PointCloud& addPoint(float x, float y, float z)
+        PointCloud& addPoint(float x, float y, float z, const data::Color& color)
         {
             ColoredPoint p;
             p.x = x;
             p.y = y;
             p.z = z;
-            p.color = viz::Color::fromRGBA(0, 0, 0, 255);
+            p.color = color;
             return addPoint(p);
         }
-
-        PointCloud& addPoint(float x, float y, float z, std::size_t id, int alpha = 255)
+        PointCloud& addPoint(float x, float y, float z, const simox::color::Color& color)
         {
-            ColoredPoint p;
-            p.x = x;
-            p.y = y;
-            p.z = z;
-            p.color = Color::fromRGBA(simox::color::GlasbeyLUT::at(id, alpha));
-            return addPoint(p);
+            return addPoint(x, y, z, Color{color});
         }
 
         PointCloud& addPoint(float x, float y, float z, std::uint8_t r, std::uint8_t g, std::uint8_t b, std::uint8_t a = 255)
         {
-            ColoredPoint p;
-            p.x = x;
-            p.y = y;
-            p.z = z;
-            p.color = Color::fromRGBA(r, g, b, a);
-            return addPoint(p);
+            return addPoint(x, y, z, Color{r, g, b, a});
+        }
+
+        PointCloud& addPoint(float x, float y, float z)
+        {
+            return addPoint(x, y, z, 0, 0, 0, 255);
+        }
+
+        PointCloud& addPoint(float x, float y, float z, std::size_t id, int alpha = 255)
+        {
+            return addPoint(x, y, z, simox::color::GlasbeyLUT::at(id, alpha));
         }
 
         // Templated setters for PCL point clouds.
@@ -85,35 +84,20 @@ namespace armarx::viz
                    std::enable_if_t < !detail::has_members_rgba<PointT>::value, int > = 0 >
         PointCloud & addPoint(const PointT& p, Color color = Color::gray())
         {
-            ColoredPoint& pv = data_->points.emplace_back(ColoredPoint());
-            pv.x = p.x;
-            pv.y = p.y;
-            pv.z = p.z;
-            pv.color = color;
-            return *this;
+            return addPoint(p.x, p.y, p.z, color);
         }
 
         template < class PointT,
                    std::enable_if_t < detail::has_members_rgba<PointT>::value, int> = 0>
         PointCloud & addPoint(const PointT& p)
         {
-            ColoredPoint& pv = data_->points.emplace_back(ColoredPoint());
-            pv.x = p.x;
-            pv.y = p.y;
-            pv.z = p.z;
-            pv.color = Color::fromRGBA(p.r, p.g, p.b, p.a);
-            return *this;
+            return addPoint(p.x, p.y, p.z, p.r, p.g, p.b, p.a);
         }
 
         template < class PointT >
         PointCloud& addPoint(const PointT& p, Color color)
         {
-            ColoredPoint& pv = data_->points.emplace_back(ColoredPoint());
-            pv.x = p.x;
-            pv.y = p.y;
-            pv.z = p.z;
-            pv.color = color;
-            return *this;
+            return addPoint(p.x, p.y, p.z, color);
         }
 
         template <class PointCloudT>