common.hpp
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2014-, 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 the copyright holder(s) 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_GRAPH_IMPL_COMMON_CPP
39 #define PCL_GRAPH_IMPL_COMMON_CPP
40 
41 #include <set>
42 
43 #include <boost/concept_check.hpp>
44 #include <boost/graph/connected_components.hpp>
45 
46 
47 #include <pcl/point_cloud.h>
48 #include <pcl/features/normal_3d.h>
49 
50 #include "common.h"
51 #include "point_cloud_graph.h"
53 
54 template <typename Graph> void
55 pcl::graph::computeNormalsAndCurvatures(Graph& graph, bool neighborhood_1ring)
56 {
57  BOOST_CONCEPT_ASSERT((pcl::graph::PointCloudGraphConcept<Graph>));
58 
60  typedef typename Graph::adjacency_iterator AdjacencyIterator;
61  typedef typename Graph::vertex_descriptor VertexId;
62 
63  typename pcl::PointCloud<PointT>::ConstPtr cloud = pcl::graph::point_cloud(graph);
64 
65  for (VertexId vertex = 0; vertex < boost::num_vertices(graph); ++vertex)
66  {
67  std::vector<int> neighbors(1, vertex);
68  neighbors.reserve(256);
69 
70  AdjacencyIterator vi1, ve1, vi2, ve2;
71  for (boost::tie(vi1, ve1) = boost::adjacent_vertices(vertex, graph); vi1 != ve1; ++vi1)
72  {
73  neighbors.push_back(*vi1);
74  if (!neighborhood_1ring)
75  for (boost::tie(vi2, ve2) = boost::adjacent_vertices(*vi1, graph); vi2 != ve2; ++vi2)
76  {
77  neighbors.push_back(*vi2);
78  }
79  }
80 
81  Eigen::Vector4f normal;
82  float curvature;
83  pcl::computePointNormal(*cloud, neighbors, normal, curvature);
84  normal[3] = 0;
85  normal.normalize();
86 
87  // Re-orient normals. If the graph already has normals, then make sure that
88  // the new normals are codirectional with them. Otherwise make sure that they
89  // point towards origin.
90  if (std::isfinite(graph[vertex].data_n[0]) &&
91  std::isfinite(graph[vertex].data_n[1]) &&
92  std::isfinite(graph[vertex].data_n[2]) &&
93  std::isfinite(graph[vertex].data_n[3]) &&
94  graph[vertex].getNormalVector4fMap().any())
95  {
96  if (graph[vertex].getNormalVector4fMap().dot(normal) < 0)
97  {
98  normal *= -1;
99  }
100  }
101  else
102  {
103  pcl::flipNormalTowardsViewpoint(graph[vertex], 0.0f, 0.0f, 0.0f, normal);
104  }
105  graph[vertex].getNormalVector4fMap() = normal;
106  graph[vertex].curvature = std::isnan(curvature) ? 0.0f : curvature;
107  }
108 }
109 
110 template <typename Graph> void
112 {
113  BOOST_CONCEPT_ASSERT((pcl::graph::PointCloudGraphConcept<Graph>));
114 
116  typedef typename Graph::edge_iterator EdgeIterator;
117  typedef typename Graph::vertex_descriptor VertexId;
118 
119  EdgeIterator ei, ee;
120  std::vector<float> convexities(boost::num_vertices(graph), 0.0f);
121  for (boost::tie(ei, ee) = boost::edges(graph); ei != ee; ++ei)
122  {
123  VertexId v1 = boost::source(*ei, graph);
124  VertexId v2 = boost::target(*ei, graph);
125  const PointT& p1 = graph[v1];
126  const PointT& p2 = graph[v2];
127  const Eigen::Vector3f& d = p2.getVector3fMap() - p1.getVector3fMap();
128  const Eigen::Vector3f& n1 = p1.getNormalVector3fMap();
129  const Eigen::Vector3f& n2 = p2.getNormalVector3fMap();
130  float c = (((d - d.dot(n1) * n1).dot(n2) > 0 ? 1.0 : -1.0) * (n1 - n2).squaredNorm());
131  convexities[v1] += c;
132  convexities[v2] += c;
133  }
134 
135  for (VertexId vertex = 0; vertex < boost::num_vertices(graph); ++vertex)
136  {
137  graph[vertex].curvature = copysign(graph[vertex].curvature, convexities[vertex]);
138  }
139 }
140 
141 template <typename Graph> size_t
143  std::vector<boost::reference_wrapper<Graph> >& subgraphs)
144 {
145  typedef typename Graph::vertex_descriptor VertexId;
146  typedef typename boost::reference_wrapper<Graph> GraphRef;
147 
148  subgraphs.clear();
149  std::vector<int> component(boost::num_vertices(graph));
150  size_t num_components = boost::connected_components(graph, &component[0]);
151  for (size_t i = 0; i < num_components; ++i)
152  {
153  subgraphs.push_back(GraphRef(graph.create_subgraph()));
154  }
155  for (VertexId v = 0; v < boost::num_vertices(graph); ++v)
156  {
157  boost::add_vertex(graph.local_to_global(v), subgraphs.at(component[v]).get());
158  }
159  return num_components;
160 }
161 
162 template <typename Graph, typename ColorMap> size_t
164  ColorMap color_map,
165  std::vector<boost::reference_wrapper<Graph> >& subgraphs)
166 {
167  typedef typename Graph::vertex_descriptor VertexId;
168  typedef typename boost::reference_wrapper<Graph> GraphRef;
169  typedef typename boost::property_traits<ColorMap>::value_type Color;
170  typedef std::map<Color, GraphRef> SubgraphMap;
171  typedef typename SubgraphMap::iterator SubgraphMapIterator;
172 
173  SubgraphMap subgraph_map;
174  for (VertexId v = 0; v < boost::num_vertices(graph); ++v)
175  {
176  SubgraphMapIterator itr = subgraph_map.find(color_map[v]);
177  if (itr == subgraph_map.end())
178  {
179  GraphRef subgraph(graph.create_subgraph());
180  boost::add_vertex(graph.local_to_global(v), subgraph.get());
181  subgraph_map.insert(std::make_pair(color_map[v], subgraph));
182  }
183  else
184  {
185  boost::add_vertex(graph.local_to_global(v), itr->second.get());
186  }
187  }
188 
189  subgraphs.clear();
190  for (SubgraphMapIterator itr = subgraph_map.begin(); itr != subgraph_map.end(); ++itr)
191  {
192  subgraphs.push_back(itr->second);
193  }
194 
195  return subgraphs.size();
196 }
197 
198 template <typename Graph> void
200  const pcl::PointIndices& indices,
201  std::vector<boost::reference_wrapper<Graph> >& subgraphs)
202 {
203  typedef typename Graph::vertex_descriptor VertexId;
204  typedef typename boost::reference_wrapper<Graph> GraphRef;
205 
206  std::set<int> index_set;
207 
208  subgraphs.clear();
209  subgraphs.push_back(GraphRef(graph.create_subgraph()));
210  subgraphs.push_back(GraphRef(graph.create_subgraph()));
211  Graph& first = subgraphs.at(0).get();
212  Graph& second = subgraphs.at(1).get();
213 
214  for (size_t i = 0; i < indices.indices.size(); ++i)
215  {
216  index_set.insert(indices.indices[i]);
217  boost::add_vertex(indices.indices[i], first);
218  }
219 
220  for (VertexId v = 0; v < boost::num_vertices(graph); ++v)
221  if (!index_set.count(v))
222  {
223  boost::add_vertex(v, second);
224  }
225 }
226 
227 template <typename Graph> void
229  const std::vector<pcl::PointIndices>& indices,
230  std::vector<boost::reference_wrapper<Graph> >& subgraphs)
231 {
232  typedef typename Graph::vertex_descriptor VertexId;
233  typedef typename boost::reference_wrapper<Graph> GraphRef;
234 
235  std::set<int> index_set;
236 
237  subgraphs.clear();
238  for (size_t i = 0; i < indices.size(); ++i)
239  {
240  subgraphs.push_back(GraphRef(graph.create_subgraph()));
241  Graph& s = subgraphs.back().get();
242  for (size_t j = 0; j < indices[i].indices.size(); ++j)
243  {
244  index_set.insert(indices[i].indices[j]);
245  boost::add_vertex(indices[i].indices[j], s);
246  }
247  }
248 
249  subgraphs.push_back(GraphRef(graph.create_subgraph()));
250  Graph& s = subgraphs.back().get();
251  for (VertexId v = 0; v < boost::num_vertices(graph); ++v)
252  if (!index_set.count(v))
253  {
254  boost::add_vertex(v, s);
255  }
256 }
257 
258 template <typename Graph> void
259 pcl::graph::smoothen(Graph& graph, float spatial_sigma, float influence_sigma)
260 {
261  BOOST_CONCEPT_ASSERT((pcl::graph::PointCloudGraphConcept<Graph>));
262 
263  typedef typename Graph::edge_iterator EdgeIterator;
264  typedef typename Graph::vertex_descriptor VertexId;
265 
266  std::vector<float> K(boost::num_vertices(graph), 0);
267  Eigen::MatrixXf P(boost::num_vertices(graph), 3);
268  P.setZero();
269 
270  EdgeIterator ei, ee;
271  for (boost::tie(ei, ee) = boost::edges(graph); ei != ee; ++ei)
272  {
273  const VertexId& src = boost::source(*ei, graph);
274  const VertexId& tgt = boost::target(*ei, graph);
275  const Eigen::Vector3f& p = graph[src].getVector3fMap();
276  const Eigen::Vector3f& q = graph[tgt].getVector3fMap();
277  const Eigen::Vector3f& np = graph[src].getNormalVector3fMap();
278  const Eigen::Vector3f& nq = graph[tgt].getNormalVector3fMap();
279  Eigen::Vector3f x = p - q;
280  float d1 = nq.dot(x);
281  float d2 = np.dot(-x);
282  float ws = std::exp(- std::pow(x.norm(), 2) / (2 * std::pow(spatial_sigma, 2)));
283  float wi1 = std::exp(- std::pow(d1, 2) / (2 * std::pow(influence_sigma, 2)));
284  float wi2 = std::exp(- std::pow(d2, 2) / (2 * std::pow(influence_sigma, 2)));
285  float w1 = ws * wi1;
286  float w2 = ws * wi2;
287  K[src] += w1;
288  K[tgt] += w2;
289  P.row(src) += nq * d1 * w1;
290  P.row(tgt) += np * d2 * w2;
291  }
292 
293  for (VertexId v = 0; v < boost::num_vertices(graph); ++v)
294  if (K[v] > 0.01)
295  {
296  graph[v].getVector3fMap() -= P.row(v) / K[v];
297  }
298 }
299 
300 #endif /* PCL_GRAPH_IMPL_COMMON_CPP */
armarx::AdjacencyIterator
boost::graph_traits< Graph >::adjacency_iterator AdjacencyIterator
Definition: Common.h:69
pcl::graph::createSubgraphsFromColorMap
size_t createSubgraphsFromColorMap(Graph &graph, ColorMap color_map, std::vector< boost::reference_wrapper< Graph > > &subgraphs)
Split a given graph into subgraphs based on the values in a given vertex color map.
Definition: common.hpp:163
pcl::graph::smoothen
void smoothen(Graph &graph, float spatial_sigma, float influence_sigma)
Apply bilateral filtering to a given point cloud graph.
Definition: common.hpp:259
armarx::GraphRef
boost::reference_wrapper< Graph > GraphRef
Definition: Common.h:59
pcl::graph::computeNormalsAndCurvatures
void computeNormalsAndCurvatures(Graph &graph, bool neighborhood_1ring=false)
Compute normals and curvatures for all vertices in a graph.
Definition: common.hpp:55
armarx::VertexId
boost::graph_traits< Graph >::vertex_descriptor VertexId
Definition: Common.h:64
boost::target
Vertex target(const detail::edge_base< Directed, Vertex > &e, const PCG &)
Definition: point_cloud_graph.h:688
c
constexpr T c
Definition: UnscentedKalmanFilterTest.cpp:43
PointCloudGraphConcept
pcl::graph::createSubgraphsFromConnectedComponents
size_t createSubgraphsFromConnectedComponents(Graph &graph, std::vector< boost::reference_wrapper< Graph > > &subgraphs)
Find connected components in a graph and create a subgraph for each of them.
Definition: common.hpp:142
std::isfinite
bool isfinite(const std::vector< T, Ts... > &v)
Definition: algorithm.h:327
Color
uint32_t Color
RGBA color.
Definition: color.h:8
pcl::graph::indices
pcl::PointIndices::Ptr indices(const PCG &g)
Retrieve the indices of the points of the point cloud stored in a point cloud graph that actually bel...
Definition: point_cloud_graph.h:737
point_cloud_graph_concept.h
armarx::PointT
pcl::PointXYZRGBL PointT
Definition: Common.h:28
common.h
pcl::graph::point_cloud
pcl::PointCloud< P >::Ptr point_cloud(PCG &g)
Retrieve the point cloud stored in a point cloud graph.
Definition: point_cloud_graph.h:710
armarx::Graph
boost::subgraph< CloudGraph > Graph
Definition: Common.h:54
boost::source
Vertex source(const detail::edge_base< Directed, Vertex > &e, const PCG &)
Definition: point_cloud_graph.h:681
pcl::graph::point_cloud_graph_traits::point_type
Graph::point_type point_type
The type of PCL points bundled in vertices.
Definition: point_cloud_graph.h:560
q
#define q
armarx::ctrlutil::v
double v(double t, double v0, double a0, double j)
Definition: CtrlUtil.h:39
pcl::graph::computeSignedCurvatures
void computeSignedCurvatures(Graph &graph)
Compute the type of curvature (concave/convex) for each vertex.
Definition: common.hpp:111
dot
double dot(const Point &x, const Point &y)
Definition: point.hpp:53
ColorMap
Definition: color.h:10
armarx::EdgeIterator
boost::graph_traits< Graph >::edge_iterator EdgeIterator
Definition: Common.h:68
pcl::graph::createSubgraphsFromIndices
void createSubgraphsFromIndices(Graph &graph, const pcl::PointIndices &indices, std::vector< boost::reference_wrapper< Graph > > &subgraphs)
Create two subgraphs of a given graph, one containing the points with the given indices,...
Definition: common.hpp:199
point_cloud_graph.h
armarx::ctrlutil::s
double s(double t, double s0, double v0, double a0, double j)
Definition: CtrlUtil.h:33