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 #include <pcl/features/normal_3d.h>
47 #include <pcl/point_cloud.h>
48 
49 #include "common.h"
50 #include "point_cloud_graph.h"
52 
53 template <typename Graph>
54 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;
76  ++vi2)
77  {
78  neighbors.push_back(*vi2);
79  }
80  }
81 
82  Eigen::Vector4f normal;
83  float curvature;
84  pcl::computePointNormal(*cloud, neighbors, normal, curvature);
85  normal[3] = 0;
86  normal.normalize();
87 
88  // Re-orient normals. If the graph already has normals, then make sure that
89  // the new normals are codirectional with them. Otherwise make sure that they
90  // point towards origin.
91  if (std::isfinite(graph[vertex].data_n[0]) && std::isfinite(graph[vertex].data_n[1]) &&
92  std::isfinite(graph[vertex].data_n[2]) && std::isfinite(graph[vertex].data_n[3]) &&
93  graph[vertex].getNormalVector4fMap().any())
94  {
95  if (graph[vertex].getNormalVector4fMap().dot(normal) < 0)
96  {
97  normal *= -1;
98  }
99  }
100  else
101  {
102  pcl::flipNormalTowardsViewpoint(graph[vertex], 0.0f, 0.0f, 0.0f, normal);
103  }
104  graph[vertex].getNormalVector4fMap() = normal;
105  graph[vertex].curvature = std::isnan(curvature) ? 0.0f : curvature;
106  }
107 }
108 
109 template <typename Graph>
110 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>
142 size_t
144  Graph& graph,
145  std::vector<boost::reference_wrapper<Graph>>& subgraphs)
146 {
147  typedef typename Graph::vertex_descriptor VertexId;
148  typedef typename boost::reference_wrapper<Graph> GraphRef;
149 
150  subgraphs.clear();
151  std::vector<int> component(boost::num_vertices(graph));
152  size_t num_components = boost::connected_components(graph, &component[0]);
153  for (size_t i = 0; i < num_components; ++i)
154  {
155  subgraphs.push_back(GraphRef(graph.create_subgraph()));
156  }
157  for (VertexId v = 0; v < boost::num_vertices(graph); ++v)
158  {
159  boost::add_vertex(graph.local_to_global(v), subgraphs.at(component[v]).get());
160  }
161  return num_components;
162 }
163 
164 template <typename Graph, typename ColorMap>
165 size_t
167  ColorMap color_map,
168  std::vector<boost::reference_wrapper<Graph>>& subgraphs)
169 {
170  typedef typename Graph::vertex_descriptor VertexId;
171  typedef typename boost::reference_wrapper<Graph> GraphRef;
172  typedef typename boost::property_traits<ColorMap>::value_type Color;
173  typedef std::map<Color, GraphRef> SubgraphMap;
174  typedef typename SubgraphMap::iterator SubgraphMapIterator;
175 
176  SubgraphMap subgraph_map;
177  for (VertexId v = 0; v < boost::num_vertices(graph); ++v)
178  {
179  SubgraphMapIterator itr = subgraph_map.find(color_map[v]);
180  if (itr == subgraph_map.end())
181  {
182  GraphRef subgraph(graph.create_subgraph());
183  boost::add_vertex(graph.local_to_global(v), subgraph.get());
184  subgraph_map.insert(std::make_pair(color_map[v], subgraph));
185  }
186  else
187  {
188  boost::add_vertex(graph.local_to_global(v), itr->second.get());
189  }
190  }
191 
192  subgraphs.clear();
193  for (SubgraphMapIterator itr = subgraph_map.begin(); itr != subgraph_map.end(); ++itr)
194  {
195  subgraphs.push_back(itr->second);
196  }
197 
198  return subgraphs.size();
199 }
200 
201 template <typename Graph>
202 void
204  const pcl::PointIndices& indices,
205  std::vector<boost::reference_wrapper<Graph>>& subgraphs)
206 {
207  typedef typename Graph::vertex_descriptor VertexId;
208  typedef typename boost::reference_wrapper<Graph> GraphRef;
209 
210  std::set<int> index_set;
211 
212  subgraphs.clear();
213  subgraphs.push_back(GraphRef(graph.create_subgraph()));
214  subgraphs.push_back(GraphRef(graph.create_subgraph()));
215  Graph& first = subgraphs.at(0).get();
216  Graph& second = subgraphs.at(1).get();
217 
218  for (size_t i = 0; i < indices.indices.size(); ++i)
219  {
220  index_set.insert(indices.indices[i]);
221  boost::add_vertex(indices.indices[i], first);
222  }
223 
224  for (VertexId v = 0; v < boost::num_vertices(graph); ++v)
225  if (!index_set.count(v))
226  {
227  boost::add_vertex(v, second);
228  }
229 }
230 
231 template <typename Graph>
232 void
234  const std::vector<pcl::PointIndices>& indices,
235  std::vector<boost::reference_wrapper<Graph>>& subgraphs)
236 {
237  typedef typename Graph::vertex_descriptor VertexId;
238  typedef typename boost::reference_wrapper<Graph> GraphRef;
239 
240  std::set<int> index_set;
241 
242  subgraphs.clear();
243  for (size_t i = 0; i < indices.size(); ++i)
244  {
245  subgraphs.push_back(GraphRef(graph.create_subgraph()));
246  Graph& s = subgraphs.back().get();
247  for (size_t j = 0; j < indices[i].indices.size(); ++j)
248  {
249  index_set.insert(indices[i].indices[j]);
250  boost::add_vertex(indices[i].indices[j], s);
251  }
252  }
253 
254  subgraphs.push_back(GraphRef(graph.create_subgraph()));
255  Graph& s = subgraphs.back().get();
256  for (VertexId v = 0; v < boost::num_vertices(graph); ++v)
257  if (!index_set.count(v))
258  {
259  boost::add_vertex(v, s);
260  }
261 }
262 
263 template <typename Graph>
264 void
265 pcl::graph::smoothen(Graph& graph, float spatial_sigma, float influence_sigma)
266 {
267  BOOST_CONCEPT_ASSERT((pcl::graph::PointCloudGraphConcept<Graph>));
268 
269  typedef typename Graph::edge_iterator EdgeIterator;
270  typedef typename Graph::vertex_descriptor VertexId;
271 
272  std::vector<float> K(boost::num_vertices(graph), 0);
273  Eigen::MatrixXf P(boost::num_vertices(graph), 3);
274  P.setZero();
275 
276  EdgeIterator ei, ee;
277  for (boost::tie(ei, ee) = boost::edges(graph); ei != ee; ++ei)
278  {
279  const VertexId& src = boost::source(*ei, graph);
280  const VertexId& tgt = boost::target(*ei, graph);
281  const Eigen::Vector3f& p = graph[src].getVector3fMap();
282  const Eigen::Vector3f& q = graph[tgt].getVector3fMap();
283  const Eigen::Vector3f& np = graph[src].getNormalVector3fMap();
284  const Eigen::Vector3f& nq = graph[tgt].getNormalVector3fMap();
285  Eigen::Vector3f x = p - q;
286  float d1 = nq.dot(x);
287  float d2 = np.dot(-x);
288  float ws = std::exp(-std::pow(x.norm(), 2) / (2 * std::pow(spatial_sigma, 2)));
289  float wi1 = std::exp(-std::pow(d1, 2) / (2 * std::pow(influence_sigma, 2)));
290  float wi2 = std::exp(-std::pow(d2, 2) / (2 * std::pow(influence_sigma, 2)));
291  float w1 = ws * wi1;
292  float w2 = ws * wi2;
293  K[src] += w1;
294  K[tgt] += w2;
295  P.row(src) += nq * d1 * w1;
296  P.row(tgt) += np * d2 * w2;
297  }
298 
299  for (VertexId v = 0; v < boost::num_vertices(graph); ++v)
300  if (K[v] > 0.01)
301  {
302  graph[v].getVector3fMap() -= P.row(v) / K[v];
303  }
304 }
305 
306 #endif /* PCL_GRAPH_IMPL_COMMON_CPP */
armarx::AdjacencyIterator
boost::graph_traits< Graph >::adjacency_iterator AdjacencyIterator
Definition: Common.h:73
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:265
armarx::GraphRef
boost::reference_wrapper< Graph > GraphRef
Definition: Common.h:63
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:68
boost::target
Vertex target(const detail::edge_base< Directed, Vertex > &e, const PCG &)
Definition: point_cloud_graph.h:668
c
constexpr T c
Definition: UnscentedKalmanFilterTest.cpp:46
PointCloudGraphConcept
std::isfinite
bool isfinite(const std::vector< T, Ts... > &v)
Definition: algorithm.h:366
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:717
point_cloud_graph_concept.h
armarx::PointT
pcl::PointXYZRGBL PointT
Definition: Common.h:30
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:690
armarx::Graph
boost::subgraph< CloudGraph > Graph
Definition: Common.h:58
boost::source
Vertex source(const detail::edge_base< Directed, Vertex > &e, const PCG &)
Definition: point_cloud_graph.h:661
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:544
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:203
q
#define q
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:166
armarx::ctrlutil::v
double v(double t, double v0, double a0, double j)
Definition: CtrlUtil.h:39
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:143
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:57
ColorMap
Definition: color.h:10
armarx::EdgeIterator
boost::graph_traits< Graph >::edge_iterator EdgeIterator
Definition: Common.h:72
point_cloud_graph.h
armarx::ctrlutil::s
double s(double t, double s0, double v0, double a0, double j)
Definition: CtrlUtil.h:33