Merge pull request #567 from nizar-sallem/copy_cloud_in_cloud

Copy cloud in cloud

common/include/pcl/common/impl/io.hpp

@@ -372,5 +372,131 @@ pcl::concatenateFields (const pcl::PointCloud<PointIn1T> &cloud1_in,
   }
 }
 
+//////////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointT> void
+pcl::copyPointCloud (const pcl::PointCloud<PointT> &cloud_in, pcl::PointCloud<PointT> &cloud_out,
+                     int top, int bottom, int left, int right, pcl::InterpolationType border_type, const PointT& value)
+{
+  if (top < 0 || left < 0 || bottom < 0 || right < 0)
+  {
+    std::string faulty = (top < 0) ? "top" : (left < 0) ? "left" : (bottom < 0) ? "bottom" : "right";
+    PCL_THROW_EXCEPTION (pcl::BadArgumentException, "[pcl::copyPointCloud] error: " << faulty << " must be positive!");
+    return;
+  }
+
+  if (top == 0 && left == 0 && bottom == 0 && right == 0)
+   cloud_out = cloud_in;
+  else
+  {
+    // Allocate enough space and copy the basics
+    cloud_out.header   = cloud_in.header;
+    cloud_out.width    = cloud_in.width + left + right;
+    cloud_out.height   = cloud_in.height + top + bottom;
+    if (cloud_out.size () != cloud_out.width * cloud_out.height)
+      cloud_out.resize (cloud_out.width * cloud_out.height);
+    cloud_out.is_dense = cloud_in.is_dense;
+    cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_;
+    cloud_out.sensor_origin_ = cloud_in.sensor_origin_;
+
+    if (border_type == pcl::BORDER_TRANSPARENT)
+    {
+      const PointT* in = &(cloud_in.points[0]);
+      PointT* out = &(cloud_out.points[0]);
+      PointT* out_inner = out + cloud_out.width*top + left;
+      for (int i = 0; i < cloud_in.height; i++, out_inner += cloud_out.width, in += cloud_in.width)
+      {
+        if (out_inner != in)
+          memcpy (out_inner, in, cloud_in.width * sizeof (PointT));
+      }
+    }
+    else
+    {
+      // Copy the data
+      if (border_type != pcl::BORDER_CONSTANT)
+      {
+        try
+        {
+          std::vector<int> padding (cloud_out.width - cloud_in.width);
+          int right = cloud_out.width - cloud_in.width - left;
+          int bottom = cloud_out.height - cloud_in.height - top;
+
+          for (int i = 0; i < left; i++)
+            padding[i] = pcl::interpolatePointIndex (i-left, cloud_in.width, border_type);
+
+          for (int i = 0; i < right; i++)
+            padding[i+left] = pcl::interpolatePointIndex (cloud_in.width+i, cloud_in.width, border_type);
+
+          const PointT* in = &(cloud_in.points[0]);
+          PointT* out = &(cloud_out.points[0]);
+          PointT* out_inner = out + cloud_out.width*top + left;
+
+          for (int i = 0; i < cloud_in.height; i++, out_inner += cloud_out.width, in += cloud_in.width)
+          {
+            if (out_inner != in)
+              memcpy (out_inner, in, cloud_in.width * sizeof (PointT));
+
+            for (int j = 0; j < left; j++)
+              out_inner[j - left] = in[padding[j]];
+
+            for (int j = 0; j < right; j++)
+              out_inner[j + cloud_in.width] = in[padding[j + left]];
+          }
+
+          for (int i = 0; i < top; i++)
+          {
+            int j = pcl::interpolatePointIndex (i - top, cloud_in.height, border_type);
+            memcpy (out + i*cloud_out.width,
+                    out + (j+top) * cloud_out.width,
+                    sizeof (PointT) * cloud_out.width);
+          }
+
+          for (int i = 0; i < bottom; i++)
+          {
+            int j = pcl::interpolatePointIndex (i + cloud_in.height, cloud_in.height, border_type);
+            memcpy (out + (i + cloud_in.height + top)*cloud_out.width,
+                    out + (j+top)*cloud_out.width,
+                    cloud_out.width * sizeof (PointT));
+          }
+        }
+        catch (pcl::BadArgumentException &e)
+        {
+          PCL_ERROR ("[pcl::copyPointCloud] Unhandled interpolation type %d!\n", border_type);
+        }
+      }
+      else
+      {
+        int right = cloud_out.width - cloud_in.width - left;
+        int bottom = cloud_out.height - cloud_in.height - top;
+        std::vector<PointT> buff (cloud_out.width, value);
+        PointT* buff_ptr = &(buff[0]);
+        const PointT* in = &(cloud_in.points[0]);
+        PointT* out = &(cloud_out.points[0]);
+        PointT* out_inner = out + cloud_out.width*top + left;
+
+        for (int i = 0; i < cloud_in.height; i++, out_inner += cloud_out.width, in += cloud_in.width)
+        {
+          if (out_inner != in)
+            memcpy (out_inner, in, cloud_in.width * sizeof (PointT));
+
+          memcpy (out_inner - left, buff_ptr, left  * sizeof (PointT));
+          memcpy (out_inner + cloud_in.width, buff_ptr, right * sizeof (PointT));
+        }
+
+        for (int i = 0; i < top; i++)
+        {
+          memcpy (out + i*cloud_out.width, buff_ptr, cloud_out.width * sizeof (PointT));
+        }
+
+        for (int i = 0; i < bottom; i++)
+        {
+          memcpy (out + (i + cloud_in.height + top)*cloud_out.width,
+                  buff_ptr,
+                  cloud_out.width * sizeof (PointT));
+        }
+      }
+    }
+  }
+}
+
 #endif // PCL_IO_IMPL_IO_H_
 

