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