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-2012, 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 00039 /* 00040 * orr_octree_zprojection.h 00041 * 00042 * Created on: Nov 17, 2012 00043 * Author: papazov 00044 */ 00045 00046 #ifndef ORR_OCTREE_ZPROJECTION_H_ 00047 #define ORR_OCTREE_ZPROJECTION_H_ 00048 00049 #include "orr_octree.h" 00050 #include <pcl/pcl_exports.h> 00051 #include <set> 00052 00053 00054 namespace pcl 00055 { 00056 namespace recognition 00057 { 00058 class ORROctree; 00059 00060 class PCL_EXPORTS ORROctreeZProjection 00061 { 00062 public: 00063 class Pixel 00064 { 00065 public: 00066 Pixel (int id): id_ (id) {} 00067 00068 inline void 00069 set_z1 (float z1) { z1_ = z1;} 00070 00071 inline void 00072 set_z2 (float z2) { z2_ = z2;} 00073 00074 float 00075 z1 () const { return z1_;} 00076 00077 float 00078 z2 () const { return z2_;} 00079 00080 int 00081 getId () const { return id_;} 00082 00083 protected: 00084 float z1_, z2_; 00085 int id_; 00086 }; 00087 00088 public: 00089 class Set 00090 { 00091 public: 00092 Set (int x, int y) 00093 : nodes_ (compare_nodes_z), x_ (x), y_ (y) 00094 {} 00095 00096 static inline bool 00097 compare_nodes_z (ORROctree::Node* node1, ORROctree::Node* node2) 00098 { 00099 return static_cast<bool> (node1->getData ()->get3dIdZ () < node2->getData ()->get3dIdZ ()); 00100 } 00101 00102 inline void 00103 insert (ORROctree::Node* leaf) { nodes_.insert(leaf);} 00104 00105 inline std::set<ORROctree::Node*, bool(*)(ORROctree::Node*,ORROctree::Node*)>& 00106 get_nodes (){ return nodes_;} 00107 00108 inline int 00109 get_x () const { return x_;} 00110 00111 inline int 00112 get_y () const { return y_;} 00113 00114 protected: 00115 std::set<ORROctree::Node*, bool(*)(ORROctree::Node*,ORROctree::Node*)> nodes_; 00116 int x_, y_; 00117 }; 00118 00119 public: 00120 ORROctreeZProjection () 00121 : pixels_(NULL), 00122 sets_(NULL) 00123 {} 00124 virtual ~ORROctreeZProjection (){ this->clear();} 00125 00126 void 00127 build (const ORROctree& input, float eps_front, float eps_back); 00128 00129 void 00130 clear (); 00131 00132 inline void 00133 getPixelCoordinates (const float* p, int& x, int& y) const 00134 { 00135 x = static_cast<int> ((p[0] - bounds_[0])*inv_pixel_size_); 00136 y = static_cast<int> ((p[1] - bounds_[2])*inv_pixel_size_); 00137 } 00138 00139 inline const Pixel* 00140 getPixel (const float* p) const 00141 { 00142 int x, y; this->getPixelCoordinates (p, x, y); 00143 00144 if ( x < 0 || x >= num_pixels_x_ ) return (NULL); 00145 if ( y < 0 || y >= num_pixels_y_ ) return (NULL); 00146 00147 return (pixels_[x][y]); 00148 } 00149 00150 inline Pixel* 00151 getPixel (const float* p) 00152 { 00153 int x, y; this->getPixelCoordinates (p, x, y); 00154 00155 if ( x < 0 || x >= num_pixels_x_ ) return (NULL); 00156 if ( y < 0 || y >= num_pixels_y_ ) return (NULL); 00157 00158 return (pixels_[x][y]); 00159 } 00160 00161 inline const std::set<ORROctree::Node*, bool(*)(ORROctree::Node*,ORROctree::Node*)>* 00162 getOctreeNodes (const float* p) const 00163 { 00164 int x, y; this->getPixelCoordinates (p, x, y); 00165 00166 if ( x < 0 || x >= num_pixels_x_ ) return (NULL); 00167 if ( y < 0 || y >= num_pixels_y_ ) return (NULL); 00168 00169 if ( !sets_[x][y] ) 00170 return NULL; 00171 00172 return (&sets_[x][y]->get_nodes ()); 00173 } 00174 00175 inline std::list<Pixel*>& 00176 getFullPixels (){ return full_pixels_;} 00177 00178 inline const Pixel* 00179 getPixel (int i, int j) const 00180 { 00181 return pixels_[i][j]; 00182 } 00183 00184 inline float 00185 getPixelSize () const 00186 { 00187 return pixel_size_; 00188 } 00189 00190 inline const float* 00191 getBounds () const 00192 { 00193 return bounds_; 00194 } 00195 00196 /** \brief Get the width ('num_x') and height ('num_y') of the image. */ 00197 inline void 00198 getNumberOfPixels (int& num_x, int& num_y) const 00199 { 00200 num_x = num_pixels_x_; 00201 num_y = num_pixels_y_; 00202 } 00203 00204 protected: 00205 float pixel_size_, inv_pixel_size_, bounds_[4], extent_x_, extent_y_; 00206 int num_pixels_x_, num_pixels_y_, num_pixels_; 00207 Pixel ***pixels_; 00208 Set ***sets_; 00209 std::list<Set*> full_sets_; 00210 std::list<Pixel*> full_pixels_; 00211 }; 00212 } // namespace recognition 00213 } // namespace pcl 00214 00215 00216 #endif /* ORR_OCTREE_ZPROJECTION_H_ */