common/include/pcl/common/io.h

@@ -45,6 +45,7 @@
 #include <pcl/pcl_base.h>
 #include <pcl/PointIndices.h>
 #include <pcl/conversions.h>
+#include <pcl/exceptions.h>
 #include <locale>
 
 namespace pcl
@@ -223,6 +224,24 @@ namespace pcl
     }
   }
 
+  typedef enum
+  {
+    BORDER_CONSTANT = 0, BORDER_REPLICATE = 1,
+    BORDER_REFLECT = 2, BORDER_WRAP = 3,
+    BORDER_REFLECT_101 = 4, BORDER_TRANSPARENT = 5,
+    BORDER_DEFAULT = BORDER_REFLECT_101
+  } InterpolationType;
+
+  /** \brief \return the right index according to the interpolation type.
+    * \note this is adapted from OpenCV
+    * \param p the index of point to interpolate
+    * \param length the top/bottom row or left/right column index
+    * \param type the requested interpolation
+    * \throws pcl::BadArgumentException if type is unknown
+    */
+  PCL_EXPORTS int
+  interpolatePointIndex (int p, int length, InterpolationType type);
+
   /** \brief Concatenate two pcl::PCLPointCloud2.
     * \param[in] cloud1 the first input point cloud dataset
     * \param[in] cloud2 the second input point cloud dataset
@@ -380,6 +399,26 @@ namespace pcl
                   const std::vector<pcl::PointIndices> &indices, 
                   pcl::PointCloud<PointOutT> &cloud_out);
 
+  /** \brief Copy a point cloud inside a larger one interpolating borders.
+    * \param[in] cloud_in the input point cloud dataset
+    * \param[out] cloud_out the resultant output point cloud dataset
+    * Position of cloud_in inside cloud_out is given by \a top, \a left, \a bottom \a right.
+    * \param[in] border_type the interpolating method (pcl::BORDER_XXX)
+    *  BORDER_REPLICATE:     aaaaaa|abcdefgh|hhhhhhh
+    *  BORDER_REFLECT:       fedcba|abcdefgh|hgfedcb
+    *  BORDER_REFLECT_101:   gfedcb|abcdefgh|gfedcba
+    *  BORDER_WRAP:          cdefgh|abcdefgh|abcdefg
+    *  BORDER_CONSTANT:      iiiiii|abcdefgh|iiiiiii  with some specified 'i'
+    *  BORDER_TRANSPARENT:   mnopqr|abcdefgh|tuvwxyz  where m-r and t-z are orignal values of cloud_out
+    * \throw pcl::BadArgumentException if any of top, bottom, left or right is negative.
+    * \ingroup common
+    */
+  template <typename PointT> void
+  copyPointCloud (const pcl::PointCloud<PointT> &cloud_in,
+                  pcl::PointCloud<PointT> &cloud_out,
+                  int top, int bottom, int left, int right,
+                  pcl::InterpolationType border_type, const PointT& value);
+
   /** \brief Concatenate two datasets representing different fields.
     *
     * \note If the input datasets have overlapping fields (i.e., both contain

common/include/pcl/exceptions.h

@@ -251,6 +251,18 @@ namespace pcl
       : pcl::PCLException (error_description, file_name, function_name, line_number) { }
   };
 
+  /** \class BadArgumentException
+    * \brief An exception that is thrown when the argments number or type is wrong/unhandled.
+    */
+  class BadArgumentException : public PCLException
+  {
+    public:
+    BadArgumentException (const std::string& error_description,
+                          const std::string& file_name = "",
+                          const std::string& function_name = "" ,
+                          unsigned line_number = 0) throw ()
+      : pcl::PCLException (error_description, file_name, function_name, line_number) { }
+  };
 }
 
 

