Point Cloud Library (PCL)  1.14.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  using Color = typename boost::property_traits<VertexColorMap>::value_type;
71  using Weight = typename boost::property_traits<EdgeWeightMap>::value_type;
72  using GraphTraits = boost::graph_traits<Graph>;
73  using EdgeDescriptor = typename GraphTraits::edge_descriptor;
74  using VertexDescriptor = typename GraphTraits::vertex_descriptor;
75  using EdgeIterator = typename GraphTraits::edge_iterator;
76  using OutEdgeIterator = typename GraphTraits::out_edge_iterator;
77  using VertexIterator = typename GraphTraits::vertex_iterator;
78  using VertexIndexMap = typename boost::property_map<Graph, boost::vertex_index_t>::type;
79  using VertexDegreeMap = boost::iterator_property_map<typename std::vector<Weight>::iterator, VertexIndexMap>;
80  using SparseMatrix = Eigen::SparseMatrix<Weight>;
81  using Matrix = Eigen::Matrix<Weight, Eigen::Dynamic, Eigen::Dynamic>;
82  using Vector = Eigen::Matrix<Weight, Eigen::Dynamic, 1>;
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  using T = Eigen::Triplet<float>;
121  using Triplets = std::vector<T>;
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  std::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  std::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  std::size_t num_equations = L_vertex_bimap.size ();
176  std::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.empty ())
180  L.setFromTriplets(L_triplets.begin(), L_triplets.end());
181  if (!B_triplets.empty ())
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 (Eigen::Index 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 (Eigen::Index i = 0; i < X.rows (); ++i)
214  {
215  std::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, std::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 (Eigen::Index 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 (std::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 (Eigen::Index 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 std::size_t
245  insertInBimap (boost::bimap<std::size_t, T>& bimap, T value)
246  {
247  if (bimap.right.count (value) != 0)
248  {
249  return bimap.right.at (value);
250  }
251  std::size_t s = bimap.size ();
252  bimap.insert (typename boost::bimap<std::size_t, T>::value_type (s, value));
253  return s;
254  }
255 
256  Graph& g_;
257  EdgeWeightMap weight_map_;
258  VertexColorMap color_map_;
260 
261  std::vector<VertexDescriptor> seeds_;
262  std::set<Color> colors_;
263 
264  std::vector<Weight> degree_storage_;
269 
270  // Map vertex identifiers to the rows/columns of L and vice versa
271  boost::bimap<std::size_t, VertexDescriptor> L_vertex_bimap;
272  // Map colors to the columns of B and vice versa
273  boost::bimap<std::size_t, Color> B_color_bimap;
274 
275  };
276 
277  }
278 
279  template <class Graph> bool
280  randomWalker (Graph& graph)
281  {
282  return randomWalker (graph,
283  boost::get (boost::edge_weight, graph),
284  boost::get (boost::vertex_color, graph));
285  }
286 
287  template <class Graph, class EdgeWeightMap, class VertexColorMap> bool
288  randomWalker (Graph& graph,
289  EdgeWeightMap weights,
290  VertexColorMap colors)
291  {
292  using namespace boost;
293 
294  using EdgeDescriptor = typename graph_traits<Graph>::edge_descriptor;
295  using VertexDescriptor = typename graph_traits<Graph>::vertex_descriptor;
296 
297  BOOST_CONCEPT_ASSERT ((VertexListGraphConcept<Graph>)); // to have vertices(), num_vertices()
298  BOOST_CONCEPT_ASSERT ((EdgeListGraphConcept<Graph>)); // to have edges()
299  BOOST_CONCEPT_ASSERT ((IncidenceGraphConcept<Graph>)); // to have source(), target() and out_edges()
300  BOOST_CONCEPT_ASSERT ((ReadablePropertyMapConcept<EdgeWeightMap, EdgeDescriptor>)); // read weight-values from edges
301  BOOST_CONCEPT_ASSERT ((ReadWritePropertyMapConcept<VertexColorMap, VertexDescriptor>)); // read and write color-values from vertices
302 
304  <
305  Graph,
306  EdgeWeightMap,
307  VertexColorMap
308  >
309  rw (graph, weights, colors);
310  return rw.segment ();
311  }
312 
313  template <class Graph, class EdgeWeightMap, class VertexColorMap> bool
314  randomWalker (Graph& graph,
315  EdgeWeightMap weights,
316  VertexColorMap colors,
317  Eigen::Matrix<typename boost::property_traits<EdgeWeightMap>::value_type, Eigen::Dynamic, Eigen::Dynamic>& potentials,
318  std::map<typename boost::property_traits<VertexColorMap>::value_type, std::size_t>& colors_to_columns_map)
319  {
320  using namespace boost;
321 
322  using EdgeDescriptor = typename graph_traits<Graph>::edge_descriptor;
323  using VertexDescriptor = typename graph_traits<Graph>::vertex_descriptor;
324 
325  BOOST_CONCEPT_ASSERT ((VertexListGraphConcept<Graph>)); // to have vertices(), num_vertices()
326  BOOST_CONCEPT_ASSERT ((EdgeListGraphConcept<Graph>)); // to have edges()
327  BOOST_CONCEPT_ASSERT ((IncidenceGraphConcept<Graph>)); // to have source(), target() and out_edges()
328  BOOST_CONCEPT_ASSERT ((ReadablePropertyMapConcept<EdgeWeightMap, EdgeDescriptor>)); // read weight-values from edges
329  BOOST_CONCEPT_ASSERT ((ReadWritePropertyMapConcept<VertexColorMap, VertexDescriptor>)); // read and write color-values from vertices
330 
332  <
333  Graph,
334  EdgeWeightMap,
335  VertexColorMap
336  >
337  rw (graph, weights, colors);
338  bool result = rw.segment ();
339  rw.getPotentials (potentials, colors_to_columns_map);
340  return result;
341  }
342 
343  }
344 
345 }
346 
347 #endif /* PCL_SEGMENTATION_IMPL_RANDOM_WALKER_HPP */
348 
Multilabel graph segmentation using random walks.
typename boost::property_map< Graph, boost::vertex_index_t >::type VertexIndexMap
boost::bimap< std::size_t, VertexDescriptor > L_vertex_bimap
typename boost::property_traits< VertexColorMap >::value_type Color
RandomWalker(Graph &g, EdgeWeightMap weights, VertexColorMap colors)
void getPotentials(Matrix &potentials, std::map< Color, std::size_t > &color_to_column_map)
std::vector< VertexDescriptor > seeds_
typename GraphTraits::out_edge_iterator OutEdgeIterator
Eigen::Matrix< Weight, Eigen::Dynamic, Eigen::Dynamic > Matrix
boost::graph_traits< Graph > GraphTraits
typename GraphTraits::edge_descriptor EdgeDescriptor
typename boost::property_traits< EdgeWeightMap >::value_type Weight
typename GraphTraits::vertex_descriptor VertexDescriptor
static std::size_t insertInBimap(boost::bimap< std::size_t, T > &bimap, T value)
typename GraphTraits::edge_iterator EdgeIterator
boost::iterator_property_map< typename std::vector< Weight >::iterator, VertexIndexMap > VertexDegreeMap
boost::bimap< std::size_t, Color > B_color_bimap
Eigen::Matrix< Weight, Eigen::Dynamic, 1 > Vector
Eigen::SparseMatrix< Weight > SparseMatrix
typename GraphTraits::vertex_iterator VertexIterator
bool randomWalker(Graph &graph)
Multilabel graph segmentation using random walks.