Point Cloud Library (PCL)  1.14.1-dev
orr_octree_zprojection.h
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  {
57  {
58  public:
59  class Pixel
60  {
61  public:
62  Pixel (int id): id_ (id) {}
63 
64  inline void
65  set_z1 (float z1) { z1_ = z1;}
66 
67  inline void
68  set_z2 (float z2) { z2_ = z2;}
69 
70  float
71  z1 () const { return z1_;}
72 
73  float
74  z2 () const { return z2_;}
75 
76  int
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:
88  Set (int x, int y)
89  : nodes_ (compare_nodes_z), x_ (x), y_ (y)
90  {}
91 
92  static inline bool
94  {
95  return node1->getData()->get3dIdZ() < node2->getData()->get3dIdZ();
96  }
97 
98  inline void
99  insert (ORROctree::Node* leaf) { nodes_.insert(leaf);}
100 
101  inline std::set<ORROctree::Node*, bool(*)(ORROctree::Node*,ORROctree::Node*)>&
102  get_nodes (){ return nodes_;}
103 
104  inline int
105  get_x () const { return x_;}
106 
107  inline int
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:
116  ORROctreeZProjection () = default;
117 
118  virtual ~ORROctreeZProjection (){ this->clear();}
119 
120  void
121  build (const ORROctree& input, float eps_front, float eps_back);
122 
123  void
124  clear ();
125 
126  inline void
127  getPixelCoordinates (const float* p, int& x, int& y) const
128  {
129  x = static_cast<int> ((p[0] - bounds_[0])*inv_pixel_size_);
130  y = static_cast<int> ((p[1] - bounds_[2])*inv_pixel_size_);
131  }
132 
133  inline const Pixel*
134  getPixel (const float* p) const
135  {
136  int x, y; this->getPixelCoordinates (p, x, y);
137 
138  if ( x < 0 || x >= num_pixels_x_ ) return (nullptr);
139  if ( y < 0 || y >= num_pixels_y_ ) return (nullptr);
140 
141  return (pixels_[x][y]);
142  }
143 
144  inline Pixel*
145  getPixel (const float* p)
146  {
147  int x, y; this->getPixelCoordinates (p, x, y);
148 
149  if ( x < 0 || x >= num_pixels_x_ ) return (nullptr);
150  if ( y < 0 || y >= num_pixels_y_ ) return (nullptr);
151 
152  return (pixels_[x][y]);
153  }
154 
155  inline const std::set<ORROctree::Node*, bool(*)(ORROctree::Node*,ORROctree::Node*)>*
156  getOctreeNodes (const float* p) const
157  {
158  int x, y; this->getPixelCoordinates (p, x, y);
159 
160  if ( x < 0 || x >= num_pixels_x_ ) return (nullptr);
161  if ( y < 0 || y >= num_pixels_y_ ) return (nullptr);
162 
163  if ( !sets_[x][y] )
164  return nullptr;
165 
166  return (&sets_[x][y]->get_nodes ());
167  }
168 
169  inline std::list<Pixel*>&
170  getFullPixels (){ return full_pixels_;}
171 
172  inline const Pixel*
173  getPixel (int i, int j) const
174  {
175  return pixels_[i][j];
176  }
177 
178  inline float
179  getPixelSize () const
180  {
181  return pixel_size_;
182  }
183 
184  inline const float*
185  getBounds () const
186  {
187  return bounds_;
188  }
189 
190  /** \brief Get the width ('num_x') and height ('num_y') of the image. */
191  inline void
192  getNumberOfPixels (int& num_x, int& num_y) const
193  {
194  num_x = num_pixels_x_;
195  num_y = num_pixels_y_;
196  }
197 
198  protected:
199  float pixel_size_, inv_pixel_size_, bounds_[4], extent_x_, extent_y_;
200  int num_pixels_x_, num_pixels_y_, num_pixels_;
201  Pixel ***pixels_{nullptr};
202  Set ***sets_{nullptr};
203  std::list<Set*> full_sets_;
204  std::list<Pixel*> full_pixels_;
205  };
206  } // namespace recognition
207 } // namespace pcl
That's a very specialized and simple octree class.
Definition: orr_octree.h:69
static bool compare_nodes_z(ORROctree::Node *node1, ORROctree::Node *node2)
const Pixel * getPixel(int i, int j) const
void getPixelCoordinates(const float *p, int &x, int &y) const
void getNumberOfPixels(int &num_x, int &num_y) const
Get the width ('num_x') and height ('num_y') of the image.
void build(const ORROctree &input, float eps_front, float eps_back)
const Pixel * getPixel(const float *p) const
#define PCL_EXPORTS
Definition: pcl_macros.h:323