33 #ifndef _TEXTURED_TRIANGLE_CPP_
34 #define _TEXTURED_TRIANGLE_CPP_
43 x=vx;
y=vy;
z=vz;
rgb=vrgb;
60 CGAL::Triangle_2<K1> t1(CGAL::Point_2<K1>(
v[0].x,
v[0].y), CGAL::Point_2<K1>(
v[1].x,
v[1].y), CGAL::Point_2<K1>(
v[2].x,
v[2].y));
61 CGAL::Triangle_2<K1> t2(CGAL::Point_2<K1>(t->
v[0].
x, t->
v[0].
y), CGAL::Point_2<K1>(t->
v[1].
x, t->
v[1].
y), CGAL::Point_2<K1>(t->
v[2].
x, t->
v[2].
y));
63 CGAL::Object result = CGAL::intersection(t1, t2);
64 if (
const CGAL::Triangle_2<K1> *itriangle = CGAL::object_cast<CGAL::Triangle_2<K1> >(&result))
70 else if (
const std::vector<CGAL::Point_2<K1> > *iptvector = CGAL::object_cast<std::vector<CGAL::Point_2<K1> > >(&result))
177 else if (method==
CHECK)
181 std::priority_queue<size_t> index_to_remove;
186 for (
size_t i=0; i<
set.size(); ++i)
188 if (t->provenience !=
set[i]->provenience)
189 if (
set[i]->overlaps(&(*t)))
198 if (t->weight >
set[i]->weight)
201 index_to_remove.push(i);
218 while (!index_to_remove.empty())
220 ROS_INFO(
"Removing triangle %d", (
int)index_to_remove.top());
221 set.erase(
set.begin() + index_to_remove.top());
222 index_to_remove.pop();
240 transform.setRotation(st->getRotation());
247 pc.points.erase(
pc.points.begin(),
pc.points.end());
250 for(
size_t k=0; k<
set.size(); k++)
256 for (
int u=0; u<3;u++)
258 pcl::PointCloud<pcl::PointXYZ> pc_local;
261 pt.x =
set[k]->v[u].x;
262 pt.y =
set[k]->v[u].y;
263 pt.z =
set[k]->v[u].z;
264 pc_local.points.push_back(pt);
266 pcl::PointCloud<pcl::PointXYZ> pc_global;
267 pcl_ros::transformPointCloud(pc_local, pc_global,
transform.inverse());
268 pcl::PointXYZRGB ptcolor;
269 ptcolor.x = pc_global.points[0].x;
270 ptcolor.y = pc_global.points[0].y;
271 ptcolor.z = pc_global.points[0].z;
272 ptcolor.rgb =
set[k]->v[u].rgb;
274 pc.points.push_back(ptcolor);
278 pc.width =
pc.points.size();
284 ROS_INFO(
"Exported triangle set %d triangles from a list with set.size()=%d", (
int)
pc.points.size(), (int)
set.size());
int set_transform(tf::Transform *st)
std::vector< int > pc_proveniences
class_textured_vertex v[3]
bool overlaps(class_textured_triangle *t)
int add_triangle(class_textured_vertex v0, class_textured_vertex v1, class_textured_vertex v2, int provenience, float weight, t_add_method method)
#define DID_NOT_ADD_TRIANGLE
class_textured_vertex(void)
std::vector< boost::shared_ptr< class_textured_triangle > > set
pcl::PointCloud< pcl::PointXYZRGB > pc
header for textured triangle
class_textured_triangle(void)