Linergbd loadtemplates fix

recognition/include/pcl/recognition/impl/linemod/line_rgbd.hpp

@@ -218,7 +218,149 @@ LineRGBD<PointXYZT, PointRGBT>::loadTemplates (const std::string &file_name, con
   return (true);
 }
 
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointXYZT, typename PointRGBT> bool
+pcl::LineRGBD<PointXYZT, PointRGBT>::loadTemplateSQMMT (
+  const std::vector<std::string> &file_name, 
+  const std::size_t object_id)
+{
+  std::string sqmmt_ext (".sqmmt");
+  std::size_t file_loaded = 0;
+
+  for (const auto file : file_name)
+  {
+    std::transform (file.begin (), file.end (), file.begin (), ::tolower);
+    std::string::size_type it;
+
+    if ((it = file.find (sqmmt_ext)) == std::string::npos)
+    {
+      PCL_WARN ("%s: Invalid file type. Skipping.\n", file.c_str());
+      continue;
+    }
+
+    std::ifstream stream;
+    int nr_templates;
+
+    stream.open (file, std::stringstream::in | std::stringstream::out | std::stringstream::binary);
+    read (stream, nr_templates);
+    for (std::size_t i=0; i<nr_templates; ++i)
+    {
+      SparseQuantizedMultiModTemplate sqmmt;
+      sqmmt.deserialize (stream);
+
+      linemod_.addTemplate (sqmmt);
+      object_ids_.push_back (object_id);
+    }
+
+    ++file_loaded;
+  }
+
+  if (file_loaded < file_name.size())
+    return (false);
+
+  return (true);
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointXYZT, typename PointRGBT> bool
+pcl::LineRGBD<PointXYZT, PointRGBT>::loadTemplatePCD (
+  const std::vector<std::string> &file_name, 
+  const std::size_t object_id)
+{
+  std::string pcd_ext (".pcd");
+  PCDReader pcd_reader;
+  std::size_t file_loaded = 0;
+
+  for (const auto file : file_name)
+  {
+    std::transform (file.begin (), file.end (), file.begin (), ::tolower);
+    std::string::size_type it;
+
+    if ((it = file.find (pcd_ext)) == std::string::npos)
+    {
+      PCL_WARN ("%s: Invalid file type. Skipping.\n", file.c_str());
+      continue;
+    }
+
+    template_point_clouds_.resize (template_point_clouds_.size () + 1);
+    pcd_reader.read (file, template_point_clouds_[template_point_clouds_.size () - 1]);
+
+    ++file_loaded;
+  }
+
+  bounding_boxes_.resize (template_point_clouds_.size ());
+  for (std::size_t i = 0; i < template_point_clouds_.size (); ++i)
+  {
+    PointCloud<PointXYZRGBA> & template_point_cloud = template_point_clouds_[i];
+    BoundingBoxXYZ & bb = bounding_boxes_[i];
+    bb.x = bb.y = bb.z = std::numeric_limits<float>::max ();
+    bb.width = bb.height = bb.depth = 0.0f;
+
+    float center_x = 0.0f;
+    float center_y = 0.0f;
+    float center_z = 0.0f;
+    float min_x = std::numeric_limits<float>::max ();
+    float min_y = std::numeric_limits<float>::max ();
+    float min_z = std::numeric_limits<float>::max ();
+    float max_x = -std::numeric_limits<float>::max ();
+    float max_y = -std::numeric_limits<float>::max ();
+    float max_z = -std::numeric_limits<float>::max ();
+    std::size_t counter = 0;
+    for (std::size_t j = 0; j < template_point_cloud.size (); ++j)
+    {
+      const PointXYZRGBA & p = template_point_cloud.points[j];
+
+      if (!std::isfinite (p.x) || !std::isfinite (p.y) || !std::isfinite (p.z))
+        continue;
+
+      min_x = std::min (min_x, p.x);
+      min_y = std::min (min_y, p.y);
+      min_z = std::min (min_z, p.z);
+      max_x = std::max (max_x, p.x);
+      max_y = std::max (max_y, p.y);
+      max_z = std::max (max_z, p.z);
+
+      center_x += p.x;
+      center_y += p.y;
+      center_z += p.z;
+
+      ++counter;
+    }
+
+    center_x /= static_cast<float> (counter);
+    center_y /= static_cast<float> (counter);
+    center_z /= static_cast<float> (counter);
+
+    bb.width  = max_x - min_x;
+    bb.height = max_y - min_y;
+    bb.depth  = max_z - min_z;
+
+    bb.x = (min_x + bb.width / 2.0f) - center_x - bb.width / 2.0f;
+    bb.y = (min_y + bb.height / 2.0f) - center_y - bb.height / 2.0f;
+    bb.z = (min_z + bb.depth / 2.0f) - center_z - bb.depth / 2.0f;
+
+    for (std::size_t j = 0; j < template_point_cloud.size (); ++j)
+    {
+      PointXYZRGBA p = template_point_cloud.points[j];
+
+      if (!std::isfinite (p.x) || !std::isfinite (p.y) || !std::isfinite (p.z))
+        continue;
+
+      p.x -= center_x;
+      p.y -= center_y;
+      p.z -= center_z;
+
+      template_point_cloud.points[j] = p;
+    }
+  }
+
+  if (file_loaded < file_name.size())
+    return (false);
+
+  return (true);
+}
 
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 template <typename PointXYZT, typename PointRGBT> int
 LineRGBD<PointXYZT, PointRGBT>::createAndAddTemplate (
   pcl::PointCloud<pcl::PointXYZRGBA> & cloud,

recognition/include/pcl/recognition/linemod/line_rgbd.h

@@ -123,6 +123,26 @@ namespace pcl
       bool
       loadTemplates (const std::string &file_name, std::size_t object_id = 0);
 
+      /** \brief Loads templates from a SQMMT file. Overrides old templates.
+        *
+        * \param[in] file_name The vector of file names that stores the templates.
+        * \param object_id
+        *
+        * \return true, if all files were loaded, false otherwise.
+        */
+      bool
+      loadTemplateSQMMT (const std::vector<std::string> &file_name, std::size_t object_id = 0);
+
+      /** \brief Loads templates from a LMT (LineMod Template) file. Overrides old templates.
+        *
+        * \param[in] file_name The vector of file names that stores the template PCD masks.
+        * \param object_id
+        *
+        * \return true, if all files were loaded, false otherwise.
+        */
+      bool
+      loadTemplatePCD (const std::vector<std::string> &file_name, std::size_t object_id = 0);
+
       bool
       addTemplate (const SparseQuantizedMultiModTemplate & sqmmt, pcl::PointCloud<pcl::PointXYZRGBA> & cloud, std::size_t object_id = 0);