00001 #ifndef __D3D_DESCRIPTORS_3D_H__ 00002 #define __D3D_DESCRIPTORS_3D_H__ 00003 /********************************************************************* 00004 * Software License Agreement (BSD License) 00005 * 00006 * Copyright (c) 2009, Willow Garage 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 the Willow Garage 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 #include <vector> 00038 #include <set> 00039 00040 #include <boost/shared_array.hpp> 00041 00042 #include <opencv/cxcore.h> 00043 #include <opencv/cv.h> 00044 #include <opencv/cvaux.hpp> 00045 00046 #include <ros/console.h> 00047 00048 #include <sensor_msgs/PointCloud.h> 00049 00050 #include <point_cloud_mapping/kdtree/kdtree.h> 00051 00052 // -------------------------------------------------------------- 00059 // -------------------------------------------------------------- 00060 00061 // -------------------------------------------------------------- 00071 // -------------------------------------------------------------- 00072 class Descriptor3D 00073 { 00074 public: 00075 // -------------------------------------------------------------- 00079 // -------------------------------------------------------------- 00080 Descriptor3D(); 00081 00082 virtual ~Descriptor3D() = 0; 00083 00084 // -------------------------------------------------------------- 00090 // -------------------------------------------------------------- 00091 virtual void clearShared() = 0; 00092 00093 // -------------------------------------------------------------- 00107 // -------------------------------------------------------------- 00108 void compute(const sensor_msgs::PointCloud& data, 00109 cloud_kdtree::KdTree& data_kdtree, 00110 const std::vector<const geometry_msgs::Point32*>& interest_pts, 00111 std::vector<std::vector<float> >& results); 00112 00113 // -------------------------------------------------------------- 00127 // -------------------------------------------------------------- 00128 void compute(const sensor_msgs::PointCloud& data, 00129 cloud_kdtree::KdTree& data_kdtree, 00130 const std::vector<const std::vector<int>*>& interest_region_indices, 00131 std::vector<std::vector<float> >& results); 00132 00133 // -------------------------------------------------------------- 00139 // -------------------------------------------------------------- 00140 inline unsigned int getResultSize() const 00141 { 00142 return result_size_; 00143 } 00144 00145 // -------------------------------------------------------------- 00151 // -------------------------------------------------------------- 00152 virtual std::string getName() const = 0; 00153 00154 // =================================================================== 00156 // =================================================================== 00158 // -------------------------------------------------------------- 00176 // -------------------------------------------------------------- 00177 static unsigned int 00178 computeAndConcatFeatures(const sensor_msgs::PointCloud& data, 00179 cloud_kdtree::KdTree& data_kdtree, 00180 const std::vector<const geometry_msgs::Point32*>& interest_pts, 00181 std::vector<Descriptor3D*>& descriptors_3d, 00182 std::vector<boost::shared_array<const float> >& concatenated_features); 00183 // -------------------------------------------------------------- 00201 // -------------------------------------------------------------- 00202 static unsigned int 00203 computeAndConcatFeatures(const sensor_msgs::PointCloud& data, 00204 cloud_kdtree::KdTree& data_kdtree, 00205 const std::vector<const std::vector<int>*>& interest_region_indices, 00206 std::vector<Descriptor3D*>& descriptors_3d, 00207 std::vector<boost::shared_array<const float> >& concatenated_features); 00209 00211 bool debug_; 00212 protected: 00213 // -------------------------------------------------------------- 00224 // -------------------------------------------------------------- 00225 virtual int precompute(const sensor_msgs::PointCloud& data, 00226 cloud_kdtree::KdTree& data_kdtree, 00227 const std::vector<const geometry_msgs::Point32*>& interest_pts) = 0; 00228 00229 // -------------------------------------------------------------- 00242 // -------------------------------------------------------------- 00243 virtual int precompute(const sensor_msgs::PointCloud& data, 00244 cloud_kdtree::KdTree& data_kdtree, 00245 const std::vector<const std::vector<int>*>& interest_region_indices) = 0; 00246 00247 // -------------------------------------------------------------- 00253 // -------------------------------------------------------------- 00254 virtual void doComputation(const sensor_msgs::PointCloud& data, 00255 cloud_kdtree::KdTree& data_kdtree, 00256 const std::vector<const geometry_msgs::Point32*>& interest_pts, 00257 std::vector<std::vector<float> >& results) = 0; 00258 00259 // -------------------------------------------------------------- 00265 // -------------------------------------------------------------- 00266 virtual void doComputation(const sensor_msgs::PointCloud& data, 00267 cloud_kdtree::KdTree& data_kdtree, 00268 const std::vector<const std::vector<int>*>& interest_region_indices, 00269 std::vector<std::vector<float> >& results) = 0; 00270 00272 unsigned int result_size_; 00273 00275 bool result_size_defined_; 00276 00277 private: 00278 // -------------------------------------------------------------- 00289 // -------------------------------------------------------------- 00290 static void 00291 concatenateFeatures(const std::vector<std::vector<std::vector<float> > >& all_descriptor_results, 00292 const unsigned int nbr_samples, 00293 const unsigned int nbr_concatenated_vals, 00294 std::vector<boost::shared_array<const float> >& concatenated_features); 00295 }; 00296 00297 #endif