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_graph.h 00041 * 00042 * Created on: Nov 23, 2012 00043 * Author: papazov 00044 */ 00045 00046 #ifndef PCL_RECOGNITION_ORR_GRAPH_H_ 00047 #define PCL_RECOGNITION_ORR_GRAPH_H_ 00048 00049 #include <vector> 00050 00051 namespace pcl 00052 { 00053 namespace recognition 00054 { 00055 template<class NodeData> 00056 class ORRGraph 00057 { 00058 public: 00059 class Node 00060 { 00061 public: 00062 enum State {ON, OFF, UNDEF}; 00063 00064 Node (int id) 00065 : id_ (id), 00066 state_(UNDEF) 00067 {} 00068 00069 virtual ~Node (){} 00070 00071 inline const std::set<Node*>& 00072 getNeighbors () const 00073 { 00074 return (neighbors_); 00075 } 00076 00077 inline const NodeData& 00078 getData () const 00079 { 00080 return (data_); 00081 } 00082 00083 inline void 00084 setData (const NodeData& data) 00085 { 00086 data_ = data; 00087 } 00088 00089 inline int 00090 getId () const 00091 { 00092 return (id_); 00093 } 00094 00095 inline void 00096 setId (int id) 00097 { 00098 id_ = id; 00099 } 00100 00101 inline void 00102 setFitness (int fitness) 00103 { 00104 fitness_ = fitness; 00105 } 00106 00107 static inline bool 00108 compare (const Node* a, const Node* b) 00109 { 00110 return (static_cast<bool> (a->fitness_ > b->fitness_)); 00111 } 00112 00113 friend class ORRGraph; 00114 00115 protected: 00116 std::set<Node*> neighbors_; 00117 NodeData data_; 00118 int id_; 00119 int fitness_; 00120 State state_; 00121 }; 00122 00123 public: 00124 ORRGraph (){} 00125 virtual ~ORRGraph (){ this->clear ();} 00126 00127 inline void 00128 clear () 00129 { 00130 for ( typename std::vector<Node*>::iterator nit = nodes_.begin () ; nit != nodes_.end () ; ++nit ) 00131 delete *nit; 00132 00133 nodes_.clear (); 00134 } 00135 00136 /** \brief Drops all existing graph nodes and creates 'n' new ones. */ 00137 inline void 00138 resize (int n) 00139 { 00140 if ( !n ) 00141 return; 00142 00143 for ( typename std::vector<Node*>::iterator nit = nodes_.begin () ; nit != nodes_.end () ; ++nit ) 00144 delete *nit; 00145 00146 nodes_.resize (static_cast<size_t> (n)); 00147 00148 for ( int i = 0 ; i < n ; ++i ) 00149 nodes_[i] = new Node (i); 00150 } 00151 00152 inline void 00153 computeMaximalOnOffPartition (std::list<Node*>& on_nodes, std::list<Node*>& off_nodes) 00154 { 00155 std::vector<Node*> sorted_nodes (nodes_.size ()); 00156 int i = 0; 00157 00158 // Set all nodes to undefined 00159 for ( typename std::vector<Node*>::iterator it = nodes_.begin () ; it != nodes_.end () ; ++it ) 00160 { 00161 sorted_nodes[i++] = *it; 00162 (*it)->state_ = Node::UNDEF; 00163 } 00164 00165 // Now sort the nodes according to the fitness 00166 std::sort (sorted_nodes.begin (), sorted_nodes.end (), Node::compare); 00167 00168 // Now run through the array and start switching nodes on and off 00169 for ( typename std::vector<Node*>::iterator it = sorted_nodes.begin () ; it != sorted_nodes.end () ; ++it ) 00170 { 00171 // Ignore graph nodes which are already OFF 00172 if ( (*it)->state_ == Node::OFF ) 00173 continue; 00174 00175 // Set the node to ON 00176 (*it)->state_ = Node::ON; 00177 00178 // Set all its neighbors to OFF 00179 for ( typename std::set<Node*>::iterator neigh = (*it)->neighbors_.begin () ; neigh != (*it)->neighbors_.end () ; ++neigh ) 00180 { 00181 (*neigh)->state_ = Node::OFF; 00182 off_nodes.push_back (*neigh); 00183 } 00184 00185 // Output the node 00186 on_nodes.push_back (*it); 00187 } 00188 } 00189 00190 inline void 00191 insertUndirectedEdge (int id1, int id2) 00192 { 00193 nodes_[id1]->neighbors_.insert (nodes_[id2]); 00194 nodes_[id2]->neighbors_.insert (nodes_[id1]); 00195 } 00196 00197 inline void 00198 insertDirectedEdge (int id1, int id2) 00199 { 00200 nodes_[id1]->neighbors_.insert (nodes_[id2]); 00201 } 00202 00203 inline void 00204 deleteUndirectedEdge (int id1, int id2) 00205 { 00206 nodes_[id1]->neighbors_.erase (nodes_[id2]); 00207 nodes_[id2]->neighbors_.erase (nodes_[id1]); 00208 } 00209 00210 inline void 00211 deleteDirectedEdge (int id1, int id2) 00212 { 00213 nodes_[id1]->neighbors_.erase (nodes_[id2]); 00214 } 00215 00216 inline typename std::vector<Node*>& 00217 getNodes (){ return nodes_;} 00218 00219 public: 00220 typename std::vector<Node*> nodes_; 00221 }; 00222 } // namespace recognition 00223 } // namespace pcl 00224 00225 #endif /* PCL_RECOGNITION_ORR_GRAPH_H_ */