C++ pointcloud::Ptr类代码示例

std::vector<GraspHypothesis> HandSearch::findHands(const PointCloud::Ptr cloud, 
  const Eigen::VectorXi& pts_cam_source, const std::vector<int>& indices, 
  const PointCloud::Ptr cloud_plot, bool calculates_antipodal, 
  bool uses_clustering)
{
  // create KdTree for neighborhood search
	pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
	kdtree.setInputCloud(cloud);

	cloud_normals_.resize(3, cloud->size());
	cloud_normals_.setZero(3, cloud->size());

	// calculate normals for all points
	if (calculates_antipodal)
	{
		//std::cout << "Calculating normals for all points\n";
		nn_radius_taubin_ = 0.01;
		std::vector<int> indices_cloud(cloud->size());
		for (int i = 0; i < indices_cloud.size(); i++)
			indices_cloud[i] = i;
		findQuadrics(cloud, pts_cam_source, kdtree, indices_cloud);
		nn_radius_taubin_ = 0.03;
	}

	// draw samples from the point cloud uniformly
	std::vector<int> indices_rand;
	Eigen::VectorXi hands_cam_source;
	if (indices.size() == 0)
	{
		double t_rand = omp_get_wtime();
		//std::cout << "Generating uniform random indices ...\n";
		indices_rand.resize(num_samples_);
		pcl::RandomSample<pcl::PointXYZ> random_sample;
		random_sample.setInputCloud(cloud);
		random_sample.setSample(num_samples_);
		random_sample.filter(indices_rand);
		hands_cam_source.resize(num_samples_);
		for (int i = 0; i < num_samples_; i++)
			hands_cam_source(i) = pts_cam_source(indices_rand[i]);
		//std::cout << " Done in " << omp_get_wtime() - t_rand << " sec\n";
	}
	else
		indices_rand = indices;

	if (plots_samples_)
		plot_.plotSamples(indices_rand, cloud);

	// find quadrics
	//std::cout << "Estimating local axes ...\n";
	std::vector<Quadric> quadric_list = findQuadrics(cloud, pts_cam_source, kdtree, indices_rand);
	if (plots_local_axes_)
		plot_.plotLocalAxes(quadric_list, cloud_plot);

	// find hands
	//std::cout << "Finding hand poses ...\n";
	std::vector<GraspHypothesis> hand_list = findHands(cloud, pts_cam_source, quadric_list, hands_cam_source, kdtree);
  
  return hand_list;
}

bool RegionGrowing::seedRegionGrowing(
    pcl::PointCloud<PointNormalT>::Ptr src_points,
    const PointT seed_point, const PointCloud::Ptr cloud,
    PointNormal::Ptr normals) {
    if (cloud->empty() || normals->size() != cloud->size()) {
       ROS_ERROR("- Region growing failed. Incorrect inputs sizes ");
       return false;
    }
    if (isnan(seed_point.x) || isnan(seed_point.y) || isnan(seed_point.z)) {
       ROS_ERROR("- Seed Point is Nan. Skipping");
       return false;
    }

    this->kdtree_->setInputCloud(cloud);
    
    std::vector<int> neigbor_indices;
    this->getPointNeigbour<int>(neigbor_indices, seed_point, 1);
    int seed_index = neigbor_indices[0];
    
    const int in_dim = static_cast<int>(cloud->size());
    int *labels = reinterpret_cast<int*>(malloc(sizeof(int) * in_dim));
    
#ifdef _OPENMP
#pragma omp parallel for num_threads(this->num_threads_)
#endif
    for (int i = 0; i < in_dim; i++) {
       if (i == seed_index) {
          labels[i] = 1;
       }
       labels[i] = -1;
    }
    this->seedCorrespondingRegion(labels, cloud, normals,
                                  seed_index, seed_index);
    src_points->clear();
    for (int i = 0; i < in_dim; i++) {
       if (labels[i] != -1) {
          PointNormalT pt;
          pt.x = cloud->points[i].x;
          pt.y = cloud->points[i].y;
          pt.z = cloud->points[i].z;
          pt.r = cloud->points[i].r;
          pt.g = cloud->points[i].g;
          pt.b = cloud->points[i].b;
          pt.normal_x = normals->points[i].normal_x;
          pt.normal_y = normals->points[i].normal_y;
          pt.normal_z = normals->points[i].normal_z;
          src_points->push_back(pt);
       }
    }
    free(labels);
    return true;
}

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

