38 #ifndef PCL_GRAPH_IMPL_COMMON_CPP
39 #define PCL_GRAPH_IMPL_COMMON_CPP
43 #include <boost/concept_check.hpp>
44 #include <boost/graph/connected_components.hpp>
46 #include <pcl/features/normal_3d.h>
47 #include <pcl/point_cloud.h>
53 template <
typename Graph>
61 typedef typename Graph::vertex_descriptor
VertexId;
65 for (
VertexId vertex = 0; vertex < boost::num_vertices(graph); ++vertex)
67 std::vector<int> neighbors(1, vertex);
68 neighbors.reserve(256);
71 for (boost::tie(vi1, ve1) = boost::adjacent_vertices(vertex, graph); vi1 != ve1; ++vi1)
73 neighbors.push_back(*vi1);
74 if (!neighborhood_1ring)
75 for (boost::tie(vi2, ve2) = boost::adjacent_vertices(*vi1, graph); vi2 != ve2;
78 neighbors.push_back(*vi2);
82 Eigen::Vector4f normal;
84 pcl::computePointNormal(*cloud, neighbors, normal, curvature);
93 graph[vertex].getNormalVector4fMap().any())
95 if (graph[vertex].getNormalVector4fMap().
dot(normal) < 0)
102 pcl::flipNormalTowardsViewpoint(graph[vertex], 0.0f, 0.0f, 0.0f, normal);
104 graph[vertex].getNormalVector4fMap() = normal;
105 graph[vertex].curvature = std::isnan(curvature) ? 0.0f : curvature;
109 template <
typename Graph>
117 typedef typename Graph::vertex_descriptor
VertexId;
120 std::vector<float> convexities(boost::num_vertices(graph), 0.0f);
121 for (boost::tie(ei, ee) = boost::edges(graph); ei != ee; ++ei)
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;
135 for (
VertexId vertex = 0; vertex < boost::num_vertices(graph); ++vertex)
137 graph[vertex].curvature = copysign(graph[vertex].curvature, convexities[vertex]);
141 template <
typename Graph>
145 std::vector<boost::reference_wrapper<Graph>>& subgraphs)
147 typedef typename Graph::vertex_descriptor
VertexId;
148 typedef typename boost::reference_wrapper<Graph>
GraphRef;
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)
155 subgraphs.push_back(
GraphRef(graph.create_subgraph()));
157 for (
VertexId v = 0;
v < boost::num_vertices(graph); ++
v)
159 boost::add_vertex(graph.local_to_global(
v), subgraphs.at(component[
v]).get());
161 return num_components;
164 template <
typename Graph,
typename ColorMap>
168 std::vector<boost::reference_wrapper<Graph>>& subgraphs)
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;
176 SubgraphMap subgraph_map;
177 for (
VertexId v = 0;
v < boost::num_vertices(graph); ++
v)
179 SubgraphMapIterator itr = subgraph_map.find(color_map[
v]);
180 if (itr == subgraph_map.end())
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));
188 boost::add_vertex(graph.local_to_global(
v), itr->second.get());
193 for (SubgraphMapIterator itr = subgraph_map.begin(); itr != subgraph_map.end(); ++itr)
195 subgraphs.push_back(itr->second);
198 return subgraphs.size();
201 template <
typename Graph>
204 const pcl::PointIndices&
indices,
205 std::vector<boost::reference_wrapper<Graph>>& subgraphs)
207 typedef typename Graph::vertex_descriptor
VertexId;
208 typedef typename boost::reference_wrapper<Graph>
GraphRef;
210 std::set<int> index_set;
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();
218 for (
size_t i = 0; i <
indices.indices.size(); ++i)
220 index_set.insert(
indices.indices[i]);
221 boost::add_vertex(
indices.indices[i], first);
224 for (
VertexId v = 0;
v < boost::num_vertices(graph); ++
v)
225 if (!index_set.count(
v))
227 boost::add_vertex(
v, second);
231 template <
typename Graph>
234 const std::vector<pcl::PointIndices>&
indices,
235 std::vector<boost::reference_wrapper<Graph>>& subgraphs)
237 typedef typename Graph::vertex_descriptor
VertexId;
238 typedef typename boost::reference_wrapper<Graph>
GraphRef;
240 std::set<int> index_set;
243 for (
size_t i = 0; i <
indices.size(); ++i)
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)
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))
259 boost::add_vertex(
v,
s);
263 template <
typename Graph>
270 typedef typename Graph::vertex_descriptor
VertexId;
272 std::vector<float> K(boost::num_vertices(graph), 0);
273 Eigen::MatrixXf P(boost::num_vertices(graph), 3);
277 for (boost::tie(ei, ee) = boost::edges(graph); ei != ee; ++ei)
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)));
295 P.row(src) += nq * d1 * w1;
296 P.row(tgt) += np * d2 * w2;
299 for (
VertexId v = 0;
v < boost::num_vertices(graph); ++
v)
302 graph[
v].getVector3fMap() -= P.row(
v) / K[
v];