C++ Ptr::clear方法代码示例

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";
}