ViewExtract::Ptr ViewSampleExtractor::extract( cv::Rect& rct) const
{
    const PointCloud::Ptr vpc = view_->points;

    // Ensure rectangle falls within organised point cloud dimensions
    const cv::Rect pcRect(0,0, vpc->cols(), vpc->rows());
    rct &= pcRect;  // Set intersection

    ViewExtract::Ptr ve( new ViewExtract( vpc, rct));
    ve->setModelName( modelName_);
    ve->setPartName( partName_);
    ve->setAspectInfo( aspectInfo_);
    ve->setPosVec( view_->posVec);
    ve->setDirVec( view_->focalVec);
    ve->setUpVec( view_->upVec);
    return ve;
}   // end extract

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::pclPointCloudToMessage(PointCloud::Ptr pcl_cloud,
  std::vector<kinfu_msgs::KinfuCloudPoint> & message)
{
  uint cloud_size = pcl_cloud->size();
  message.resize(cloud_size);

  for (uint i = 0; i < cloud_size; i++)
  {
    message[i].x = (*pcl_cloud)[i].x;
    message[i].y = (*pcl_cloud)[i].y;
    message[i].z = (*pcl_cloud)[i].z;
  }
}

int ObjectAspect::calculate(const cv::Mat &image, const PointCloud::Ptr &pointcloud) {

    cv::Mat gray,timg;

    image.copyTo(this->image);
    this->points = pointcloud;

#if 1
    cv::cvtColor(image,gray,CV_BGR2HLS);
    //gray = planes[2]-planes[1];
    std::vector<cv::Mat> planes;
    cv::split(gray,planes);

    gray = planes[1];
    cv::merge(&planes[0],(size_t)3,timg);
    cv::cvtColor(timg,timg,CV_HSV2BGR);
#else
    timg = image;
#endif

#ifdef DEBUG_VIS
    cv::cvtColor(gray,timg,CV_GRAY2BGR);
#endif

    const int HessianThreshold = 500;
    cv::SURF bug(HessianThreshold);
    bug.extended = false;

    bug(gray,255*cv::Mat::ones(gray.rows,gray.cols,CV_8U),keypoints,descriptors);

    for(size_t i = 0; i < keypoints.size(); i++) {
        cv::KeyPoint kp = keypoints[i];
        PointType pt3d = pointcloud->at(kp.pt.x,kp.pt.y);
        pt3d.rgb = kp.response;
        if(!isnan(pt3d.x)/*&&(kp.response/1000>3)*/) {
            keypoints3D->points.push_back(pt3d);
            map2D3D.insert(std::make_pair<int,int>(i,keypoints3D->points.size()-1));
            map2D3Dinv.insert(std::make_pair<int,int>(keypoints3D->points.size()-1,i));
        }
    }

#ifdef DEBUG_VIS
    plotKeypoints(timg);

    cv::namedWindow("surf");
    cv::imshow("surf",timg);
#endif

    return 0;
}

int main( int argc, char* argv[] )
{
    if( argc < 3 ) 
    {
	usage();
	exit(0);
    }

    string asc_file = argv[1];
    string ply_file = argv[2];
    int sampling = 1;
    float cell_size = 0;
    if( argc >= 4 )
	sampling = lexical_cast<int>( argv[3] );
    if( argc >= 5 )
	cell_size = lexical_cast<float>( argv[4] );

    Environment env;

    Pointcloud::Ptr pc = new Pointcloud();
    env.attachItem( pc.get() );
    env.setFrameNode( pc.get(), env.getRootNode() );
    ifstream asc( asc_file.c_str() );
    pc->readText( asc, sampling, Pointcloud::XYZR );

    if( cell_size > 0 )
    {
#ifdef ENVIRE_USE_CGAL
	Pointcloud::Ptr pc_simp = new Pointcloud();
	env.attachItem( pc_simp.get() );
	env.setFrameNode( pc_simp.get(), env.getRootNode() );

	SimplifyPointcloud::Ptr spc = new SimplifyPointcloud();
	env.attachItem( spc.get() );
	spc->addInput( pc.get() );
	spc->addOutput( pc_simp.get() );
	spc->setSimplifyCellSize( cell_size );

	spc->updateAll();

	pc = pc_simp;
#else
	std::cout << "ignoring cell_size parameter since CGAL is not compiled in." << std::endl;
#endif
    }

    ofstream ply( ply_file.c_str() );
    pc->writePly( ply_file, ply );
}

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

