void drawScene() {
static float pitch = 22.5;
static float yaw = 12.25;
static float time = 0.0;
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glPushMatrix();
if (move) {
time += 0.025;
}
setTime(time);
glTranslatef(0.0, 0.0, -1.0);
glRotatef(-pitch, 1.0, 0.0, 0.0);
glRotatef(0.0, 0.0, 1.0, 0.0);
glRotatef(yaw, 0.0, 0.0, 1.0);
glPushAttrib(GL_LIGHTING_BIT);
glDisable(GL_LIGHTING);
glColor3f(1.0, 1.0, 1.0);
glutWireCube(1.0);
glPopAttrib();
glPushMatrix();
glTranslatef(-0.5, -0.5, -0.5);
glBegin(GL_TRIANGLES);
marchingCubes();
glEnd();
glPopMatrix();
glPopMatrix();
glutSwapBuffers();
}
void WorldDownloadManager::extractMeshWorker(kinfu_msgs::KinfuTsdfRequestConstPtr req)
{
ROS_INFO("kinfu: Extract Mesh Worker started.");
// prepare with same header
kinfu_msgs::KinfuTsdfResponsePtr resp(new kinfu_msgs::KinfuTsdfResponse());
resp->tsdf_header = req->tsdf_header;
resp->reference_frame_id = m_reference_frame_name;
ROS_INFO("kinfu: Locking kinfu...");
if (!lockKinfu())
return; // shutting down or synchronization error
ROS_INFO("kinfu: Locked.");
pcl::PointCloud<pcl::PointXYZI>::Ptr kinfu_cloud = req->request_reset ?
m_kinfu->extractWorldAndReset() : m_kinfu->extractWorld();
unlockKinfu();
Eigen::Affine3f transformation = m_reverse_initial_transformation;
std::vector<Mesh::Ptr> meshes;
ROS_INFO("kinfu: Marching cubes...");
if (!marchingCubes(kinfu_cloud,meshes))
return; // ERROR
kinfu_cloud->clear(); // save some memory
std::vector<PointCloud::Ptr> clouds;
std::vector<TrianglesPtr> clouds_triangles;
ROS_INFO("kinfu: Divide triangles and points...");
for (uint i = 0; i < meshes.size(); i++)
{
clouds.push_back(PointCloud::Ptr(new PointCloud()));
clouds_triangles.push_back(TrianglesPtr(new Triangles()));
separateMesh(meshes[i],clouds[i],clouds_triangles[i]);
}
meshes.clear(); // save some memory
TrianglesPtr triangles(new Triangles());
PointCloud::Ptr cloud(new PointCloud());
ROS_INFO("kinfu: Merging points...");
mergePointCloudsAndMesh(clouds,cloud,&clouds_triangles,&(*triangles));
// if needed, transform
if (req->request_transformation)
{
ROS_INFO("kinfu: A custom transformation will be applied.");
Eigen::Affine3f tr = toEigenAffine(req->transformation_linear,req->transformation_translation);
transformation = tr * transformation;
}
ROS_INFO("kinfu: Applying transformation...");
pcl::transformPointCloud(*cloud,*cloud,transformation);
// if needed, crop
if (req->request_bounding_box)
{
ROS_INFO("kinfu: Cropping...");
TrianglesPtr out_triangles(new Triangles());
PointCloud::Ptr out_cloud(new PointCloud());
cropMesh(req->bounding_box_min,req->bounding_box_max,cloud,triangles,out_cloud,out_triangles);
cloud = out_cloud;
triangles = out_triangles;
}
ROS_INFO("kinfu: Publishing...");
pclPointCloudToMessage(cloud,resp->point_cloud);
resp->triangles.swap(*triangles);
m_pub.publish(resp);
ROS_INFO("kinfu: Extract Mesh Worker complete.");
}
