Point Cloud Library (PCL)  1.8.1-dev
random_walker.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2012-, Open Perception, 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 #ifndef PCL_SEGMENTATION_IMPL_RANDOM_WALKER_HPP
39 #define PCL_SEGMENTATION_IMPL_RANDOM_WALKER_HPP
40 
41 #include <boost/bimap.hpp>
42 
43 #include <Eigen/Sparse>
44 
45 namespace pcl
46 {
47 
48  namespace segmentation
49  {
50 
51  namespace detail
52  {
53 
54  /** \brief Multilabel graph segmentation using random walks.
55  *
56  * This is an implementation of the algorithm described in "Random Walks
57  * for Image Segmentation" by Leo Grady.
58  *
59  * See the documentation of the randomWalker() function for details.
60  *
61  * \author Sergey Alexandrov
62  * \ingroup segmentation
63  */
64  template <class Graph, class EdgeWeightMap, class VertexColorMap>
66  {
67 
68  public:
69 
70  typedef typename boost::property_traits<VertexColorMap>::value_type Color;
71  typedef typename boost::property_traits<EdgeWeightMap>::value_type Weight;
72  typedef boost::graph_traits<Graph> GraphTraits;
73  typedef typename GraphTraits::edge_descriptor EdgeDescriptor;
74  typedef typename GraphTraits::vertex_descriptor VertexDescriptor;
75  typedef typename GraphTraits::edge_iterator EdgeIterator;
76  typedef typename GraphTraits::out_edge_iterator OutEdgeIterator;
77  typedef typename GraphTraits::vertex_iterator VertexIterator;
78  typedef typename boost::property_map<Graph, boost::vertex_index_t>::type VertexIndexMap;
79  typedef boost::iterator_property_map<typename std::vector<Weight>::iterator, VertexIndexMap> VertexDegreeMap;
80  typedef Eigen::SparseMatrix<Weight> SparseMatrix;
81  typedef Eigen::Matrix<Weight, Eigen::Dynamic, Eigen::Dynamic> Matrix;
82  typedef Eigen::Matrix<Weight, Eigen::Dynamic, 1> Vector;
83 
84  RandomWalker (Graph& g, EdgeWeightMap weights, VertexColorMap colors)
85  : g_ (g)
86  , weight_map_ (weights)
87  , color_map_ (colors)
88  , index_map_ (boost::get (boost::vertex_index, g_))
89  , degree_storage_ (boost::num_vertices (g_), 0)
90  , degree_map_ (boost::make_iterator_property_map (degree_storage_.begin (), index_map_))
91  {
92  }
93 
94  bool
96  {
99  return solveLinearSystem ();
100  }
101 
102  void
104  {
105  using namespace boost;
106  EdgeIterator ei, e_end;
107  for (tie (ei, e_end) = edges (g_); ei != e_end; ++ei)
108  {
109  Weight w = weight_map_[*ei];
110  degree_map_[source (*ei, g_)] += w;
111  degree_map_[target (*ei, g_)] += w;
112  }
113  }
114 
115  void
117  {
118  using namespace boost;
119 
120  typedef Eigen::Triplet<float> T;
121  typedef std::vector<T> Triplets;
122  Triplets L_triplets;
123  Triplets B_triplets;
124 
125  VertexIterator vi, v_end;
126  for (tie (vi, v_end) = vertices (g_); vi != v_end; ++vi)
127  {
128  // If this is a labeled vertex add it to the seeds list and register its color
129  if (color_map_[*vi])
130  {
131  seeds_.push_back (*vi);
132  colors_.insert (color_map_[*vi]);
133  }
134  // Skip seeds and vertices with zero connectivity
135  if (color_map_[*vi] || std::fabs (degree_map_[*vi]) < std::numeric_limits<Weight>::epsilon ())
136  continue;
137  // Create a row in L matrix for the vertex
138  size_t current_row = insertInBimap (L_vertex_bimap, *vi);
139  // Add diagonal degree entry for the vertex
140  L_triplets.push_back (T (current_row, current_row, degree_map_[*vi]));
141  // Iterate over incident vertices and add entries on corresponding columns of L or B
142  OutEdgeIterator ei, e_end;
143  for (tie (ei, e_end) = out_edges (*vi, g_); ei != e_end; ++ei)
144  {
145  Weight w = weight_map_[*ei];
146  VertexDescriptor tgt = target (*ei, g_);
147  Color color = color_map_[tgt];
148  if (color)
149  {
150  // This is a seed and will go to B matrix
151  size_t column;
152  if (B_color_bimap.right.count (color) == 0)
153  {
154  // This is the first time we encountered this color, create a new column in B
155  column = insertInBimap (B_color_bimap, color);
156  }
157  else
158  {
159  column = B_color_bimap.right.at (color);
160  }
161  B_triplets.push_back (T (current_row, column, w));
162  }
163  else
164  {
165  // This is a non-seed and will go to L matrix,
166  // but only if a row for this vertex already exists
167  if (L_vertex_bimap.right.count (tgt) && L_vertex_bimap.right.at (tgt) != current_row)
168  {
169  L_triplets.push_back (T (current_row, L_vertex_bimap.right.at (tgt), -w));
170  }
171  }
172  }
173  }
174 
175  size_t num_equations = L_vertex_bimap.size ();
176  size_t num_colors = B_color_bimap.size ();
177  L.resize (num_equations, num_equations);
178  B.resize (num_equations, num_colors);
179  if (L_triplets.size ())
180  L.setFromTriplets(L_triplets.begin(), L_triplets.end());
181  if (B_triplets.size ())
182  B.setFromTriplets(B_triplets.begin(), B_triplets.end());
183  }
184 
186  {
187  X.resize (L.rows (), B.cols ());
188 
189  // Nothing to solve
190  if (L.rows () == 0 || B.cols () == 0)
191  return true;
192 
193  Eigen::SimplicialCholesky<SparseMatrix, Eigen::Lower> cg;
194  cg.compute (L);
195  bool succeeded = true;
196  for (int i = 0; i < B.cols (); ++i)
197  {
198  Vector b = B.col (i);
199  X.col (i) = cg.solve (b);
200  if (cg.info () != Eigen::Success)
201  succeeded = false;
202  }
203 
204  assignColors ();
205  return succeeded;
206  }
207 
208  void
210  {
211  using namespace boost;
212  if (X.cols ())
213  for (int i = 0; i < X.rows (); ++i)
214  {
215  size_t max_column;
216  X.row (i).maxCoeff (&max_column);
217  VertexDescriptor vertex = L_vertex_bimap.left.at (i);
218  Color color = B_color_bimap.left.at (max_column);
219  color_map_[vertex] = color;
220  }
221  }
222 
223  void
224  getPotentials (Matrix& potentials, std::map<Color, size_t>& color_to_column_map)
225  {
226  using namespace boost;
227  potentials = Matrix::Zero (num_vertices (g_), colors_.size ());
228  // Copy over rows from X
229  for (int i = 0; i < X.rows (); ++i)
230  potentials.row (L_vertex_bimap.left.at (i)).head (X.cols ()) = X.row (i);
231  // In rows that correspond to seeds put ones in proper columns
232  for (size_t i = 0; i < seeds_.size (); ++i)
233  {
234  VertexDescriptor v = seeds_[i];
236  potentials (seeds_[i], B_color_bimap.right.at (color_map_[seeds_[i]])) = 1;
237  }
238  // Fill in a map that associates colors with columns in potentials matrix
239  color_to_column_map.clear ();
240  for (int i = 0; i < potentials.cols (); ++i)
241  color_to_column_map[B_color_bimap.left.at (i)] = i;
242  }
243 
244  template <typename T> static inline size_t
245  insertInBimap (boost::bimap<size_t, T>& bimap, T value)
246  {
247  if (bimap.right.count (value) != 0)
248  {
249  return bimap.right.at (value);
250  }
251  else
252  {
253  size_t s = bimap.size ();
254  bimap.insert (typename boost::bimap<size_t, T>::value_type (s, value));
255  return s;
256  }
257  }
258 
259  Graph& g_;
260  EdgeWeightMap weight_map_;
261  VertexColorMap color_map_;
263 
264  std::vector<VertexDescriptor> seeds_;
265  std::set<Color> colors_;
266 
267  std::vector<Weight> degree_storage_;
272 
273  // Map vertex identifiers to the rows/columns of L and vice versa
274  boost::bimap<size_t, VertexDescriptor> L_vertex_bimap;
275  // Map colors to the columns of B and vice versa
276  boost::bimap<size_t, Color> B_color_bimap;
277 
278  };
279 
280  }
281 
282  template <class Graph> bool
283  randomWalker (Graph& graph)
284  {
285  return randomWalker (graph,
286  boost::get (boost::edge_weight, graph),
287  boost::get (boost::vertex_color, graph));
288  }
289 
290  template <class Graph, class EdgeWeightMap, class VertexColorMap> bool
291  randomWalker (Graph& graph,
292  EdgeWeightMap weights,
293  VertexColorMap colors)
294  {
295  using namespace boost;
296 
297  typedef typename graph_traits<Graph>::edge_descriptor EdgeDescriptor;
298  typedef typename graph_traits<Graph>::vertex_descriptor VertexDescriptor;
299 
300  BOOST_CONCEPT_ASSERT ((VertexListGraphConcept<Graph>)); // to have vertices(), num_vertices()
301  BOOST_CONCEPT_ASSERT ((EdgeListGraphConcept<Graph>)); // to have edges()
302  BOOST_CONCEPT_ASSERT ((IncidenceGraphConcept<Graph>)); // to have source(), target() and out_edges()
303  BOOST_CONCEPT_ASSERT ((ReadablePropertyMapConcept<EdgeWeightMap, EdgeDescriptor>)); // read weight-values from edges
304  BOOST_CONCEPT_ASSERT ((ReadWritePropertyMapConcept<VertexColorMap, VertexDescriptor>)); // read and write color-values from vertices
305 
307  <
308  Graph,
309  EdgeWeightMap,
310  VertexColorMap
311  >
312  rw (graph, weights, colors);
313  return rw.segment ();
314  }
315 
316  template <class Graph, class EdgeWeightMap, class VertexColorMap> bool
317  randomWalker (Graph& graph,
318  EdgeWeightMap weights,
319  VertexColorMap colors,
320  Eigen::Matrix<typename boost::property_traits<EdgeWeightMap>::value_type, Eigen::Dynamic, Eigen::Dynamic>& potentials,
321  std::map<typename boost::property_traits<VertexColorMap>::value_type, size_t>& colors_to_columns_map)
322  {
323  using namespace boost;
324 
325  typedef typename graph_traits<Graph>::edge_descriptor EdgeDescriptor;
326  typedef typename graph_traits<Graph>::vertex_descriptor VertexDescriptor;
327 
328  BOOST_CONCEPT_ASSERT ((VertexListGraphConcept<Graph>)); // to have vertices(), num_vertices()
329  BOOST_CONCEPT_ASSERT ((EdgeListGraphConcept<Graph>)); // to have edges()
330  BOOST_CONCEPT_ASSERT ((IncidenceGraphConcept<Graph>)); // to have source(), target() and out_edges()
331  BOOST_CONCEPT_ASSERT ((ReadablePropertyMapConcept<EdgeWeightMap, EdgeDescriptor>)); // read weight-values from edges
332  BOOST_CONCEPT_ASSERT ((ReadWritePropertyMapConcept<VertexColorMap, VertexDescriptor>)); // read and write color-values from vertices
333 
335  <
336  Graph,
337  EdgeWeightMap,
338  VertexColorMap
339  >
340  rw (graph, weights, colors);
341  bool result = rw.segment ();
342  rw.getPotentials (potentials, colors_to_columns_map);
343  return result;
344  }
345 
346  }
347 
348 }
349 
350 #endif /* PCL_SEGMENTATION_IMPL_RANDOM_WALKER_HPP */
351 
boost::property_traits< VertexColorMap >::value_type Color
Eigen::Matrix< Weight, Eigen::Dynamic, Eigen::Dynamic > Matrix
Multilabel graph segmentation using random walks.
boost::iterator_property_map< typename std::vector< Weight >::iterator, VertexIndexMap > VertexDegreeMap
RandomWalker(Graph &g, EdgeWeightMap weights, VertexColorMap colors)
GraphTraits::vertex_iterator VertexIterator
boost::bimap< size_t, Color > B_color_bimap
static size_t insertInBimap(boost::bimap< size_t, T > &bimap, T value)
Eigen::Matrix< Weight, Eigen::Dynamic, 1 > Vector
GraphTraits::vertex_descriptor VertexDescriptor
bool randomWalker(Graph &graph)
Multilabel graph segmentation using random walks.
GraphTraits::out_edge_iterator OutEdgeIterator
boost::graph_traits< Graph > GraphTraits
std::vector< VertexDescriptor > seeds_
boost::property_map< Graph, boost::vertex_index_t >::type VertexIndexMap
GraphTraits::edge_iterator EdgeIterator
boost::property_traits< EdgeWeightMap >::value_type Weight
GraphTraits::edge_descriptor EdgeDescriptor
Eigen::SparseMatrix< Weight > SparseMatrix
void getPotentials(Matrix &potentials, std::map< Color, size_t > &color_to_column_map)
boost::bimap< size_t, VertexDescriptor > L_vertex_bimap