//--------------------------------------------------------------------------------------------------
int main( int argc, char** argv )
{
    // Read in a configuration file
    if ( argc < 2 )
    {
        fprintf( stderr, "No configuration file provided\n" );
        showUsage( argv[ 0 ] );
        return -1;
    }

    initialiseTestPositions();

    //std::string configFilename = Utilities::getDataDir() + std::string( "/" ) + std::string( argv[ 1 ] );
    std::string configFilename( argv[ 1 ] );

    cv::FileStorage configFileStorage;
    configFileStorage.open( configFilename, cv::FileStorage::READ );

    if ( !configFileStorage.isOpened() )
    {
        fprintf( stderr, "Unable to open %s\n", configFilename.c_str() );
        return -1;
    }

    cv::Mat kinectDepthCameraPos;
    cv::Mat kinectDepthCameraRotXYZDeg;
    double kinectDepthFocalLengthPixel;
    int32_t kinectDepthImageWidth;
    int32_t kinectDepthImageHeight;

    cv::Mat kinectRGBCameraPos;
    cv::Mat kinectRGBCameraRotXYZDeg;
    double kinectRGBFocalLengthPixel;
    int32_t kinectRGBImageWidth;
    int32_t kinectRGBImageHeight;

    cv::Mat highResCameraPos;
    cv::Mat highResCameraRotXYZDeg;
    double highResFocalLengthPixel;
    int32_t highResImageWidth;
    int32_t highResImageHeight;

    // The camera poses are defined in a coordinate system with the Kinect depth camera at the
    // origin looking down the +ve z-axis, so z values increase into the image. The x and y axes
    // of the depth camera are aligned with its image plane, so +ve x points to its right, and
    // +ve y points down through the base of the camera. To position the camera in world space, it
    // is rotated 180 degrees about the z axis, the chessboard positions are defined in world space.

    configFileStorage[ "kinectDepthFocalLengthPixel" ] >> kinectDepthFocalLengthPixel;
    configFileStorage[ "kinectDepthImageWidth" ] >> kinectDepthImageWidth;
    configFileStorage[ "kinectDepthImageHeight" ] >> kinectDepthImageHeight;

    configFileStorage[ "kinectRGBCameraPos" ] >> kinectRGBCameraPos;
    configFileStorage[ "kinectRGBCameraRotXYZDeg" ] >> kinectRGBCameraRotXYZDeg;
    configFileStorage[ "kinectRGBFocalLengthPixel" ] >> kinectRGBFocalLengthPixel;
    configFileStorage[ "kinectRGBImageWidth" ] >> kinectRGBImageWidth;
    configFileStorage[ "kinectRGBImageHeight" ] >> kinectRGBImageHeight;

    configFileStorage[ "highResCameraPos" ] >> highResCameraPos;
    configFileStorage[ "highResCameraRotXYZDeg" ] >> highResCameraRotXYZDeg;
    configFileStorage[ "highResFocalLengthPixel" ] >> highResFocalLengthPixel;
    configFileStorage[ "highResImageWidth" ] >> highResImageWidth;
    configFileStorage[ "highResImageHeight" ] >> highResImageHeight;

    // Construct camera matrices
    cv::Mat zeroVec = cv::Mat::zeros( 3, 1, CV_64FC1 );
    cv::Mat kinectDepthCalibMtx = createCameraCalibrationMatrix(
        kinectDepthFocalLengthPixel, kinectDepthImageWidth, kinectDepthImageHeight );
    cv::Mat kinectDepthWorldMtx = createCameraWorldMatrix( zeroVec, zeroVec );

    cv::Mat kinectRGBCalibMtx = createCameraCalibrationMatrix(
        kinectRGBFocalLengthPixel, kinectRGBImageWidth, kinectRGBImageHeight );
    cv::Mat kinectRGBWorldMtx = createCameraWorldMatrix(
        kinectRGBCameraPos, kinectRGBCameraRotXYZDeg );

    cv::Mat highResCalibMtx = createCameraCalibrationMatrix(
        highResFocalLengthPixel, highResImageWidth, highResImageHeight );
    cv::Mat highResWorldMtx = createCameraWorldMatrix(
        highResCameraPos, highResCameraRotXYZDeg );

    // First generate calibration images for the high resolution camera
    for ( uint32_t testPosIdx = 0; testPosIdx < gHighResCalibrationPositions.size(); testPosIdx++ )
    {
        cv::Mat chessboardPoseMtx = createChessboardPoseMatrix( gHighResCalibrationPositions[ testPosIdx ] );

        // Generate an image from the high resolution camera
        cv::Mat highResRgbImage = generateRGBImageOfChessboard( highResWorldMtx, highResCalibMtx,
            highResImageWidth, highResImageHeight, chessboardPoseMtx );

        cv::imwrite( createOutputFilename( "chessboard_highres", testPosIdx + 1, ".png" ), highResRgbImage );
    }

    // Now generate images to identify the relative poses of the cameras
    for ( uint32_t testPosIdx = 0; testPosIdx < gRelativeCalibrationPositions.size(); testPosIdx++ )
    {
        cv::Mat chessboardPoseMtx = createChessboardPoseMatrix( gRelativeCalibrationPositions[ testPosIdx ] );

        // Generate a point cloud from the Kinect
        PointCloud::Ptr pCloud = generatePointCloudOfChessboard( kinectDepthWorldMtx, kinectDepthCalibMtx,
//.........这里部分代码省略.........

void RegionGrowing::fastSeedRegionGrowing(
    pcl::PointCloud<PointNormalT>::Ptr src_points,
    cv::Point2i &seed_index2D, const PointCloud::Ptr cloud,
    const PointNormal::Ptr normals, const PointT seed_pt) {
    if (cloud->empty() || normals->size() != cloud->size()) {
       return;
    }
    cv::Point2f image_index;
    int seed_index = -1;
    if (this->projectPoint3DTo2DIndex(image_index, seed_pt)) {
       seed_index = (static_cast<int>(image_index.x) +
                     (static_cast<int>(image_index.y) * camera_info_->width));
       seed_index2D = cv::Point2i(static_cast<int>(image_index.x),
                                   static_cast<int>(image_index.y));
    } else {
       ROS_ERROR("INDEX IS NAN");
       return;
    }

#ifdef _DEBUG
    cv::Mat test = cv::Mat::zeros(480, 640, CV_8UC3);
    cv::circle(test, image_index, 3, cv::Scalar(0, 255, 0), -1);
    cv::imshow("test", test);
    cv::waitKey(3);
#endif
    
    Eigen::Vector4f seed_point = cloud->points[seed_index].getVector4fMap();
    Eigen::Vector4f seed_normal = normals->points[
       seed_index].getNormalVector4fMap();
    
    std::vector<int> processing_list;
    std::vector<int> labels(static_cast<int>(cloud->size()), -1);

    const int window_size = 3;
    const int wsize = window_size * window_size;
    const int lenght = std::floor(window_size/2);

    processing_list.clear();
    for (int j = -lenght; j <= lenght; j++) {
       for (int i = -lenght; i <= lenght; i++) {
          int index = (seed_index + (j * camera_info_->width)) + i;
          if (index >= 0 && index < cloud->size()) {
             processing_list.push_back(index);
          }
       }
    }
    std::vector<int> temp_list;
    while (true) {
       if (processing_list.empty()) {
          break;
       }
       temp_list.clear();
       for (int i = 0; i < processing_list.size(); i++) {
          int idx = processing_list[i];
          if (labels[idx] == -1) {
             Eigen::Vector4f c = cloud->points[idx].getVector4fMap();
             Eigen::Vector4f n = normals->points[idx].getNormalVector4fMap();
             
             if (this->seedVoxelConvexityCriteria(
                    seed_point, seed_normal, seed_point, c, n, -0.01) == 1) {
                labels[idx] = 1;

                for (int j = -lenght; j <= lenght; j++) {
                   for (int k = -lenght; k <= lenght; k++) {
                      int index = (idx + (j * camera_info_->width)) + k;
                      if (index >= 0 && index < cloud->size()) {
                         temp_list.push_back(index);
                      }
                   }
                }
             }
          }
       }
       processing_list.clear();
       processing_list.insert(processing_list.end(), temp_list.begin(),
                              temp_list.end());
    }
    src_points->clear();
    for (int i = 0; i < labels.size(); i++) {
       if (labels[i] != -1) {
          PointNormalT pt;
          pt.x = cloud->points[i].x;
          pt.y = cloud->points[i].y;
          pt.z = cloud->points[i].z;
          pt.r = cloud->points[i].r;
          pt.g = cloud->points[i].g;
          pt.b = cloud->points[i].b;
          pt.normal_x = normals->points[i].normal_x;
          pt.normal_y = normals->points[i].normal_y;
          pt.normal_z = normals->points[i].normal_z;
          src_points->push_back(pt);
       }
    }
}

int main( int argc, char** argv )
{
    vector<cv::Mat> colorImgs, depthImgs;    // 彩色图和深度图
    vector<Eigen::Isometry3d> poses;         // 相机位姿
    
    ifstream fin("./data/pose.txt");
    if (!fin)
    {
        cerr<<"cannot find pose file"<<endl;
        return 1;
    }
    
    for ( int i=0; i<5; i++ )
    {
        boost::format fmt( "./data/%s/%d.%s" ); //图像文件格式
        colorImgs.push_back( cv::imread( (fmt%"color"%(i+1)%"png").str() ));
        depthImgs.push_back( cv::imread( (fmt%"depth"%(i+1)%"pgm").str(), -1 )); // 使用-1读取原始图像
        
        double data[7] = {0};
        for ( int i=0; i<7; i++ )
        {
            fin>>data[i];
        }
        Eigen::Quaterniond q( data[6], data[3], data[4], data[5] );
        Eigen::Isometry3d T(q);
        T.pretranslate( Eigen::Vector3d( data[0], data[1], data[2] ));
        poses.push_back( T );
    }
    
    // 计算点云并拼接
    // 相机内参 
    double cx = 325.5;
    double cy = 253.5;
    double fx = 518.0;
    double fy = 519.0;
    double depthScale = 1000.0;
    
    cout<<"正在将图像转换为点云..."<<endl;
    
    // 定义点云使用的格式：这里用的是XYZRGB
    typedef pcl::PointXYZRGB PointT; 
    typedef pcl::PointCloud<PointT> PointCloud;
    
    // 新建一个点云
    PointCloud::Ptr pointCloud( new PointCloud ); 
    for ( int i=0; i<5; i++ )
    {
        PointCloud::Ptr current( new PointCloud );
        cout<<"转换图像中: "<<i+1<<endl; 
        cv::Mat color = colorImgs[i]; 
        cv::Mat depth = depthImgs[i];
        Eigen::Isometry3d T = poses[i];
        for ( int v=0; v<color.rows; v++ )
            for ( int u=0; u<color.cols; u++ )
            {
                unsigned int d = depth.ptr<unsigned short> ( v )[u]; // 深度值
                if ( d==0 ) continue; // 为0表示没有测量到
                if ( d >= 7000 ) continue; // 深度太大时不稳定，去掉
                Eigen::Vector3d point; 
                point[2] = double(d)/depthScale; 
                point[0] = (u-cx)*point[2]/fx;
                point[1] = (v-cy)*point[2]/fy; 
                Eigen::Vector3d pointWorld = T*point;
                
                PointT p ;
                p.x = pointWorld[0];
                p.y = pointWorld[1];
                p.z = pointWorld[2];
                p.b = color.data[ v*color.step+u*color.channels() ];
                p.g = color.data[ v*color.step+u*color.channels()+1 ];
                p.r = color.data[ v*color.step+u*color.channels()+2 ];
                current->points.push_back( p );
            }
        // depth filter and statistical removal 
        PointCloud::Ptr tmp ( new PointCloud );
        pcl::StatisticalOutlierRemoval<PointT> statistical_filter;
        statistical_filter.setMeanK(50);
        statistical_filter.setStddevMulThresh(1.0);
        statistical_filter.setInputCloud(current);
        statistical_filter.filter( *tmp );
        (*pointCloud) += *tmp;
    }
    
    pointCloud->is_dense = false;
    cout<<"点云共有"<<pointCloud->size()<<"个点."<<endl;
    
    // voxel filter 
    pcl::VoxelGrid<PointT> voxel_filter; 
    voxel_filter.setLeafSize( 0.01, 0.01, 0.01 );       // resolution 
    PointCloud::Ptr tmp ( new PointCloud );
    voxel_filter.setInputCloud( pointCloud );
    voxel_filter.filter( *tmp );
    tmp->swap( *pointCloud );
    
    cout<<"滤波之后，点云共有"<<pointCloud->size()<<"个点."<<endl;
    
    pcl::io::savePCDFileBinary("map.pcd", *pointCloud );
    return 0;
}

std::vector<GraspHypothesis> Localization::localizeHands(const PointCloud::Ptr& cloud_in, int size_left,
	const std::vector<int>& indices, bool calculates_antipodal, bool uses_clustering)
{		
	double t0 = omp_get_wtime();
	std::vector<GraspHypothesis> hand_list;
	
	if (size_left == 0 || cloud_in->size() == 0)
	{
		//std::cout << "Input cloud is empty!\n";
		//std::cout << size_left << std::endl;
		hand_list.resize(0);
		return hand_list;
	}
	
	// set camera source for all points (0 = left, 1 = right)
	//std::cout << "Generating camera sources for " << cloud_in->size() << " points ...\n";
	Eigen::VectorXi pts_cam_source(cloud_in->size());
	if (size_left == cloud_in->size())
		pts_cam_source << Eigen::VectorXi::Zero(size_left);
	else
		pts_cam_source << Eigen::VectorXi::Zero(size_left), Eigen::VectorXi::Ones(cloud_in->size() - size_left);
		
	// remove NAN points from the cloud
	std::vector<int> nan_indices;
	pcl::removeNaNFromPointCloud(*cloud_in, *cloud_in, nan_indices);

	// reduce point cloud to workspace
	//std::cout << "Filtering workspace ...\n";
	PointCloud::Ptr cloud(new PointCloud);
	filterWorkspace(cloud_in, pts_cam_source, cloud, pts_cam_source);
	//std::cout << " " << cloud->size() << " points left\n";

	// store complete cloud for later plotting
	PointCloud::Ptr cloud_plot(new PointCloud);
	*cloud_plot = *cloud;
	*cloud_ = *cloud;

	// voxelize point cloud
	//std::cout << "Voxelizing point cloud\n";
	double t1_voxels = omp_get_wtime();
	voxelizeCloud(cloud, pts_cam_source, cloud, pts_cam_source, 0.003);
	double t2_voxels = omp_get_wtime() - t1_voxels;
	//std::cout << " Created " << cloud->points.size() << " voxels in " << t2_voxels << " sec\n";

	// plot camera source for each point in the cloud
	if (plots_camera_sources_)
		plot_.plotCameraSource(pts_cam_source, cloud);

	if (uses_clustering)
	{
    //std::cout << "Finding point cloud clusters ... \n";
        
		// Create the segmentation object for the planar model and set all the parameters
		pcl::SACSegmentation<pcl::PointXYZ> seg;
		pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
		pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane(new pcl::PointCloud<pcl::PointXYZ>());
		seg.setOptimizeCoefficients(true);
		seg.setModelType(pcl::SACMODEL_PLANE);
		seg.setMethodType(pcl::SAC_RANSAC);
		seg.setMaxIterations(100);
		seg.setDistanceThreshold(0.01);

		// Segment the largest planar component from the remaining cloud
		seg.setInputCloud(cloud);
		seg.segment(*inliers, *coefficients);
		if (inliers->indices.size() == 0)
		{
			//std::cout << " Could not estimate a planar model for the given dataset." << std::endl;
			hand_list.resize(0);
			return hand_list;
		}
    
    //std::cout << " PointCloud representing the planar component: " << inliers->indices.size()
    //  << " data points." << std::endl;

		// Extract the nonplanar inliers
		pcl::ExtractIndices<pcl::PointXYZ> extract;
		extract.setInputCloud(cloud);
		extract.setIndices(inliers);
		extract.setNegative(true);
		std::vector<int> indices_cluster;
		extract.filter(indices_cluster);
		PointCloud::Ptr cloud_cluster(new PointCloud);
		cloud_cluster->points.resize(indices_cluster.size());
		Eigen::VectorXi cluster_cam_source(indices_cluster.size());
		for (int i = 0; i < indices_cluster.size(); i++)
		{
			cloud_cluster->points[i] = cloud->points[indices_cluster[i]];
			cluster_cam_source[i] = pts_cam_source[indices_cluster[i]];
		}
		cloud = cloud_cluster;
		*cloud_plot = *cloud;
		//std::cout << " PointCloud representing the non-planar component: " << cloud->points.size()
     // << " data points." << std::endl;
	}

	// draw down-sampled and workspace reduced cloud
	cloud_plot = cloud;
  
//.........这里部分代码省略.........

void GraphicEnd::downsamplePointCloud(PointCloud::Ptr& pc_in,PointCloud::Ptr& pc_downsampled)
{
    if(use_voxel)
    {
        pcl::VoxelGrid<pcl::PointXYZRGB> grid;
        grid.setLeafSize(0.05,0.05,0.05);
        grid.setFilterFieldName ("z");
        grid.setFilterLimits (0.0,5.0);

        grid.setInputCloud(pc_in);
        grid.filter(*pc_downsampled);
    }
    else
    {
        int downsamplingStep=8;
        static int j;j=0;
        std::vector<double> xV;
        std::vector<double> yV;
        std::vector<double> zV;
        std::vector<double> rV;
        std::vector<double> gV;
        std::vector<double> bV;

        pc_downsampled.reset(new pcl::PointCloud<pcl::PointXYZRGB> );
        pc_downsampled->points.resize(640*480/downsamplingStep*downsamplingStep);
        for(int r=0;r<480;r=r+downsamplingStep)
        {
            for(int c=0;c<640;c=c+downsamplingStep)
            {
                int nPoints=0;
                xV.resize(downsamplingStep*downsamplingStep);
                yV.resize(downsamplingStep*downsamplingStep);
                zV.resize(downsamplingStep*downsamplingStep);
                rV.resize(downsamplingStep*downsamplingStep);
                gV.resize(downsamplingStep*downsamplingStep);
                bV.resize(downsamplingStep*downsamplingStep);
                
                for(int r2=r;r2<r+downsamplingStep;r2++)
                {
                    for(int c2=c;c2<c+downsamplingStep;c2++)
                    {
                        //Check if the point has valid data
                        if(pcl_isfinite (pc_in->points[r2*640+c2].x) &&
                           pcl_isfinite (pc_in->points[r2*640+c2].y) &&
                           pcl_isfinite (pc_in->points[r2*640+c2].z) &&
                           0.3<pc_in->points[r2*640+c2].z &&
                           pc_in->points[r2*640+c2].z<5)
                        {
                            //Create a vector with the x, y and z coordinates of the square region and RGB info
                            xV[nPoints]=pc_in->points[r2*640+c2].x;
                            yV[nPoints]=pc_in->points[r2*640+c2].y;
                            zV[nPoints]=pc_in->points[r2*640+c2].z;
                            rV[nPoints]=pc_in->points[r2*640+c2].r;
                            gV[nPoints]=pc_in->points[r2*640+c2].g;
                            bV[nPoints]=pc_in->points[r2*640+c2].b;
                            
                            nPoints++;
                        }
                    }
                }
                
                if(nPoints>0)
                {
                    xV.resize(nPoints);
                    yV.resize(nPoints);
                    zV.resize(nPoints);
                    rV.resize(nPoints);
                    gV.resize(nPoints);
                    bV.resize(nPoints);
                    
                    //Compute the mean 3D point and mean RGB value
                    std::sort(xV.begin(),xV.end());
                    std::sort(yV.begin(),yV.end());
                    std::sort(zV.begin(),zV.end());
                    std::sort(rV.begin(),rV.end());
                    std::sort(gV.begin(),gV.end());
                    std::sort(bV.begin(),bV.end());
                    
                    pcl::PointXYZRGB point;
                    point.x=xV[nPoints/2];
                    point.y=yV[nPoints/2];
                    point.z=zV[nPoints/2];
                    point.r=rV[nPoints/2];
                    point.g=gV[nPoints/2];
                    point.b=bV[nPoints/2];
                    
                    //Set the mean point as the representative point of the region
                    pc_downsampled->points[j]=point;
                    j++;
                }
            }
        }
        pc_downsampled->points.resize(j);
        pc_downsampled->width=pc_downsampled->size();
        pc_downsampled->height=1;
    }
}