template <typename PointT> typename pcl::gpu::kinfuLS::StandaloneMarchingCubes<PointT>::MeshPtr
pcl::gpu::kinfuLS::StandaloneMarchingCubes<PointT>::convertTrianglesToMesh (const pcl::gpu::DeviceArray<pcl::PointXYZ>& triangles)
{
if (triangles.empty () )
{
return MeshPtr ();
}
pcl::PointCloud<pcl::PointXYZ> cloud;
cloud.width = (int)triangles.size ();
cloud.height = 1;
triangles.download (cloud.points);
boost::shared_ptr<pcl::PolygonMesh> mesh_ptr ( new pcl::PolygonMesh () );
pcl::toPCLPointCloud2 (cloud, mesh_ptr->cloud);
mesh_ptr->polygons.resize (triangles.size () / 3);
for (size_t i = 0; i < mesh_ptr->polygons.size (); ++i)
{
pcl::Vertices v;
v.vertices.push_back (i*3+0);
v.vertices.push_back (i*3+2);
v.vertices.push_back (i*3+1);
mesh_ptr->polygons[i] = v;
}
return (mesh_ptr);
}
boost::shared_ptr<pcl::PolygonMesh> convertToMesh(const DeviceArray<PointXYZ>& triangles)
{
if (triangles.empty())
{
std::cerr << "kinfu_util::convertToMesh(): triangles empty...returning null..." << std::endl;
return boost::shared_ptr<pcl::PolygonMesh>();
}
pcl::PointCloud<pcl::PointXYZ> cloud;
cloud.width = (int)triangles.size();
cloud.height = 1;
triangles.download(cloud.points);
boost::shared_ptr<pcl::PolygonMesh> mesh_ptr( new pcl::PolygonMesh() );
pcl::toROSMsg(cloud, mesh_ptr->cloud);
mesh_ptr->polygons.resize (triangles.size() / 3);
for (size_t i = 0; i < mesh_ptr->polygons.size (); ++i)
{
pcl::Vertices v;
v.vertices.push_back(i*3+0);
v.vertices.push_back(i*3+2);
v.vertices.push_back(i*3+1);
mesh_ptr->polygons[i] = v;
}
return mesh_ptr;
}
Mesh_Ptr Mesh_File_Reader::read(
const Label & name,
bool load_vertices,
bool load_triangles,
bool load_triangle_strips,
bool load_mapping
) /* throw IO_Error */
{
Mesh_Ptr mesh_ptr(new Mesh());
Mesh &mesh = *mesh_ptr.get();
Filename binary_filename(_mesh_path / (name + ".bin"));
if(!boost::filesystem::exists(binary_filename))
{
Filename vertex_filename, triangle_filename,
triangle_strip_filename, mapping_filename;
if (load_vertices)
vertex_filename = _mesh_path / (name + ".vert");
if (load_triangles)
triangle_filename = _mesh_path / (name + ".tri");
if (load_triangle_strips)
triangle_strip_filename = _mesh_path / (name + ".strip");
if (load_mapping)
mapping_filename = _mesh_path / (name + ".map");
Mesh_ASCII_File_Parser parser;
parser.read_mesh(
vertex_filename,
triangle_filename,
triangle_strip_filename,
mapping_filename,
vertex_count(mesh),
triangle_count(mesh),
triangle_strip_length(mesh),
vertices(mesh),
vertex_sections(mesh),
vertex_relative_distances(mesh),
triangles(mesh),
triangle_strip(mesh));
} else {
Mesh_Binary_File_Parser parser;
parser.read_mesh(
binary_filename,
load_vertices,
load_triangles,
load_triangle_strips,
load_mapping,
vertex_count(mesh),
triangle_count(mesh),
triangle_strip_length(mesh),
vertices(mesh),
vertex_sections(mesh),
vertex_relative_distances(mesh),
triangles(mesh),
triangle_strip(mesh));
}
return mesh_ptr;
}
boost::shared_ptr<pcl17::PolygonMesh> SceneCloudView::convertToMesh(float* triangles, unsigned int maxverts)
{
if (maxverts == 0)
return boost::shared_ptr<pcl17::PolygonMesh>();
pcl17::PointCloud<pcl17::PointXYZ> cloud;
cloud.width = (int)(maxverts);
cloud.height = 1;
cloud.points.clear();
for (uint i = 0; i < 3 * maxverts; i += 3)
cloud.points.push_back(pcl17::PointXYZ(triangles[i], triangles[i + 1], triangles[i + 2]));
boost::shared_ptr<pcl17::PolygonMesh> mesh_ptr(new pcl17::PolygonMesh());
try
{
pcl17::toROSMsg(cloud, mesh_ptr->cloud);
}
catch (std::runtime_error e)
{
ROS_ERROR_STREAM("Error in converting cloud to image message: "
<< e.what());
}
mesh_ptr->polygons.resize(maxverts / 3);
for (size_t i = 0; i < mesh_ptr->polygons.size(); ++i)
{
pcl17::Vertices v;
v.vertices.push_back(i * 3 + 0);
v.vertices.push_back(i * 3 + 2);
v.vertices.push_back(i * 3 + 1);
mesh_ptr->polygons[i] = v;
}
return mesh_ptr;
}
RobotMeshModel::RobotMeshModel():nh_priv_("~") {
//Load robot description from parameter server
std::string robot_desc_string;
if(!nh_.getParam("robot_description", robot_desc_string)) {
ROS_ERROR("Could not get urdf from param server");
}
if (!urdf_.initString(robot_desc_string)) {
ROS_ERROR("Failed to parse urdf");
}
modelframe_= "/torso_lift_link";
nh_priv_.param<std::string>("robot_description_package_path", description_path, "..");
ROS_INFO("package_path %s", description_path.c_str());
nh_priv_.param<std::string>("camera_topic", camera_topic_, "/wide_stereo/right" );
nh_priv_.param<std::string>("camera_info_topic", camera_info_topic_, "/wide_stereo/right/camera_info" );
//Load robot mesh for each link
std::vector<boost::shared_ptr<urdf::Link> > links ;
urdf_.getLinks(links);
for (int i=0; i< links.size(); i++) {
if (links[i]->visual.get() == NULL) continue;
if (links[i]->visual->geometry.get() == NULL) continue;
if (links[i]->visual->geometry->type == urdf::Geometry::MESH) {
//todo: this should really be done by resource retriever
boost::shared_ptr<urdf::Mesh> mesh = boost::dynamic_pointer_cast<urdf::Mesh> (links[i]->visual->geometry);
std::string filename (mesh->filename);
if (filename.substr(filename.size() - 4 , 4) != ".stl" && filename.substr(filename.size() - 4 , 4) != ".dae") continue;
if (filename.substr(filename.size() - 4 , 4) == ".dae")
filename.replace(filename.size() - 4 , 4, ".stl");
ROS_INFO("adding link %d %s",i,links[i]->name.c_str());
filename.erase(0,25);
filename = description_path + filename;
boost::shared_ptr<CMeshO> mesh_ptr(new CMeshO);
if(vcg::tri::io::ImporterSTL<CMeshO>::Open(*mesh_ptr,filename.c_str())) {
ROS_ERROR("could not load mesh %s", filename.c_str());
continue;
}
links_with_meshes.push_back(links[i]);
meshes[links[i]->name] = mesh_ptr;
tf::Vector3 origin(links[i]->visual->origin.position.x, links[i]->visual->origin.position.y, links[i]->visual->origin.position.z);
tf::Quaternion rotation(links[i]->visual->origin.rotation.x, links[i]->visual->origin.rotation.y, links[i]->visual->origin.rotation.z, links[i]->visual->origin.rotation.w);
offsets_[links[i]->name] = tf::Transform(rotation, origin);
}
}
initRobot();
//get camera intinsics
ROS_INFO("waiting for %s", camera_info_topic_.c_str());
cam_info_ = ros::topic::waitForMessage<sensor_msgs::CameraInfo>(camera_info_topic_);
cameraframe_ = cam_info_->header.frame_id;
ROS_INFO("%s: robot model initialization done!", ros::this_node::getName().c_str());
}