common/src/io.cpp

@@ -474,3 +474,44 @@ pcl::copyPointCloud (const pcl::PCLPointCloud2 &cloud_in,
   cloud_out.data         = cloud_in.data;
 }
 
+////////////////////////////////////////////////////////////////////////////////
+int
+pcl::interpolatePointIndex (int p, int len, InterpolationType type)
+{
+  if (static_cast<unsigned> (p) >= static_cast<unsigned> (len))
+  {
+    if (type == BORDER_REPLICATE)
+      p = p < 0 ? 0 : len - 1;
+    else if (type == BORDER_REFLECT || type == BORDER_REFLECT_101)
+    {
+      int delta = type == BORDER_REFLECT_101;
+      if (len == 1)
+        return 0;
+      do
+      {
+        if (p < 0)
+          p = -p - 1 + delta;
+        else
+          p = len - 1 - (p - len) - delta;
+      }
+      while (static_cast<unsigned> (p) >= static_cast<unsigned> (len));
+    }
+    else if (type == BORDER_WRAP)
+    {
+      if (p < 0)
+        p -= ((p-len+1)/len)*len;
+      if (p >= len)
+        p %= len;
+    }
+    else if (type == BORDER_CONSTANT)
+      p = -1;
+    else
+    {
+      PCL_THROW_EXCEPTION (BadArgumentException,
+                           "[pcl::interpolate_point_index] error: Unhandled interpolation type "
+                           << type << " !");
+    }
+  }
+
+  return (p);
+}

test/common/CMakeLists.txt

@@ -14,5 +14,6 @@ PCL_ADD_TEST(common_eigen test_eigen FILES test_eigen.cpp LINK_WITH pcl_gtest pc
 PCL_ADD_TEST(common_intensity test_intensity FILES test_intensity.cpp LINK_WITH pcl_gtest pcl_common)
 PCL_ADD_TEST(common_generator test_generator FILES test_generator.cpp LINK_WITH pcl_gtest pcl_common)
 PCL_ADD_TEST(common_io test_common_io FILES test_io.cpp LINK_WITH pcl_gtest pcl_common)
+PCL_ADD_TEST(common_copy_make_borders test_copy_make_borders FILES test_copy_make_borders.cpp LINK_WITH pcl_gtest pcl_common)
 
 PCL_ADD_TEST(common_point_type_conversion test_common_point_type_conversion FILES test_point_type_conversion.cpp LINK_WITH pcl_gtest pcl_common)