void straightSkeletonToMultiLineString(
const CGAL::Straight_skeleton_2<K> & ss,
MultiLineString& result
)
{
typedef CGAL::Straight_skeleton_2<K> Ss ;
typedef typename Ss::Vertex_const_handle Vertex_const_handle ;
typedef typename Ss::Halfedge_const_handle Halfedge_const_handle ;
typedef typename Ss::Halfedge_const_iterator Halfedge_const_iterator ;
Halfedge_const_handle null_halfedge ;
Vertex_const_handle null_vertex ;
for ( Halfedge_const_iterator it = ss.halfedges_begin(); it != ss.halfedges_end(); ++it ){
// skip contour edge
if ( ! it->is_bisector() ){
continue ;
}
// avoid duplicates
if ( it->opposite() < it ){
continue ;
}
result.addGeometry( new LineString(
Point( it->opposite()->vertex()->point() ),
Point( it->vertex()->point() )
) );
}
}
void
MainWindow::on_actionInnerSkeleton_triggered()
{
if(poly.size()>0){
if(! poly.is_simple()){
return;
}
clear();
if(! poly.is_counterclockwise_oriented()){
poly.reverse_orientation();
}
SsPtr iss = CGAL::create_interior_straight_skeleton_2(poly.vertices_begin(), poly.vertices_end());
CGAL::Straight_skeleton_2<K> const& ss = *iss;
typedef Ss::Vertex_const_handle Vertex_const_handle ;
typedef Ss::Halfedge_const_handle Halfedge_const_handle ;
typedef Ss::Halfedge_const_iterator Halfedge_const_iterator ;
Halfedge_const_handle null_halfedge ;
Vertex_const_handle null_vertex ;
for ( Halfedge_const_iterator i = ss.halfedges_begin(); i != ss.halfedges_end(); ++i )
{
if ( i->is_bisector() ){
Segment_2 s(i->opposite()->vertex()->point(), i->vertex()->point());
skeletonGraphicsItems.push_back(new QGraphicsLineItem(convert(s)));
scene.addItem(skeletonGraphicsItems.back());
skeletonGraphicsItems.back()->setPen(QPen(Qt::blue, 0, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin));
}
}
}
}
void print_straight_skeleton( CGAL::Straight_skeleton_2<K> const& ss )
{
typedef CGAL::Straight_skeleton_2<K> Ss ;
typedef typename Ss::Vertex_const_handle Vertex_const_handle ;
typedef typename Ss::Halfedge_const_handle Halfedge_const_handle ;
typedef typename Ss::Halfedge_const_iterator Halfedge_const_iterator ;
Halfedge_const_handle null_halfedge ;
Vertex_const_handle null_vertex ;
std::cerr << "Straight skeleton with " << ss.size_of_vertices()
<< " vertices, " << ss.size_of_halfedges()
<< " halfedges and " << ss.size_of_faces()
<< " faces" << std::endl ;
for ( Halfedge_const_iterator i = ss.halfedges_begin(); i != ss.halfedges_end(); ++i )
{
if ( !i->is_bisector() ) continue;
std::cout << "2 " ;
print_point(i->opposite()->vertex()->point()) ;
std::cout << " 0 " ;
print_point(i->vertex()->point());
std::cout << " 0\n" ;
//std::cout << " " << ( i->is_bisector() ? "bisector" : "contour" ) << std::endl;
}
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "cgal_inner_skeleton");
ros::NodeHandle n;
ros::Publisher contourCubePub = n.advertise<visualization_msgs::MarkerArray>("contour_vertex_array", 10);
ros::Publisher skeletonCubePub = n.advertise<visualization_msgs::MarkerArray>("skeleton_vertex_array", 10);
ros::Publisher skeletonLinePub = n.advertise<visualization_msgs::Marker>("skeleton_Lines", 10);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
std::string path = ros::package::getPath("component_test");
std::cout<<"PATH is:"<<path;
geometry_msgs::PoseArray points;
geometry_msgs::Pose pose;
Polygon_2 poly ;
poly.push_back( Point(-1,-1) ) ;
poly.push_back( Point(0,-5) ) ;
poly.push_back( Point(1,-1) ) ;
poly.push_back( Point(5,0) ) ;
poly.push_back( Point(1,1) ) ;
poly.push_back( Point(0,5) ) ;
poly.push_back( Point(-1,1) ) ;
poly.push_back( Point(-5,0) ) ;
// You can pass the polygon via an iterator pair
SsPtr iss = CGAL::create_interior_straight_skeleton_2(poly.vertices_begin(), poly.vertices_end());
// Or you can pass the polygon directly, as below.
// To create an exterior straight skeleton you need to specify a maximum offset.
double lMaxOffset = 5 ;
SsPtr oss = CGAL::create_exterior_straight_skeleton_2(lMaxOffset, poly);
//print_straight_skeleton(*iss);
//print_straight_skeleton(*oss);
typedef CGAL::Straight_skeleton_2<K> Ss ;
typedef typename Ss::Vertex_const_handle Vertex_const_handle;
typedef typename Ss::Halfedge_const_handle Halfedge_const_handle;
typedef typename Ss::Halfedge_const_iterator Halfedge_const_iterator;
Halfedge_const_handle null_halfedge;
Vertex_const_handle null_vertex;
std::cout << "Straight skeleton with " << (*iss).size_of_vertices()
<< " vertices, " << (*iss).size_of_halfedges()
<< " halfedges and " << (*iss).size_of_faces()
<< " faces" << std::endl ;
visualization_msgs::MarkerArray contour_marker_array,bisector_marker_array;
visualization_msgs::Marker marker;
int id=0;
std::vector<geometry_msgs::Point> lineSegments;
geometry_msgs::Point point;
for ( Halfedge_const_iterator i = (*iss).halfedges_begin(); i != (*iss).halfedges_end(); ++i )
{
//print_point(i->opposite()->vertex()->point()) ;
Vec3f vec2(i->opposite()->vertex()->point().x() , i->opposite()->vertex()->point().y(), 1);
std::cout << "->" ;
//print_point(i->vertex()->point());
std::cout << " " << ( i->is_bisector() ? "bisector" : "contour" ) << std::endl;
Vec3f vec1(i->vertex()->point().x(), i->vertex()->point().y(), 1);
if(i->is_bisector())
{
point.x = i->vertex()->point().x(); point.y = i->vertex()->point().y(); point.z = 1;
lineSegments.push_back(point);
point.x = i->opposite()->vertex()->point().x(); point.y = i->opposite()->vertex()->point().y(); point.z = 1;
lineSegments.push_back(point);
/*
marker = drawLines(vec1, id, 1);
bisector_marker_array.markers.push_back(marker);
marker = drawLines(vec2, id, 1);
bisector_marker_array.markers.push_back(marker);
*/
}
else
{
/*
marker = drawLines(vec1, id, 2);
contour_marker_array.markers.push_back(marker);
marker = drawLines(vec2, id, 2);
contour_marker_array.markers.push_back(marker);
*/
}
id++;
}
visualization_msgs::Marker linesList = drawLines(lineSegments);
/*
contourCubePub.publish(contour_marker_array);
skeletonCubePub.publish(bisector_marker_array);
*/
ros::Rate loop_rate(0.5);
while(ros::ok())
{
ROS_INFO("Looping till eternity");
contourCubePub.publish(contour_marker_array);
skeletonCubePub.publish(bisector_marker_array);
skeletonLinePub.publish(linesList);
ros::spinOnce();
loop_rate.sleep();
}
return EXIT_SUCCESS;
}