diff --git a/recognition/include/pcl/recognition/impl/linemod/line_rgbd.hpp b/recognition/include/pcl/recognition/impl/linemod/line_rgbd.hpp index bc305c48607..0ff5d32c8ac 100644 --- a/recognition/include/pcl/recognition/impl/linemod/line_rgbd.hpp +++ b/recognition/include/pcl/recognition/impl/linemod/line_rgbd.hpp @@ -218,7 +218,149 @@ LineRGBD::loadTemplates (const std::string &file_name, con return (true); } +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::LineRGBD::loadTemplateSQMMT ( + const std::vector &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 bool +pcl::LineRGBD::loadTemplatePCD ( + const std::vector &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 & template_point_cloud = template_point_clouds_[i]; + BoundingBoxXYZ & bb = bounding_boxes_[i]; + bb.x = bb.y = bb.z = std::numeric_limits::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::max (); + float min_y = std::numeric_limits::max (); + float min_z = std::numeric_limits::max (); + float max_x = -std::numeric_limits::max (); + float max_y = -std::numeric_limits::max (); + float max_z = -std::numeric_limits::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 (counter); + center_y /= static_cast (counter); + center_z /= static_cast (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 int LineRGBD::createAndAddTemplate ( pcl::PointCloud & cloud, diff --git a/recognition/include/pcl/recognition/linemod/line_rgbd.h b/recognition/include/pcl/recognition/linemod/line_rgbd.h index 340b8cee065..c8c837ee368 100644 --- a/recognition/include/pcl/recognition/linemod/line_rgbd.h +++ b/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 &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 &file_name, std::size_t object_id = 0); + bool addTemplate (const SparseQuantizedMultiModTemplate & sqmmt, pcl::PointCloud & cloud, std::size_t object_id = 0);