Point Cloud Library (PCL)  1.12.1-dev
orr_graph.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_graph.h
41  *
42  * Created on: Nov 23, 2012
43  * Author: papazov
44  */
45 
46 #pragma once
47 
48 #include <algorithm>
49 #include <cstddef>
50 #include <list>
51 #include <set>
52 #include <vector>
53 
54 namespace pcl
55 {
56  namespace recognition
57  {
58  template<class NodeData>
59  class ORRGraph
60  {
61  public:
62  class Node
63  {
64  public:
65  enum State {ON, OFF, UNDEF};
66 
67  Node (int id)
68  : id_ (id),
69  state_(UNDEF)
70  {}
71 
72  virtual ~Node () = default;
73 
74  inline const std::set<Node*>&
75  getNeighbors () const
76  {
77  return (neighbors_);
78  }
79 
80  inline const NodeData&
81  getData () const
82  {
83  return (data_);
84  }
85 
86  inline void
87  setData (const NodeData& data)
88  {
89  data_ = data;
90  }
91 
92  inline int
93  getId () const
94  {
95  return (id_);
96  }
97 
98  inline void
99  setId (int id)
100  {
101  id_ = id;
102  }
103 
104  inline void
105  setFitness (int fitness)
106  {
107  fitness_ = fitness;
108  }
109 
110  static inline bool
111  compare (const Node* a, const Node* b)
112  {
113  return a->fitness_ > b->fitness_;
114  }
115 
116  friend class ORRGraph;
117 
118  protected:
119  std::set<Node*> neighbors_;
120  NodeData data_;
121  int id_;
122  int fitness_;
124  };
125 
126  public:
127  ORRGraph () = default;
128  virtual ~ORRGraph (){ this->clear ();}
129 
130  inline void
131  clear ()
132  {
133  for ( auto nit = nodes_.begin () ; nit != nodes_.end () ; ++nit )
134  delete *nit;
135 
136  nodes_.clear ();
137  }
138 
139  /** \brief Drops all existing graph nodes and creates 'n' new ones. */
140  inline void
141  resize (int n)
142  {
143  if ( !n )
144  return;
145 
146  for ( auto nit = nodes_.begin () ; nit != nodes_.end () ; ++nit )
147  delete *nit;
148 
149  nodes_.resize (static_cast<std::size_t> (n));
150 
151  for ( int i = 0 ; i < n ; ++i )
152  nodes_[i] = new Node (i);
153  }
154 
155  inline void
156  computeMaximalOnOffPartition (std::list<Node*>& on_nodes, std::list<Node*>& off_nodes)
157  {
158  std::vector<Node*> sorted_nodes (nodes_.size ());
159  int i = 0;
160 
161  // Set all nodes to undefined
162  for ( auto it = nodes_.begin () ; it != nodes_.end () ; ++it )
163  {
164  sorted_nodes[i++] = *it;
165  (*it)->state_ = Node::UNDEF;
166  }
167 
168  // Now sort the nodes according to the fitness
169  std::sort (sorted_nodes.begin (), sorted_nodes.end (), Node::compare);
170 
171  // Now run through the array and start switching nodes on and off
172  for ( auto it = sorted_nodes.begin () ; it != sorted_nodes.end () ; ++it )
173  {
174  // Ignore graph nodes which are already OFF
175  if ( (*it)->state_ == Node::OFF )
176  continue;
177 
178  // Set the node to ON
179  (*it)->state_ = Node::ON;
180 
181  // Set all its neighbors to OFF
182  for ( auto neigh = (*it)->neighbors_.begin () ; neigh != (*it)->neighbors_.end () ; ++neigh )
183  {
184  (*neigh)->state_ = Node::OFF;
185  off_nodes.push_back (*neigh);
186  }
187 
188  // Output the node
189  on_nodes.push_back (*it);
190  }
191  }
192 
193  inline void
194  insertUndirectedEdge (int id1, int id2)
195  {
196  nodes_[id1]->neighbors_.insert (nodes_[id2]);
197  nodes_[id2]->neighbors_.insert (nodes_[id1]);
198  }
199 
200  inline void
201  insertDirectedEdge (int id1, int id2)
202  {
203  nodes_[id1]->neighbors_.insert (nodes_[id2]);
204  }
205 
206  inline void
207  deleteUndirectedEdge (int id1, int id2)
208  {
209  nodes_[id1]->neighbors_.erase (nodes_[id2]);
210  nodes_[id2]->neighbors_.erase (nodes_[id1]);
211  }
212 
213  inline void
214  deleteDirectedEdge (int id1, int id2)
215  {
216  nodes_[id1]->neighbors_.erase (nodes_[id2]);
217  }
218 
219  inline typename std::vector<Node*>&
220  getNodes (){ return nodes_;}
221 
222  public:
223  typename std::vector<Node*> nodes_;
224  };
225  } // namespace recognition
226 } // namespace pcl
const NodeData & getData() const
Definition: orr_graph.h:81
const std::set< Node * > & getNeighbors() const
Definition: orr_graph.h:75
std::set< Node * > neighbors_
Definition: orr_graph.h:119
void setData(const NodeData &data)
Definition: orr_graph.h:87
void setFitness(int fitness)
Definition: orr_graph.h:105
static bool compare(const Node *a, const Node *b)
Definition: orr_graph.h:111
void computeMaximalOnOffPartition(std::list< Node * > &on_nodes, std::list< Node * > &off_nodes)
Definition: orr_graph.h:156
void resize(int n)
Drops all existing graph nodes and creates 'n' new ones.
Definition: orr_graph.h:141
std::vector< Node * > nodes_
Definition: orr_graph.h:223
void insertDirectedEdge(int id1, int id2)
Definition: orr_graph.h:201
void deleteUndirectedEdge(int id1, int id2)
Definition: orr_graph.h:207
void insertUndirectedEdge(int id1, int id2)
Definition: orr_graph.h:194
std::vector< Node * > & getNodes()
Definition: orr_graph.h:220
void deleteDirectedEdge(int id1, int id2)
Definition: orr_graph.h:214