Add emplace[_back] to pcl::PointCloud (#3207)

common/include/pcl/point_cloud.h

@@ -48,6 +48,7 @@
 #include <pcl/exceptions.h>
 #include <pcl/pcl_macros.h>
 #include <pcl/point_traits.h>
+#include <utility>
 
 namespace pcl
 {
@@ -484,6 +485,20 @@ namespace pcl
         height = 1;
       }
 
+      /** \brief Emplace a new point in the cloud, at the end of the container.
+        * \note This breaks the organized structure of the cloud by setting the height to 1!
+        * \param[in] args the parameters to forward to the point to construct
+        * \return reference to the emplaced point
+        */
+      template <class... Args> inline reference
+      emplace_back (Args&& ...args)
+      {
+        points.emplace_back (std::forward<Args> (args)...);
+        width = static_cast<uint32_t> (points.size ());
+        height = 1;
+        return points.back();
+      }
+
       /** \brief Insert a new point in the cloud, given an iterator.
         * \note This breaks the organized structure of the cloud by setting the height to 1!
         * \param[in] position where to insert the point
@@ -527,6 +542,21 @@ namespace pcl
         height = 1;
       }
 
+      /** \brief Emplace a new point in the cloud, given an iterator.
+        * \note This breaks the organized structure of the cloud by setting the height to 1!
+        * \param[in] position iterator before which the point will be emplaced
+        * \param[in] args the parameters to forward to the point to construct
+        * \return returns the new position iterator
+        */
+      template <class... Args> inline iterator
+      emplace (iterator position, Args&& ...args)
+      {
+        iterator it = points.emplace (position, std::forward<Args> (args)...);
+        width = static_cast<uint32_t> (points.size ());
+        height = 1;
+        return (it);
+      }
+
       /** \brief Erase a point in the cloud.
         * \note This breaks the organized structure of the cloud by setting the height to 1!
         * \param[in] position what data point to erase

test/common/test_common.cpp

@@ -89,8 +89,8 @@ TEST (PCL, PointXYZRGBNormal)
   PointXYZRGBNormal p;
 
   uint8_t r = 127, g = 64, b = 254;
-  uint32_t rgb = (static_cast<uint32_t> (r) << 16 | 
-                  static_cast<uint32_t> (g) << 8 | 
+  uint32_t rgb = (static_cast<uint32_t> (r) << 16 |
+                  static_cast<uint32_t> (g) << 8 |
                   static_cast<uint32_t> (b));
   p.rgba = rgb;
 
@@ -217,7 +217,7 @@ TEST (PCL, PointCloud)
     EXPECT_EQ (mat_xyz (0, 0), 0);
     EXPECT_EQ (mat_xyz (2, cloud.width - 1), 3 * cloud.width - 1);    // = 29
   }
-  
+
 #ifdef NDEBUG
   if (Eigen::MatrixXf::Flags & Eigen::RowMajorBit)
   {
@@ -274,6 +274,15 @@ TEST (PCL, PointCloud)
   cloud.erase (cloud.begin (), cloud.end ());
   EXPECT_EQ (cloud.isOrganized (), false);
   EXPECT_EQ (cloud.width, 0);
+
+  cloud.emplace (cloud.end (), 1, 1, 1);
+  EXPECT_EQ (cloud.isOrganized (), false);
+  EXPECT_EQ (cloud.width, 1);
+
+  auto& new_point = cloud.emplace_back (1, 1, 1);
+  EXPECT_EQ (cloud.isOrganized (), false);
+  EXPECT_EQ (cloud.width, 2);
+  EXPECT_EQ (&new_point, &cloud.back ());
 }
 
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
@@ -391,7 +400,7 @@ TEST (PCL, CopyIfFieldExists)
   p.normal_x = 1.0; p.normal_y = 0.0; p.normal_z = 0.0;
 
   using FieldList = pcl::traits::fieldList<PointXYZRGBNormal>::type;
-  bool is_x = false, is_y = false, is_z = false, is_rgb = false, 
+  bool is_x = false, is_y = false, is_z = false, is_rgb = false,
        is_normal_x = false, is_normal_y = false, is_normal_z = false;
 
   float x_val, y_val, z_val, normal_x_val, normal_y_val, normal_z_val, rgb_val;
@@ -422,7 +431,7 @@ TEST (PCL, CopyIfFieldExists)
   pcl::for_each_type<FieldList> (CopyIfFieldExists<PointXYZRGBNormal, float> (p, "normal_z", is_normal_z, normal_z_val));
   EXPECT_EQ (is_normal_z, true);
   EXPECT_EQ (normal_z_val, 0.0);
-  
+
   pcl::for_each_type<FieldList> (CopyIfFieldExists<PointXYZRGBNormal, float> (p, "x", x_val));
   EXPECT_EQ (x_val, 1.0);
 
@@ -479,7 +488,7 @@ TEST (PCL, IsSamePointType)
   EXPECT_FALSE (status);
   status = isSamePointType<PointXYZRGB, PointXYZRGB> ();
   EXPECT_TRUE (status);
-  
+
   // Even though it's the "same" type, rgb != rgba
   status = isSamePointType<PointXYZRGB, PointXYZRGBA> ();
   EXPECT_FALSE (status);