Point Cloud Library (PCL)
1.7.0
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Point Cloud Library (PCL) - www.pointclouds.org 00005 * Copyright (c) 2010-2011, Willow Garage, Inc. 00006 * 00007 * All rights reserved. 00008 * 00009 * Redistribution and use in source and binary forms, with or without 00010 * modification, are permitted provided that the following conditions 00011 * are met: 00012 * 00013 * * Redistributions of source code must retain the above copyright 00014 * notice, this list of conditions and the following disclaimer. 00015 * * Redistributions in binary form must reproduce the above 00016 * copyright notice, this list of conditions and the following 00017 * disclaimer in the documentation and/or other materials provided 00018 * with the distribution. 00019 * * Neither the name of Willow Garage, Inc. nor the names of its 00020 * contributors may be used to endorse or promote products derived 00021 * from this software without specific prior written permission. 00022 * 00023 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00024 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00025 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00026 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00027 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00028 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00029 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00030 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00031 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00032 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00033 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00034 * POSSIBILITY OF SUCH DAMAGE. 00035 * 00036 */ 00037 00038 #ifndef PCL_RECOGNITION_LINEMOD_LINE_RGBD 00039 #define PCL_RECOGNITION_LINEMOD_LINE_RGBD 00040 00041 #include <pcl/recognition/linemod.h> 00042 #include <pcl/recognition/color_gradient_modality.h> 00043 #include <pcl/recognition/surface_normal_modality.h> 00044 #include <pcl/io/tar.h> 00045 00046 namespace pcl 00047 { 00048 00049 struct BoundingBoxXYZ 00050 { 00051 /** \brief Constructor. */ 00052 BoundingBoxXYZ () : x (0.0f), y (0.0f), z (0.0f), width (0.0f), height (0.0f), depth (0.0f) {} 00053 00054 /** \brief X-coordinate of the upper left front point */ 00055 float x; 00056 /** \brief Y-coordinate of the upper left front point */ 00057 float y; 00058 /** \brief Z-coordinate of the upper left front point */ 00059 float z; 00060 00061 /** \brief Width of the bounding box */ 00062 float width; 00063 /** \brief Height of the bounding box */ 00064 float height; 00065 /** \brief Depth of the bounding box */ 00066 float depth; 00067 }; 00068 00069 /** \brief High-level class for template matching using the LINEMOD approach based on RGB and Depth data. 00070 * \author Stefan Holzer 00071 */ 00072 template <typename PointXYZT, typename PointRGBT=PointXYZT> 00073 class PCL_EXPORTS LineRGBD 00074 { 00075 public: 00076 00077 /** \brief A LineRGBD detection. */ 00078 struct Detection 00079 { 00080 /** \brief Constructor. */ 00081 Detection () : template_id (0), object_id (0), detection_id (0), response (0.0f), bounding_box () {} 00082 00083 /** \brief The ID of the template. */ 00084 size_t template_id; 00085 /** \brief The ID of the object corresponding to the template. */ 00086 size_t object_id; 00087 /** \brief The ID of this detection. This is only valid for the last call of the method detect (...). */ 00088 size_t detection_id; 00089 /** \brief The response of this detection. Responses are between 0 and 1, where 1 is best. */ 00090 float response; 00091 /** \brief The 3D bounding box of the detection. */ 00092 BoundingBoxXYZ bounding_box; 00093 /** \brief The 2D template region of the detection. */ 00094 RegionXY region; 00095 }; 00096 00097 /** \brief Constructor */ 00098 LineRGBD () 00099 : intersection_volume_threshold_ (1.0f) 00100 , linemod_ () 00101 , color_gradient_mod_ () 00102 , surface_normal_mod_ () 00103 , cloud_xyz_ () 00104 , cloud_rgb_ () 00105 , template_point_clouds_ () 00106 , bounding_boxes_ () 00107 , object_ids_ () 00108 , detections_ () 00109 { 00110 } 00111 00112 /** \brief Destructor */ 00113 virtual ~LineRGBD () 00114 { 00115 } 00116 00117 /** \brief Loads templates from a LMT (LineMod Template) file. Overrides old templates. 00118 * 00119 * LineMod Template files are TAR files that store pairs of PCD datasets 00120 * together with their LINEMOD signatures in \ref 00121 * SparseQuantizedMultiModTemplate format. 00122 * 00123 * \param[in] file_name The name of the file that stores the templates. 00124 * 00125 * \return true, if the operation was succesful, false otherwise. 00126 */ 00127 bool 00128 loadTemplates (const std::string &file_name, size_t object_id = 0); 00129 00130 bool 00131 addTemplate (const SparseQuantizedMultiModTemplate & sqmmt, pcl::PointCloud<pcl::PointXYZRGBA> & cloud, size_t object_id = 0); 00132 00133 /** \brief Sets the threshold for the detection responses. Responses are between 0 and 1, where 1 is a best. 00134 * \param[in] threshold The threshold used to decide where a template is detected. 00135 */ 00136 inline void 00137 setDetectionThreshold (float threshold) 00138 { 00139 linemod_.setDetectionThreshold (threshold); 00140 } 00141 00142 /** \brief Sets the threshold on the magnitude of color gradients. Color gradients with a magnitude below 00143 * this threshold are not considered in the detection process. 00144 * \param[in] threshold The threshold on the magnitude of color gradients. 00145 */ 00146 inline void 00147 setGradientMagnitudeThreshold (const float threshold) 00148 { 00149 color_gradient_mod_.setGradientMagnitudeThreshold (threshold); 00150 } 00151 00152 /** \brief Sets the threshold for the decision whether two detections of the same template are merged or not. 00153 * If ratio between the intersection of the bounding boxes of two detections and the original bounding 00154 * boxes is larger than the specified threshold then they are merged. If detection A overlaps with 00155 * detection B and B with C than A, B, and C are merged. Threshold has to be between 0 and 1. 00156 * \param[in] threshold The threshold on the ratio between the intersection bounding box and the original 00157 * bounding box. 00158 */ 00159 inline void 00160 setIntersectionVolumeThreshold (const float threshold = 1.0f) 00161 { 00162 intersection_volume_threshold_ = threshold; 00163 } 00164 00165 /** \brief Sets the input cloud with xyz point coordinates. The cloud has to be organized. 00166 * \param[in] cloud The input cloud with xyz point coordinates. 00167 */ 00168 inline void 00169 setInputCloud (const typename pcl::PointCloud<PointXYZT>::ConstPtr & cloud) 00170 { 00171 cloud_xyz_ = cloud; 00172 00173 surface_normal_mod_.setInputCloud (cloud); 00174 surface_normal_mod_.processInputData (); 00175 } 00176 00177 /** \brief Sets the input cloud with rgb values. The cloud has to be organized. 00178 * \param[in] cloud The input cloud with rgb values. 00179 */ 00180 inline void 00181 setInputColors (const typename pcl::PointCloud<PointRGBT>::ConstPtr & cloud) 00182 { 00183 cloud_rgb_ = cloud; 00184 00185 color_gradient_mod_.setInputCloud (cloud); 00186 color_gradient_mod_.processInputData (); 00187 } 00188 00189 /** \brief Creates a template from the specified data and adds it to the matching queue. 00190 * \param[in] mask_xyz the mask that determine which parts of the xyz-modality are used for creating the template. 00191 * \param[in] mask_rgb the mask that determine which parts of the rgb-modality are used for creating the template. 00192 * \param[in] region the region which will be associated with the template (can be larger than the actual modality-maps). 00193 */ 00194 int 00195 createAndAddTemplate ( 00196 pcl::PointCloud<pcl::PointXYZRGBA> & cloud, 00197 const size_t object_id, 00198 const MaskMap & mask_xyz, 00199 const MaskMap & mask_rgb, 00200 const RegionXY & region); 00201 00202 00203 /** \brief Applies the detection process and fills the supplied vector with the detection instances. 00204 * \param[out] detections The storage for the detection instances. 00205 */ 00206 void 00207 detect (std::vector<typename pcl::LineRGBD<PointXYZT, PointRGBT>::Detection> & detections); 00208 00209 /** \brief Applies the detection process in a semi-scale-invariant manner. This is done by acutally 00210 * scaling the template to different sizes. 00211 */ 00212 void 00213 detectSemiScaleInvariant (std::vector<typename pcl::LineRGBD<PointXYZT, PointRGBT>::Detection> & detections, 00214 float min_scale = 0.6944444f, 00215 float max_scale = 1.44f, 00216 float scale_multiplier = 1.2f); 00217 00218 /** \brief Computes and returns the point cloud of the specified detection. This is the template point 00219 * cloud transformed to the detection coordinates. The detection ID refers to the last call of 00220 * the method detect (...). 00221 * \param[in] detection_id The ID of the detection (according to the last call of the method detect (...)). 00222 * \param[out] cloud The storage for the transformed points. 00223 */ 00224 void 00225 computeTransformedTemplatePoints (const size_t detection_id, 00226 pcl::PointCloud<pcl::PointXYZRGBA> & cloud); 00227 00228 /** \brief Finds the indices of the points in the input cloud which correspond to the specified detection. 00229 * The detection ID refers to the last call of the method detect (...). 00230 * \param[in] detection_id The ID of the detection (according to the last call of the method detect (...)). 00231 */ 00232 inline std::vector<size_t> 00233 findObjectPointIndices (const size_t detection_id) 00234 { 00235 if (detection_id >= detections_.size ()) 00236 PCL_ERROR ("ERROR pcl::LineRGBD::computeTransformedTemplatePoints - detection_id is out of bounds\n"); 00237 00238 // TODO: compute transform from detection 00239 // TODO: transform template points 00240 std::vector<size_t> vec; 00241 return (vec); 00242 } 00243 00244 00245 protected: 00246 00247 /** \brief Aligns the template points with the points found at the detection position of the specified detection. 00248 * The detection ID refers to the last call of the method detect (...). 00249 * \param[in] detection_id The ID of the detection (according to the last call of the method detect (...)). 00250 */ 00251 inline std::vector<size_t> 00252 alignTemplatePoints (const size_t detection_id) 00253 { 00254 if (detection_id >= detections_.size ()) 00255 PCL_ERROR ("ERROR pcl::LineRGBD::computeTransformedTemplatePoints - detection_id is out of bounds\n"); 00256 00257 // TODO: compute transform from detection 00258 // TODO: transform template points 00259 std::vector<size_t> vec; 00260 return (vec); 00261 } 00262 00263 /** \brief Refines the found detections along the depth. */ 00264 void 00265 refineDetectionsAlongDepth (); 00266 00267 /** \brief Applies projective ICP on detections to find their correct position in depth. */ 00268 void 00269 applyProjectiveDepthICPOnDetections (); 00270 00271 /** \brief Checks for overlapping detections, removes them and keeps only the strongest one. */ 00272 void 00273 removeOverlappingDetections (); 00274 00275 /** \brief Computes the volume of the intersection between two bounding boxes. 00276 * \param[in] First bounding box. 00277 * \param[in] Second bounding box. 00278 */ 00279 static float 00280 computeBoundingBoxIntersectionVolume (const BoundingBoxXYZ &box1, const BoundingBoxXYZ &box2); 00281 00282 private: 00283 /** \brief Read another LTM header chunk. */ 00284 bool 00285 readLTMHeader (int fd, pcl::io::TARHeader &header); 00286 00287 /** \brief Intersection volume threshold. */ 00288 float intersection_volume_threshold_; 00289 00290 /** \brief LINEMOD instance. */ 00291 public: pcl::LINEMOD linemod_; 00292 /** \brief Color gradient modality. */ 00293 pcl::ColorGradientModality<PointRGBT> color_gradient_mod_; 00294 /** \brief Surface normal modality. */ 00295 pcl::SurfaceNormalModality<PointXYZT> surface_normal_mod_; 00296 00297 /** \brief XYZ point cloud. */ 00298 typename pcl::PointCloud<PointXYZT>::ConstPtr cloud_xyz_; 00299 /** \brief RGB point cloud. */ 00300 typename pcl::PointCloud<PointRGBT>::ConstPtr cloud_rgb_; 00301 00302 /** \brief Point clouds corresponding to the templates. */ 00303 pcl::PointCloud<pcl::PointXYZRGBA>::CloudVectorType template_point_clouds_; 00304 /** \brief Bounding boxes corresonding to the templates. */ 00305 std::vector<pcl::BoundingBoxXYZ> bounding_boxes_; 00306 /** \brief Object IDs corresponding to the templates. */ 00307 std::vector<size_t> object_ids_; 00308 00309 /** \brief Detections from last call of method detect (...). */ 00310 std::vector<typename pcl::LineRGBD<PointXYZT, PointRGBT>::Detection> detections_; 00311 }; 00312 00313 } 00314 00315 #include <pcl/recognition/impl/linemod/line_rgbd.hpp> 00316 00317 #endif