void PointCloud::initPointGraph(double distance_threshold)
{
if (distance_threshold == point_graph_threshold_
&& boost::num_edges(*point_graph_) != 0)
return;
triangulate();
point_graph_threshold_ = distance_threshold;
boost::PointGraph& g_point = *point_graph_;
g_point = boost::PointGraph(points_num_);
size_t t = 8;
for (CGAL::Delaunay::Finite_edges_iterator it = triangulation_->finite_edges_begin();
it != triangulation_->finite_edges_end(); ++ it)
{
const CGAL::Delaunay::Cell_handle& cell_handle = it->first;
const CGAL::Delaunay::Vertex_handle& source_handle = cell_handle->vertex(it->second);
const CGAL::Delaunay::Vertex_handle& target_handle = cell_handle->vertex(it->third);
double distance_L1 = std::sqrt(CGAL::squared_distance(source_handle->point(), target_handle->point()));
if (distance_L1 > point_graph_threshold_)
continue;
size_t source_id = source_handle->info();
size_t target_id = target_handle->info();
assert (source_id < points_num_ && target_id < points_num_);
WeightedEdge weighted_edge(distance_L1);
boost::add_edge(source_id, target_id, weighted_edge, g_point);
}
return;
}
double MaxflowInference::segment(PotentialsCache::ConstPtr pc,
Eigen::VectorXi* seg) const
{
// -- Check for monkey business and allocate.
ROS_ASSERT(pc->numEdgePotentials() == model_.eweights_.rows());
ROS_ASSERT(pc->numNodePotentials() == model_.nweights_.rows());
if(seg->rows() != pc->numNodes())
*seg = VecXi::Zero(pc->numNodes());
for(size_t i = 0; i < pc->node_.size(); ++i)
ROS_ASSERT(seg->rows() == pc->node_[i].rows());
int num_edges = 0;
for(size_t i = 0; i < pc->edge_.size(); ++i) {
ROS_ASSERT(seg->rows() == pc->edge_[i].rows());
ROS_ASSERT(seg->rows() == pc->edge_[i].cols());
num_edges += pc->edge_[i].nonZeros();
}
Graph3d graph(seg->rows(), num_edges);
graph.add_node(seg->rows());
Eigen::VectorXd weighted_node(seg->rows());
SparseMat weighted_edge(seg->rows(), seg->rows());
pc->applyWeights(model_, &weighted_edge, &weighted_node);
// -- Fill the graph with weighted node potentials.
for(int i = 0; i < weighted_node.rows(); ++i)
graph.add_tweights(i, weighted_node(i), 0);
// -- Fill the graph with edge potentials. Assumes symmetry & upper triangular.
for(int i = 0; i < weighted_edge.outerSize(); ++i) {
for(SparseMatrix<double, RowMajor>::InnerIterator it(weighted_edge, i); it; ++it) {
if(it.col() <= it.row())
continue;
ROS_WARN_STREAM_COND(it.value() < 0, "Edgepot weighted sum is negative: " << it.value());
ROS_FATAL_STREAM_COND(isnan(it.value()), "NaN in edgepot.");
graph.add_edge(it.row(), it.col(), it.value(), it.value());
}
}
// -- Run graph cuts.
HighResTimer hrt("maxflow");
hrt.start();
double mf = graph.maxflow();
hrt.stop();
//cout << hrt.reportMilliseconds() << endl;
// -- Fill the segmentation.
for(int i = 0; i < seg->rows(); ++i) {
if(graph.what_segment(i, Graph3d::SINK) == Graph3d::SINK)
seg->coeffRef(i) = -1;
else
seg->coeffRef(i) = 1;
}
return mf;
}
