void Mapper::viewer() { pcl::visualization::CloudViewer viewer("viewer"); PointCloud::Ptr globalMap (new PointCloud); pcl::VoxelGrid<PointT> voxel; voxel.setLeafSize( resolution, resolution, resolution ); while (shutdownFlag == false) { static int cntGlobalUpdate = 0; if ( poseGraph.keyframes.size() <= this->keyframe_size ) { usleep(1000); continue; } // keyframe is updated PointCloud::Ptr tmp(new PointCloud()); if (cntGlobalUpdate % 15 == 0) { // update all frames cout<<"redrawing frames"<<endl; globalMap->clear(); for ( int i=0; i<poseGraph.keyframes.size(); i+=2 ) { PointCloud::Ptr cloud = this->generatePointCloud(poseGraph.keyframes[i]); *globalMap += *cloud; } } else { for ( int i=poseGraph.keyframes.size()-1; i>=0 && i>poseGraph.keyframes.size()-6; i-- ) { PointCloud::Ptr cloud = this->generatePointCloud(poseGraph.keyframes[i]); *globalMap += *cloud; } } cntGlobalUpdate ++ ; //voxel voxel.setInputCloud( globalMap ); voxel.filter( *tmp ); keyframe_size = poseGraph.keyframes.size(); globalMap->swap( *tmp ); viewer.showCloud( globalMap ); cout<<"points in global map: "<<globalMap->points.size()<<endl; } }
int main(int argC,char **argV) { ros::init(argC,argV,"startPointCloud"); ros::NodeHandle n; std::string serverAddress; n.getParam("/serverNameOrIP",serverAddress); Socket mySocket(serverAddress.c_str(),"9005",streamSize); ros::Publisher pub = n.advertise<PointCloud>("point_cloud",1); PointCloud::Ptr pc (new PointCloud); pc->header.frame_id = ros::this_node::getNamespace().substr(1,std::string::npos) + "/kinect_pcl"; while(ros::ok()) { // TODO(Somhtr): change to ROS' logging API cout << "Reading data..." << endl; mySocket.readData(); // TODO(Somhtr): change to ROS' logging API cout << "Copying data..." << endl; float* pt_coords = reinterpret_cast<float*>(mySocket.mBuffer); for(uint64_t idx=0; idx<numberOfPoints; idx+=3) { pc->push_back(pcl::PointXYZ( pt_coords[idx], pt_coords[idx+1], pt_coords[idx+2] )); } double utcTime; memcpy(&utcTime,&mySocket.mBuffer[pointBufferSize],sizeof(double)); pc->header.stamp = ros::Time(utcTime).toSec(); pub.publish(pc); pc->clear(); ros::spinOnce(); } return 0; }
void WorldDownloadManager::mergePointCloudsAndMesh(std::vector<PointCloud::Ptr> &pointclouds, PointCloud::Ptr out_cloud, std::vector<TrianglesPtr> * meshes,Triangles * out_mesh) { uint offset = 0; const uint pointcloud_count = pointclouds.size(); out_cloud->clear(); if (out_mesh) out_mesh->clear(); for (uint pointcloud_i = 0; pointcloud_i < pointcloud_count; pointcloud_i++) { const uint pointcloud_size = pointclouds[pointcloud_i]->size(); // copy the points (*out_cloud) += *(pointclouds[pointcloud_i]); if (out_mesh) { // copy the triangles, shifting vertex id by an offset const uint mesh_size = (*meshes)[pointcloud_i]->size(); out_mesh->reserve(out_mesh->size() + mesh_size); for (uint triangle_i = 0; triangle_i < mesh_size; triangle_i++) { kinfu_msgs::KinfuMeshTriangle tri; const kinfu_msgs::KinfuMeshTriangle & v = (*(*meshes)[pointcloud_i])[triangle_i]; for (uint i = 0; i < 3; i++) tri.vertex_id[i] = v.vertex_id[i] + offset; out_mesh->push_back(tri); } offset += pointcloud_size; } } }
void WorldDownloadManager::cropMesh(const kinfu_msgs::KinfuCloudPoint & min, const kinfu_msgs::KinfuCloudPoint & max,PointCloud::ConstPtr cloud, TrianglesConstPtr triangles,PointCloud::Ptr out_cloud,TrianglesPtr out_triangles) { const uint triangles_size = triangles->size(); const uint cloud_size = cloud->size(); std::vector<bool> valid_points(cloud_size,true); std::vector<uint> valid_points_remap(cloud_size,0); std::cout << "Starting with " << cloud_size << " points and " << triangles_size << " triangles.\n"; uint offset; // check the points for (uint i = 0; i < cloud_size; i++) { const pcl::PointXYZ & pt = (*cloud)[i]; if (pt.x > max.x || pt.y > max.y || pt.z > max.z || pt.x < min.x || pt.y < min.y || pt.z < min.z) valid_points[i] = false; } // discard invalid points out_cloud->clear(); out_cloud->reserve(cloud_size); offset = 0; for (uint i = 0; i < cloud_size; i++) if (valid_points[i]) { out_cloud->push_back((*cloud)[i]); // save new position for triangles remap valid_points_remap[i] = offset; offset++; } out_cloud->resize(offset); // discard invalid triangles out_triangles->clear(); out_triangles->reserve(triangles_size); offset = 0; for (uint i = 0; i < triangles_size; i++) { const kinfu_msgs::KinfuMeshTriangle & tri = (*triangles)[i]; bool is_valid = true; // validate all the vertices for (uint h = 0; h < 3; h++) if (!valid_points[tri.vertex_id[h]]) { is_valid = false; break; } if (is_valid) { kinfu_msgs::KinfuMeshTriangle out_tri; // remap the triangle for (uint h = 0; h < 3; h++) out_tri.vertex_id[h] = valid_points_remap[(*triangles)[i].vertex_id[h]]; out_triangles->push_back(out_tri); offset++; } } out_triangles->resize(offset); std::cout << "Ended with " << out_cloud->size() << " points and " << out_triangles->size() << " triangles.\n"; }