1 /* 2 * Software License Agreement (BSD License) 3 * 4 * Point Cloud Library (PCL) - www.pointclouds.org 5 * Copyright (c) 2010-2012, Willow Garage, Inc. 6 * 7 * All rights reserved. 8 * 9 * Redistribution and use in source and binary forms, with or without 10 * modification, are permitted provided that the following conditions 11 * are met: 12 * 13 * * Redistributions of source code must retain the above copyright 14 * notice, this list of conditions and the following disclaimer. 15 * * Redistributions in binary form must reproduce the above 16 * copyright notice, this list of conditions and the following 17 * disclaimer in the documentation and/or other materials provided 18 * with the distribution. 19 * * Neither the name of Willow Garage, Inc. nor the names of its 20 * contributors may be used to endorse or promote products derived 21 * from this software without specific prior written permission. 22 * 23 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 * POSSIBILITY OF SUCH DAMAGE. 35 * 36 * 37 */ 38 39 /* 40 * orr_octree_zprojection.h 41 * 42 * Created on: Nov 17, 2012 43 * Author: papazov 44 */ 45 46 #pragma once 47 48 #include "orr_octree.h" 49 #include <pcl/pcl_exports.h> 50 #include <set> 51 52 namespace pcl 53 { 54 namespace recognition 55 { 56 class PCL_EXPORTS ORROctreeZProjection 57 { 58 public: 59 class Pixel 60 { 61 public: Pixel(int id)62 Pixel (int id): id_ (id) {} 63 64 inline void set_z1(float z1)65 set_z1 (float z1) { z1_ = z1;} 66 67 inline void set_z2(float z2)68 set_z2 (float z2) { z2_ = z2;} 69 70 float z1()71 z1 () const { return z1_;} 72 73 float z2()74 z2 () const { return z2_;} 75 76 int getId()77 getId () const { return id_;} 78 79 protected: 80 float z1_, z2_; 81 int id_; 82 }; 83 84 public: 85 class Set 86 { 87 public: Set(int x,int y)88 Set (int x, int y) 89 : nodes_ (compare_nodes_z), x_ (x), y_ (y) 90 {} 91 92 static inline bool compare_nodes_z(ORROctree::Node * node1,ORROctree::Node * node2)93 compare_nodes_z (ORROctree::Node* node1, ORROctree::Node* node2) 94 { 95 return node1->getData()->get3dIdZ() < node2->getData()->get3dIdZ(); 96 } 97 98 inline void insert(ORROctree::Node * leaf)99 insert (ORROctree::Node* leaf) { nodes_.insert(leaf);} 100 101 inline std::set<ORROctree::Node*, bool(*)(ORROctree::Node*,ORROctree::Node*)>& get_nodes()102 get_nodes (){ return nodes_;} 103 104 inline int get_x()105 get_x () const { return x_;} 106 107 inline int get_y()108 get_y () const { return y_;} 109 110 protected: 111 std::set<ORROctree::Node*, bool(*)(ORROctree::Node*,ORROctree::Node*)> nodes_; 112 int x_, y_; 113 }; 114 115 public: ORROctreeZProjection()116 ORROctreeZProjection () 117 : pixels_(nullptr), 118 sets_(nullptr) 119 {} ~ORROctreeZProjection()120 virtual ~ORROctreeZProjection (){ this->clear();} 121 122 void 123 build (const ORROctree& input, float eps_front, float eps_back); 124 125 void 126 clear (); 127 128 inline void getPixelCoordinates(const float * p,int & x,int & y)129 getPixelCoordinates (const float* p, int& x, int& y) const 130 { 131 x = static_cast<int> ((p[0] - bounds_[0])*inv_pixel_size_); 132 y = static_cast<int> ((p[1] - bounds_[2])*inv_pixel_size_); 133 } 134 135 inline const Pixel* getPixel(const float * p)136 getPixel (const float* p) const 137 { 138 int x, y; this->getPixelCoordinates (p, x, y); 139 140 if ( x < 0 || x >= num_pixels_x_ ) return (nullptr); 141 if ( y < 0 || y >= num_pixels_y_ ) return (nullptr); 142 143 return (pixels_[x][y]); 144 } 145 146 inline Pixel* getPixel(const float * p)147 getPixel (const float* p) 148 { 149 int x, y; this->getPixelCoordinates (p, x, y); 150 151 if ( x < 0 || x >= num_pixels_x_ ) return (nullptr); 152 if ( y < 0 || y >= num_pixels_y_ ) return (nullptr); 153 154 return (pixels_[x][y]); 155 } 156 157 inline const std::set<ORROctree::Node*, bool(*)(ORROctree::Node*,ORROctree::Node*)>* getOctreeNodes(const float * p)158 getOctreeNodes (const float* p) const 159 { 160 int x, y; this->getPixelCoordinates (p, x, y); 161 162 if ( x < 0 || x >= num_pixels_x_ ) return (nullptr); 163 if ( y < 0 || y >= num_pixels_y_ ) return (nullptr); 164 165 if ( !sets_[x][y] ) 166 return nullptr; 167 168 return (&sets_[x][y]->get_nodes ()); 169 } 170 171 inline std::list<Pixel*>& getFullPixels()172 getFullPixels (){ return full_pixels_;} 173 174 inline const Pixel* getPixel(int i,int j)175 getPixel (int i, int j) const 176 { 177 return pixels_[i][j]; 178 } 179 180 inline float getPixelSize()181 getPixelSize () const 182 { 183 return pixel_size_; 184 } 185 186 inline const float* getBounds()187 getBounds () const 188 { 189 return bounds_; 190 } 191 192 /** \brief Get the width ('num_x') and height ('num_y') of the image. */ 193 inline void getNumberOfPixels(int & num_x,int & num_y)194 getNumberOfPixels (int& num_x, int& num_y) const 195 { 196 num_x = num_pixels_x_; 197 num_y = num_pixels_y_; 198 } 199 200 protected: 201 float pixel_size_, inv_pixel_size_, bounds_[4], extent_x_, extent_y_; 202 int num_pixels_x_, num_pixels_y_, num_pixels_; 203 Pixel ***pixels_; 204 Set ***sets_; 205 std::list<Set*> full_sets_; 206 std::list<Pixel*> full_pixels_; 207 }; 208 } // namespace recognition 209 } // namespace pcl 210