Point Cloud Library (PCL)  1.7.0
/tmp/buildd/pcl-1.7-1.7.0/recognition/include/pcl/recognition/linemod/line_rgbd.h
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