void Cylinder::triangulate(list<TPPLPoly>& tri_list) const { TPPLPartition pp; list<TPPLPoly> polys; TPPLPoly poly; TPPLPoint pt; double d_alpha = 0.5; double alpha_max = 0, alpha_min = std::numeric_limits<double>::max(); for(size_t i = 0; i < contours_[0].size(); ++i) { double alpha = contours_[0][i](0) / r_; if (alpha > alpha_max) alpha_max = alpha; if (alpha < alpha_min) alpha_min = alpha; } std::cout << "r " << r_ << std::endl; std::cout << "alpha " << alpha_min << "," << alpha_max << std::endl; std::vector<std::vector<std::vector<Eigen::Vector2f> > > contours_split; for(size_t j = 0; j < contours_.size(); j++) { for(double i = alpha_min + d_alpha; i <= alpha_max; i += d_alpha) { std::vector<Eigen::Vector2f> contour_segment; for(size_t k = 0; k < contours_[j].size(); ++k) { double alpha = contours_[j][k](0) / r_; if( alpha >= i - d_alpha - 0.25 && alpha < i + 0.25) { contour_segment.push_back(contours_[j][k]); } } //std::cout << "c " << j << i << " has " << contour_segment.size() << " points" << std::endl; if(contour_segment.size() < 3) continue; poly.Init(contour_segment.size()); poly.SetHole(holes_[j]); for( unsigned int l = 0; l < contour_segment.size(); l++) { pt.x = contour_segment[l](0); pt.y = contour_segment[l](1); poly[l] = pt; } if (holes_[j]) poly.SetOrientation(TPPL_CW); else poly.SetOrientation(TPPL_CCW); polys.push_back(poly); } } // triangulation into monotone triangles pp.Triangulate_EC (&polys, &tri_list); }
GeneralPolygon::operator list<TPPLPoly>() { auto isPolygonOutside = [&](const Contour &referencePolygon, const Contour &poly) { for(unsigned int p=0; p < referencePolygon.size() ; ++p) if( !_evenOddRuleAlgorithm( referencePolygon[p], poly) ) return true; return false; }; list<TPPLPoly> polys; for(unsigned int c=0; c < _contours.size() ; ++c) { const Contour &contour = _contours[c]; TPPLPoly poly; poly.Init(contour.size()); for(unsigned int v=0; v < contour.size() ; ++v) { TPPLPoint point; point.x = contour[v].x; point.y = contour[v].y; poly[v] = point; } // BE CAREFULL!!! not really correct because the polygon could be concave then // the center of mass could not be inside the polygon sf::Vector2f centerOfMass(0.0f, 0.0f); for(unsigned int i=0; i < contour.size() ; ++i) centerOfMass += contour[i]; centerOfMass *= (1.0f/contour.size()); std::vector<Contour> outsideContours; for(unsigned int i=0; i < _contours.size() ; ++i) { if( i == c ) { outsideContours.push_back(_contours[i]); continue; } if( isPolygonOutside(_contours[i], contour) ) outsideContours.push_back(_contours[i]); } // first test if is a hole or not if( !_evenOddRuleAlgorithm( centerOfMass, outsideContours ) ) poly.SetHole(true); else poly.SetHole(false); // if it is a hole then it must be in CW order to the algorithm to recognize // else must be in CCW if( poly.IsHole() ) poly.SetOrientation(TPPL_CW);// Hole orientation (needed in the algorithm) else poly.SetOrientation(TPPL_CCW);// Not a hole orientation (needed in the algorithm) polys.push_back(poly); } return polys; }
void ShapeMarker::createMarker (visualization_msgs::InteractiveMarkerControl& im_ctrl) { marker.id = shape_.id; marker.header = shape_.header; //std::cout << marker.header.frame_id << std::endl; //marker.header.stamp = ros::Time::now() ; marker.type = visualization_msgs::Marker::TRIANGLE_LIST; marker.ns = "shape visualization"; marker.action = visualization_msgs::Marker::ADD; marker.lifetime = ros::Duration (); //set color marker.color.g = shape_.color.g; marker.color.b = shape_.color.b; marker.color.r = shape_.color.r; if (arrows_ || deleted_){ marker.color.a = 0.5; } else { marker.color.a = shape_.color.a; // marker.color.r = shape_.color.r; } //set scale marker.scale.x = 1; marker.scale.y = 1; marker.scale.z = 1; /* transform shape points to 2d and store 2d point in triangle list */ TPPLPartition pp; list<TPPLPoly> polys, tri_list; Eigen::Vector3f v, normal, origin; if(shape_.type== cob_3d_mapping_msgs::Shape::CYLINDER) { cob_3d_mapping::Cylinder c; cob_3d_mapping::fromROSMsg (shape_, c); c.ParamsFromShapeMsg(); // make trinagulated cylinder strip //transform cylinder in local coordinate system c.makeCyl2D(); c.TransformContours(c.transform_from_world_to_plane); //c.transform2tf(c.transform_from_world_to_plane); //TODO: WATCH OUT NO HANDLING FOR MULTY CONTOUR CYLINDERS AND HOLES TPPLPoly poly; TPPLPoint pt; for(size_t j=0;j<c.contours.size();j++){ poly.Init(c.contours[j].size()); poly.SetHole (shape_.holes[j]); for(size_t i=0;i<c.contours[j].size();++i){ pt.x=c.contours[j][i][0]; pt.y=c.contours[j][i][1]; poly[i]=pt; } if (shape_.holes[j]) poly.SetOrientation (TPPL_CW); else poly.SetOrientation (TPPL_CCW); polys.push_back(poly); } // triangualtion itno monotone triangles pp.Triangulate_EC (&polys, &tri_list); transformation_inv_ = c.transform_from_world_to_plane.inverse(); // optional refinement step list<TPPLPoly> refined_tri_list; triangle_refinement(tri_list,refined_tri_list); tri_list=refined_tri_list; } if(shape_.type== cob_3d_mapping_msgs::Shape::POLYGON) { cob_3d_mapping::Polygon p; if (shape_.params.size () == 4) { cob_3d_mapping::fromROSMsg (shape_, p); normal (0) = shape_.params[0]; normal (1) = shape_.params[1]; normal (2) = shape_.params[2]; origin (0) = shape_.centroid.x; origin (1) = shape_.centroid.y; origin (2) = shape_.centroid.z; v = normal.unitOrthogonal (); pcl::getTransformationFromTwoUnitVectorsAndOrigin (v, normal, origin, transformation_); transformation_inv_ = transformation_.inverse (); } for (size_t i = 0; i < shape_.points.size (); i++) { pcl::PointCloud<pcl::PointXYZ> pc; TPPLPoly poly; pcl::fromROSMsg (shape_.points[i], pc); poly.Init (pc.points.size ()); poly.SetHole (shape_.holes[i]); for (size_t j = 0; j < pc.points.size (); j++) { poly[j] = msgToPoint2D (pc[j]); } if (shape_.holes[i]) poly.SetOrientation (TPPL_CW); else poly.SetOrientation (TPPL_CCW); polys.push_back (poly); } pp.Triangulate_EC (&polys, &tri_list); }//Polygon if(tri_list.size()==0) { ROS_WARN("Could not triangulate, will not display this shape! (ID: %d)", shape_.id); } //ROS_INFO(" creating markers for this shape....."); marker.points.resize (/*it->GetNumPoints ()*/tri_list.size()*3); TPPLPoint pt; int ctr=0; for (std::list<TPPLPoly>::iterator it = tri_list.begin (); it != tri_list.end (); it++) { //draw each triangle switch(shape_.type) { case(cob_3d_mapping_msgs::Shape::POLYGON): { for (long i = 0; i < it->GetNumPoints (); i++) { pt = it->GetPoint (i); marker.points[3*ctr+i].x = pt.x; marker.points[3*ctr+i].y = pt.y; marker.points[3*ctr+i].z = 0; //if(shape_.id == 39) std::cout << pt.x << "," << pt.y << std::endl; } //std::cout << marker.points.size() << std::endl; } case(cob_3d_mapping_msgs::Shape::CYLINDER): { for (long i = 0; i < it->GetNumPoints (); i++) { pt = it->GetPoint(i); //apply rerolling of cylinder analogous to cylinder class if(shape_.params.size()!=10){ break; } double alpha=pt.x/shape_.params[9]; marker.points[3*ctr+i].x = shape_.params[9]*sin(-alpha); marker.points[3*ctr+i].y = pt.y; marker.points[3*ctr+i].z = shape_.params[9]*cos(-alpha); ////Keep Cylinder flat - Debuging //marker.points[i].x = pt.x; //marker.points[i].y = pt.y; //marker.points[i].z = 0; } } } ctr++; } //set pose Eigen::Quaternionf quat (transformation_inv_.rotation ()); Eigen::Vector3f trans (transformation_inv_.translation ()); marker.pose.position.x = trans (0); marker.pose.position.y = trans (1); marker.pose.position.z = trans (2); marker.pose.orientation.x = quat.x (); marker.pose.orientation.y = quat.y (); marker.pose.orientation.z = quat.z (); marker.pose.orientation.w = quat.w (); im_ctrl.markers.push_back (marker); // if(!arrows_) { // Added For displaying the arrows on Marker Position marker_.pose.position.x = marker.pose.position.x ; marker_.pose.position.y = marker.pose.position.y ; marker_.pose.position.z = marker.pose.position.z ; marker_.pose.orientation.x = marker.pose.orientation.x ; marker_.pose.orientation.y = marker.pose.orientation.y ; marker_.pose.orientation.z = marker.pose.orientation.z ; // end }
std::vector<std::vector<Point3d> > computeTriangulation(const Point3dVector& vertices, const std::vector<std::vector<Point3d> >& holes, double tol) { std::vector<std::vector<Point3d> > result; // check input if (vertices.size () < 3){ return result; } boost::optional<Vector3d> normal = getOutwardNormal(vertices); if (!normal || normal->z() > -0.999){ return result; } for (const auto& hole : holes){ normal = getOutwardNormal(hole); if (!normal || normal->z() > -0.999){ return result; } } std::vector<Point3d> allPoints; // PolyPartition does not support holes which intersect the polygon or share an edge // if any hole is not fully contained we will use boost to remove all the holes bool polyPartitionHoles = true; for (const std::vector<Point3d>& hole : holes){ if (!within(hole, vertices, tol)){ // PolyPartition can't handle this polyPartitionHoles = false; break; } } if (!polyPartitionHoles){ // use boost to do all the intersections std::vector<std::vector<Point3d> > allFaces = subtract(vertices, holes, tol); std::vector<std::vector<Point3d> > noHoles; for (const std::vector<Point3d>& face : allFaces){ std::vector<std::vector<Point3d> > temp = computeTriangulation(face, noHoles); result.insert(result.end(), temp.begin(), temp.end()); } return result; } // convert input to vector of TPPLPoly std::list<TPPLPoly> polys; TPPLPoly outerPoly; // must be counter-clockwise, input vertices are clockwise outerPoly.Init(vertices.size()); outerPoly.SetHole(false); unsigned n = vertices.size(); for(unsigned i = 0; i < n; ++i){ // should all have zero z coordinate now double z = vertices[n-i-1].z(); if (abs(z) > tol){ LOG_FREE(Error, "utilities.geometry.computeTriangulation", "All points must be on z = 0 plane for triangulation methods"); return result; } Point3d point = getCombinedPoint(vertices[n-i-1], allPoints, tol); outerPoly[i].x = point.x(); outerPoly[i].y = point.y(); } outerPoly.SetOrientation(TPPL_CCW); polys.push_back(outerPoly); for (const std::vector<Point3d>& holeVertices : holes){ if (holeVertices.size () < 3){ LOG_FREE(Error, "utilities.geometry.computeTriangulation", "Hole has fewer than 3 points, ignoring"); continue; } TPPLPoly innerPoly; // must be clockwise, input vertices are clockwise innerPoly.Init(holeVertices.size()); innerPoly.SetHole(true); //std::cout << "inner :"; for(unsigned i = 0; i < holeVertices.size(); ++i){ // should all have zero z coordinate now double z = holeVertices[i].z(); if (abs(z) > tol){ LOG_FREE(Error, "utilities.geometry.computeTriangulation", "All points must be on z = 0 plane for triangulation methods"); return result; } Point3d point = getCombinedPoint(holeVertices[i], allPoints, tol); innerPoly[i].x = point.x(); innerPoly[i].y = point.y(); } innerPoly.SetOrientation(TPPL_CW); polys.push_back(innerPoly); } // do partitioning TPPLPartition pp; std::list<TPPLPoly> resultPolys; int test = pp.Triangulate_EC(&polys,&resultPolys); if (test == 0){ test = pp.Triangulate_MONO(&polys, &resultPolys); } if (test == 0){ LOG_FREE(Error, "utilities.geometry.computeTriangulation", "Failed to partition polygon"); return result; } // convert back to vertices std::list<TPPLPoly>::iterator it, itend; //std::cout << "Start" << std::endl; for(it = resultPolys.begin(), itend = resultPolys.end(); it != itend; ++it){ it->SetOrientation(TPPL_CW); std::vector<Point3d> triangle; for (long i = 0; i < it->GetNumPoints(); ++i){ TPPLPoint point = it->GetPoint(i); triangle.push_back(Point3d(point.x, point.y, 0)); } //std::cout << triangle << std::endl; result.push_back(triangle); } //std::cout << "End" << std::endl; return result; }
void ShapeMarker::onNewMessage( const MarkerConstPtr& old_message, const MarkerConstPtr& new_message ) { TPPLPartition pp; list<TPPLPoly> polys,result; //fill polys for(size_t i=0; i<new_message->points.size(); i++) { pcl::PointCloud<pcl::PointXYZ> pc; TPPLPoly poly; pcl::fromROSMsg(new_message->points[i],pc); poly.Init(pc.size()); poly.SetHole(new_message->holes[i]); for(size_t j=0; j<pc.size(); j++) { poly[j] = MsgToPoint2D(pc[j], new_message); } if(new_message->holes[i]) poly.SetOrientation(TPPL_CW); else poly.SetOrientation(TPPL_CCW); polys.push_back(poly); } pp.Triangulate_EC(&polys,&result); polygon_->clear(); polygon_->begin(createMaterialIfNotExists(new_message->color.r,new_message->color.b,new_message->color.g,new_message->color.a), Ogre::RenderOperation::OT_TRIANGLE_LIST); TPPLPoint p1,p2,p3,p4, p12,p23,p31; for(std::list<TPPLPoly>::iterator it=result.begin(); it!=result.end(); it++) { //draw each triangle if(it->GetNumPoints()!=3) continue; p1 = it->GetPoint(0); p2 = it->GetPoint(1); p3 = it->GetPoint(2); p4.x = (p1.x+p2.x+p3.x)/3; p4.y = (p1.y+p2.y+p3.y)/3; p12.x = (p1.x+p2.x)/2; p12.y = (p1.y+p2.y)/2; p23.x = (p3.x+p2.x)/2; p23.y = (p3.y+p2.y)/2; p31.x = (p1.x+p3.x)/2; p31.y = (p1.y+p3.y)/2; triangle(new_message,polygon_,p1,p12,p4); triangle(new_message,polygon_,p1,p31,p4); triangle(new_message,polygon_,p3,p23,p4); triangle(new_message,polygon_,p12,p2,p4); triangle(new_message,polygon_,p31,p3,p4); triangle(new_message,polygon_,p23,p2,p4); } polygon_->end(); vis_manager_->getSelectionManager()->removeObject(coll_); /*coll_ = vis_manager_->getSelectionManager()->createCollisionForObject( shape_, SelectionHandlerPtr(new MarkerSelectionHandler(this, MarkerID( "fake_ns", new_message->id))), coll_);*/ Ogre::Vector3 pos, scale, scale_correct; Ogre::Quaternion orient; //transform(new_message, pos, orient, scale); /*if (owner_ && (new_message->scale.x * new_message->scale.y * new_message->scale.z == 0.0f)) { owner_->setMarkerStatus(getID(), status_levels::Warn, "Scale of 0 in one of x/y/z"); }*/ Eigen::Vector3f origin=MsgToOrigin(new_message); pos.x = origin(0); pos.y = origin(1); pos.z = origin(2); //setPosition(pos); return; setOrientation( orient * Ogre::Quaternion( Ogre::Degree(90), Ogre::Vector3(1,0,0) ) ); //scale_correct = Ogre::Quaternion( Ogre::Degree(90), Ogre::Vector3(1,0,0) ) * scale; //shape_->setScale(scale_correct); }