示例#1
0
/* Normalizes a histogram.
 * sum over all elements of the histogram will be equal to 1.0
 */
inline std::vector<double> normalize_histogram( const std::vector<size_t>& in )
{
    std::vector<double> result;

    if ( in.empty() )
        return result;

    size_t count = std::accumulate( in.begin(), in.end(), size_t() );

    return normalize_histogram( in, count );
}
示例#2
0
/**
  gets observations (histograms) for sampled particles and writes them into an array without receiving a new frame

  D E B U G   F U N C T I O N

  @param sampled_particles: pointer to array with sampled particles
  @param number_of_particles: number of particles
  @param observations: pointer to array of observations (histograms)
*/
void get_observations_without_new_frame ( particle * sampled_particles, int number_of_particles, histogram * observations) {

    int i,x1,x2,y1,y2;
    histogram * tmp = observations;


    // 1) read frame
    read_frame();

    // 2) create hsv image
    convert_bgr2hsv();

    // 3) for every particle: calculate histogram
    for (i=0; i<number_of_particles; i++) {

        // calculate start and end position of region
        x1 = (sampled_particles->x / PF_GRANULARITY)  - ((sampled_particles->s * (( sampled_particles->width - 1) / 2)) / PF_GRANULARITY);
        x2 = (sampled_particles->x / PF_GRANULARITY)  + ((sampled_particles->s * (( sampled_particles->width - 1) / 2)) / PF_GRANULARITY);

        y1 = (sampled_particles->y / PF_GRANULARITY)  - ((sampled_particles->s * (( sampled_particles->height - 1) / 2)) / PF_GRANULARITY);
        y2 = (sampled_particles->y / PF_GRANULARITY)  + ((sampled_particles->s * (( sampled_particles->height - 1) / 2)) / PF_GRANULARITY);

        // correct positions, if needed
        if (x1<0) {
            x1 = 0;
        }
        if (y1<0) {
            y1 = 0;
        }
        if (x2>SIZE_X-1) {
            x2 = SIZE_X - 1;
        }
        if (y2>SIZE_Y-1) {
            y2 = SIZE_Y - 1;
        }

        // calculate histogram
        calc_histogram(hsvImage, x1, y1, x2, y2, tmp);
        //calc_histogram(x1, y1, x2, y2, tmp);

        //4) normalize histogram
        normalize_histogram (tmp);

        // next histogram
        tmp++;
        sampled_particles++;

    }
}
histogram* calc_histogram( PointCloudT::Ptr& cloud )
{
  histogram* histo;
  float* hist;
  int bin;
  float h = 0, s = 0, v = 0;

  histo = (histogram*)malloc( sizeof(histogram) );
  histo->n = NH*NS + NV;
  hist = histo->histo;
  memset( hist, 0, histo->n * sizeof(float) );
  
  for( int i = 0; i < cloud->points.size(); i++ )
  {
    rgb2hsv( (int)cloud->points[i].r, (int)cloud->points[i].g, (int)cloud->points[i].b, h, s, v );
  bin = histo_bin( h, s, v );
  hist[bin] += 1;
  }
  normalize_histogram( histo );
  return histo;
}
int main( int argc, char** argv )
{
  IplImage* hsv_img;
  IplImage** hsv_ref_imgs;
  IplImage* l32f, * l;
  histogram* ref_histo;
  double max;
  int i;

  arg_parse( argc, argv );

  /* compute HSV histogram over all reference image */
  hsv_img = bgr2hsv( in_img );
  hsv_ref_imgs = (IplImage**)malloc( num_ref_imgs * sizeof( IplImage* ) );
  for( i = 0; i < num_ref_imgs; i++ )
    hsv_ref_imgs[i] = bgr2hsv( ref_imgs[i] );
  ref_histo = calc_histogram( hsv_ref_imgs, num_ref_imgs );
  normalize_histogram( ref_histo );

  /* compute likelihood at every pixel in input image */
  fprintf( stderr, "Computing likelihood... " );
  fflush( stderr );
  l32f = likelihood_image( hsv_img, ref_imgs[0]->width,
			   ref_imgs[0]->height, ref_histo );
  fprintf( stderr, "done\n");

  /* convert likelihood image to uchar and display */
  cvMinMaxLoc( l32f, NULL, &max, NULL, NULL, NULL );
  l = cvCreateImage( cvGetSize( l32f ), IPL_DEPTH_8U, 1 );
  cvConvertScale( l32f, l, 255.0 / max, 0 );
  cvNamedWindow( "likelihood", 1 );
  cvShowImage( "likelihood", l );
  cvNamedWindow( "image", 1 );
  cvShowImage( "image", in_img );
  cvWaitKey(0);
}
/** @brief MATLAB Driver.
 **/
void
mexFunction(int nout, mxArray *out[], 
            int nin, const mxArray *in[])
{
  int M,N,S=0,smin=0,K,num_levels=0 ;
  const int* dimensions ;
  const double* P_pt ;
  const double* G_pt ;
  float* descr_pt ;
  float* buffer_pt ;
  float sigma0 ;
  float magnif = 3.0f ; /* Spatial bin extension factor. */
  int NBP = 4 ;         /* Number of bins for one spatial direction (even). */
  int NBO = 8 ;         /* Number of bins for the ortientation. */
  int mode = NOSCALESPACE ;
  int buffer_size=0;

  enum {IN_G=0,IN_P,IN_SIGMA0,IN_S,IN_SMIN} ;
  enum {OUT_L=0} ;

  /* ------------------------------------------------------------------
  **                                                Check the arguments
  ** --------------------------------------------------------------- */ 
 
  if (nin < 3) {
    mexErrMsgTxt("At least three arguments are required") ;
  } else if (nout > 1) {
    mexErrMsgTxt("Too many output arguments.");
  }
                
  if( !uIsRealScalar(in[IN_SIGMA0]) ) {
    mexErrMsgTxt("SIGMA0 should be a real scalar") ;
  }
        
  if(!mxIsDouble(in[IN_G]) ||
     mxGetNumberOfDimensions(in[IN_G]) > 3) {
    mexErrMsgTxt("G should be a real matrix or 3-D array") ;
  }
  
  sigma0 = (float) *mxGetPr(in[IN_SIGMA0]) ;
  
  dimensions = mxGetDimensions(in[IN_G]) ;
  M = dimensions[0] ;
  N = dimensions[1] ;
  G_pt = mxGetPr(in[IN_G]) ;
  
  P_pt = mxGetPr(in[IN_P]) ;    
  K = mxGetN(in[IN_P]) ;
  
  if( !uIsRealMatrix(in[IN_P],-1,-1)) {
    mexErrMsgTxt("P should be a real matrix") ;
  }

  if ( mxGetM(in[IN_P])  == 4) {
    /* Standard (scale space) mode */ 
    mode = SCALESPACE ;
    num_levels = dimensions[2] ;
    
    if(nin < 5) {
      mexErrMsgTxt("Five arguments are required in standard mode") ;
    }
    
    if( !uIsRealScalar(in[IN_S]) ) {
      mexErrMsgTxt("S should be a real scalar") ;
    }
    
    if( !uIsRealScalar(in[IN_SMIN]) ) {
      mexErrMsgTxt("SMIN should be a real scalar") ;
    }
    
    if( !uIsRealMatrix(in[IN_P],4,-1)) {
      mexErrMsgTxt("When the e mode P should be a 4xK matrix.") ;
    }
    
    S = (int)(*mxGetPr(in[IN_S])) ;
    smin = (int)(*mxGetPr(in[IN_SMIN])) ;
    
  } else if (  mxGetM(in[IN_P])  == 3 ) {
    mode = NOSCALESPACE ;
    num_levels = 1 ;
    S      = 1 ;
    smin   = 0 ;
  } else {
    mexErrMsgTxt("P should be either a 3xK or a 4xK matrix.") ;
  }

  /* Parse the property-value pairs */
  {
    char str [80] ;
    int arg = (mode == SCALESPACE) ? IN_SMIN + 1 : IN_SIGMA0 + 1 ;

    while(arg < nin) {
      int k ;

      if( !uIsString(in[arg],-1) ) {
        mexErrMsgTxt("The first argument in a property-value pair"
                     " should be a string") ;
      }
      mxGetString(in[arg], str, 80) ;

#ifdef WINDOWS      
      for(k = 0 ; properties[k] && strcmpi(str, properties[k]) ; ++k) ;
#else
      for(k = 0 ; properties[k] && strcasecmp(str, properties[k]) ; ++k) ;
#endif

      switch (k) {
      case PROP_NBP:
        if( !uIsRealScalar(in[arg+1]) ) {
          mexErrMsgTxt("'NumSpatialBins' should be real scalar") ;
        }
        NBP = (int) *mxGetPr(in[arg+1]) ;
        if( NBP <= 0 || (NBP & 0x1) ) {
          mexErrMsgTxt("'NumSpatialBins' must be positive and even") ;
        }
        break ;

      case PROP_NBO:
        if( !uIsRealScalar(in[arg+1]) ) {
          mexErrMsgTxt("'NumOrientBins' should be a real scalar") ;
        }
        NBO = (int) *mxGetPr(in[arg+1]) ;
        if( NBO <= 0 ) {
          mexErrMsgTxt("'NumOrientlBins' must be positive") ;
        }
        break ;

      case PROP_MAGNIF:
        if( !uIsRealScalar(in[arg+1]) ) {
          mexErrMsgTxt("'Magnif' should be a real scalar") ;
        }
        magnif = (float) *mxGetPr(in[arg+1]) ;
        if( magnif <= 0 ) {
          mexErrMsgTxt("'Magnif' must be positive") ;
        }
        break ;

      case PROP_UNKNOWN:
        mexErrMsgTxt("Property unknown.") ;
        break ;
      }
      arg += 2 ;
    }
  }
  
  /* -----------------------------------------------------------------
   *                                   Pre-compute gradient and angles
   * -------------------------------------------------------------- */
  /* Alloc two buffers and make sure their size is multiple of 128 for
   * better alignment (used also by the Altivec code below.)
   */
  buffer_size = (M*N*num_levels + 0x7f) & (~ 0x7f) ;
  buffer_pt = (float*) mxMalloc( sizeof(float) * 2 * buffer_size ) ;
  descr_pt  = (float*) mxCalloc( NBP*NBP*NBO*K,  sizeof(float)  ) ;

  {
    /* Offsets to move in the scale space. */
    const int yo = 1 ;
    const int xo = M ;
    const int so = M*N ;
    int x,y,s ;

#define at(x,y) (*(pt + (x)*xo + (y)*yo))

    /* Compute the gradient */
    for(s = 0 ; s < num_levels ; ++s) {
      const double* pt = G_pt + s*so ;
      for(x = 1 ; x < N-1 ; ++x) {
        for(y = 1 ; y < M-1 ; ++y) {
          float Dx = 0.5 * ( at(x+1,y) - at(x-1,y) ) ;
          float Dy = 0.5 * ( at(x,y+1) - at(x,y-1) ) ;
          buffer_pt[(x*xo+y*yo+s*so) + 0          ] = Dx ;
          buffer_pt[(x*xo+y*yo+s*so) + buffer_size] = Dy ;
        }
      }
    }
    
    /* Compute angles and modules */
    {
      float* pt = buffer_pt ;
      int j = 0 ;
      while (j < N*M*num_levels) {

#if defined( MACOSX ) && defined( __ALTIVEC__ )
        if( ((unsigned int)pt & 0x7) == 0 && j+3 < N*M*num_levels ) {
          /* If aligned to 128 bit and there are at least 4 pixels left */
          float4 a, b, c, d ;
          a.vec = vec_ld(0,(vector float*)(pt              )) ;
          b.vec = vec_ld(0,(vector float*)(pt + buffer_size)) ;
          c.vec = vatan2f(b.vec,a.vec) ;
          a.x[0] = a.x[0]*a.x[0]+b.x[0]*b.x[0] ;
          a.x[1] = a.x[1]*a.x[1]+b.x[1]*b.x[1] ;
          a.x[2] = a.x[2]*a.x[2]+b.x[2]*b.x[2] ;
          a.x[3] = a.x[3]*a.x[3]+b.x[3]*b.x[3] ;
          d.vec = vsqrtf(a.vec) ;
          vec_st(c.vec,0,(vector float*)(pt + buffer_size)) ;
          vec_st(d.vec,0,(vector float*)(pt              )) ;
          j += 4 ;
          pt += 4 ;
        } else {
#endif
          float Dx = *(pt              ) ;
          float Dy = *(pt + buffer_size) ;
          *(pt              ) = sqrtf(Dx*Dx + Dy*Dy) ;
          if (*pt > 0) 
            *(pt + buffer_size) = atan2f(Dy, Dx) ;
          else
            *(pt + buffer_size) = 0 ;
          j += 1 ;
          pt += 1 ;
#if defined( MACOSX ) && defined( __ALTIVEC__ )
        }
#endif

      }
    }
  }

  /* -----------------------------------------------------------------
   *                                                        Do the job
   * -------------------------------------------------------------- */ 
  if(K > 0) {    
    int p ;

    /* Offsets to move in the buffer */
    const int yo = 1 ;
    const int xo = M ;
    const int so = M*N ;

    /* Offsets to move in the descriptor. */
    /* Use Lowe's convention. */
    const int binto = 1 ;
    const int binyo = NBO * NBP ;
    const int binxo = NBO ;
    const int bino  = NBO * NBP * NBP ;

    for(p = 0 ; p < K ; ++p, descr_pt += bino) {
      /* The SIFT descriptor is a  three dimensional histogram of the position
       * and orientation of the gradient.  There are NBP bins for each spatial
       * dimesions and NBO  bins for the orientation dimesion,  for a total of
       * NBP x NBP x NBO bins.
       *
       * The support  of each  spatial bin  has an extension  of SBP  = 3sigma
       * pixels, where sigma is the scale  of the keypoint.  Thus all the bins
       * together have a  support SBP x NBP pixels wide  . Since weighting and
       * interpolation of  pixel is used, another  half bin is  needed at both
       * ends of  the extension. Therefore, we  need a square window  of SBP x
       * (NBP + 1) pixels. Finally, since the patch can be arbitrarly rotated,
       * we need to consider  a window 2W += sqrt(2) x SBP  x (NBP + 1) pixels
       * wide.
       */      
      const float x = (float) *P_pt++ ;
      const float y = (float) *P_pt++ ;
      const float s = (float) (mode == SCALESPACE) ? (*P_pt++) : 0.0 ;
      const float theta0 = (float) *P_pt++ ;

      const float st0 = sinf(theta0) ;
      const float ct0 = cosf(theta0) ;
      const int xi = (int) floor(x+0.5) ; /* Round-off */
      const int yi = (int) floor(y+0.5) ;
      const int si = (int) floor(s+0.5) - smin ;
      const float sigma = sigma0 * powf(2, s / S) ;
      const float SBP = magnif * sigma ;
      const int W = (int) floor( sqrt(2.0) * SBP * (NBP + 1) / 2.0 + 0.5) ;      
      int bin ;
      int dxi ;
      int dyi ;
      const float* pt ;
      float* dpt ;
      
      /* Check that keypoints are within bounds . */

      if(xi < 0   || 
         xi > N-1 || 
         yi < 0   || 
         yi > M-1 ||
         ((mode==SCALESPACE) && 
          (si < 0   ||
           si > dimensions[2]-1) ) )
        continue ;

      /* Center the scale space and the descriptor on the current keypoint. 
       * Note that dpt is pointing to the bin of center (SBP/2,SBP/2,0).
       */
      pt  = buffer_pt + xi*xo + yi*yo + si*so ;
      dpt = descr_pt + (NBP/2) * binyo + (NBP/2) * binxo ;
     
#define atd(dbinx,dbiny,dbint) (*(dpt + (dbint)*binto + (dbiny)*binyo + (dbinx)*binxo))
      
      /*
       * Process each pixel in the window and in the (1,1)-(M-1,N-1)
       * rectangle.
       */
      for(dxi = max(-W, 1-xi) ; dxi <= min(+W, N-2-xi) ; ++dxi) {
        for(dyi = max(-W, 1-yi) ; dyi <= min(+W, M-2-yi) ; ++dyi) {

          /* Compute the gradient. */
          float mod   = *(pt + dxi*xo + dyi*yo + 0           ) ;
          float angle = *(pt + dxi*xo + dyi*yo + buffer_size ) ;
#ifdef LOWE_COMPATIBLE         
          float theta = fast_mod(-angle + theta0) ;
#else
          float theta = fast_mod(angle - theta0) ;
#endif
          /* Get the fractional displacement. */
          float dx = ((float)(xi+dxi)) - x;
          float dy = ((float)(yi+dyi)) - y;

          /* Get the displacement normalized w.r.t. the keypoint orientation
           * and extension. */
          float nx = ( ct0 * dx + st0 * dy) / SBP ;
          float ny = (-st0 * dx + ct0 * dy) / SBP ; 
          float nt = NBO * theta / (2*M_PI) ;

          /* Get the gaussian weight of the sample. The gaussian window
           * has a standard deviation of NBP/2. Note that dx and dy are in
           * the normalized frame, so that -NBP/2 <= dx <= NBP/2. */
          const float wsigma = NBP/2 ;
          float win =  expf(-(nx*nx + ny*ny)/(2.0 * wsigma * wsigma)) ;

          /* The sample will be distributed in 8 adijacient bins. 
           * Now we get the ``lower-left'' bin. */
          int binx = fast_floor( nx - 0.5 ) ;
          int biny = fast_floor( ny - 0.5 ) ;
          int bint = fast_floor( nt ) ;
          float rbinx = nx - (binx+0.5) ;
          float rbiny = ny - (biny+0.5) ;
          float rbint = nt - bint ;
          int dbinx ;
          int dbiny ;
          int dbint ;

          /* Distribute the current sample into the 8 adijacient bins. */
          for(dbinx = 0 ; dbinx < 2 ; ++dbinx) {
            for(dbiny = 0 ; dbiny < 2 ; ++dbiny) {
              for(dbint = 0 ; dbint < 2 ; ++dbint) {
                
                if( binx+dbinx >= -(NBP/2) &&
                    binx+dbinx <   (NBP/2) &&
                    biny+dbiny >= -(NBP/2) &&
                    biny+dbiny <   (NBP/2) ) {
                  float weight = win 
                    * mod 
                    * fabsf(1 - dbinx - rbinx)
                    * fabsf(1 - dbiny - rbiny)
                    * fabsf(1 - dbint - rbint) ;

                      atd(binx+dbinx, biny+dbiny, (bint+dbint) % NBO) += weight ;
                }
              }            
            }
          }
        }  
      }


      {
        /* Normalize the histogram to L2 unit length. */        
        normalize_histogram(descr_pt, descr_pt + NBO*NBP*NBP) ;
        
        /* Truncate at 0.2. */
        for(bin = 0; bin < NBO*NBP*NBP ; ++bin) {
          if (descr_pt[bin] > 0.2) descr_pt[bin] = 0.2;
        }
        
        /* Normalize again. */
        normalize_histogram(descr_pt, descr_pt + NBO*NBP*NBP) ;
      }
    }
  }

  /* Restore pointer to the beginning of the descriptors. */
  descr_pt -= NBO*NBP*NBP*K ;

  {
    int k ;
    double* L_pt ;
    out[OUT_L] = mxCreateDoubleMatrix(NBP*NBP*NBO, K, mxREAL) ;
    L_pt = mxGetPr(out[OUT_L]) ;
    for(k = 0 ; k < NBP*NBP*NBO*K ; ++k) {
      L_pt[k] = descr_pt[k] ;
    }
  }

  mxFree(descr_pt) ;  
  mxFree(buffer_pt) ;
}
int main (int argc, char** argv)
{

  //ROS Initialization
  ros::init(argc, argv, "detecting_people");
  ros::NodeHandle nh;
  ros::Rate rate(13);

  ros::Subscriber state_sub = nh.subscribe("followme_state", 5, &stateCallback);
  ros::Publisher people_pub = nh.advertise<frmsg::people>("followme_people", 5);
  frmsg::people pub_people_;

  CloudConverter* cc_ = new CloudConverter();

  while (!cc_->ready_xyzrgb_)
  {
    ros::spinOnce();
    rate.sleep();
    if (!ros::ok())
    {
      printf("Terminated by Control-c.\n");
      return -1;
    }
  }

  // Input parameter from the .yaml
  std::string package_path_ = ros::package::getPath("detecting_people") + "/";
  cv::FileStorage* fs_ = new cv::FileStorage(package_path_ + "parameters.yml", cv::FileStorage::READ);

  // Algorithm parameters:
  std::string svm_filename = package_path_ + "trainedLinearSVMForPeopleDetectionWithHOG.yaml";
  std::cout << svm_filename << std::endl;

  float min_confidence = -1.5;
  float min_height = 1.3;
  float max_height = 2.3;
  float voxel_size = 0.06;
  Eigen::Matrix3f rgb_intrinsics_matrix;
  rgb_intrinsics_matrix << 525, 0.0, 319.5, 0.0, 525, 239.5, 0.0, 0.0, 1.0; // Kinect RGB camera intrinsics
  
  // Read if some parameters are passed from command line:
  pcl::console::parse_argument (argc, argv, "--svm", svm_filename);
  pcl::console::parse_argument (argc, argv, "--conf", min_confidence);
  pcl::console::parse_argument (argc, argv, "--min_h", min_height);
  pcl::console::parse_argument (argc, argv, "--max_h", max_height);


  // Read Kinect live stream:
  PointCloudT::Ptr cloud_people (new PointCloudT);
  cc_->ready_xyzrgb_ = false;
  while ( !cc_->ready_xyzrgb_ )
  {
    ros::spinOnce();
    rate.sleep();
  }
  pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr cloud = cc_->msg_xyzrgb_;

  // Display pointcloud:
  pcl::visualization::PointCloudColorHandlerRGBField<PointT> rgb(cloud);
  viewer.addPointCloud<PointT> (cloud, rgb, "input_cloud");
  viewer.setCameraPosition(0,0,-2,0,-1,0,0);

  // Add point picking callback to viewer:
  struct callback_args cb_args;
  PointCloudT::Ptr clicked_points_3d (new PointCloudT);
  cb_args.clicked_points_3d = clicked_points_3d;
  cb_args.viewerPtr = pcl::visualization::PCLVisualizer::Ptr(&viewer);
  viewer.registerPointPickingCallback (pp_callback, (void*)&cb_args);
  std::cout << "Shift+click on three floor points, then press 'Q'..." << std::endl;

  // Spin until 'Q' is pressed:
  viewer.spin();
  std::cout << "done." << std::endl;
  
  //cloud_mutex.unlock ();    

  // Ground plane estimation:
  Eigen::VectorXf ground_coeffs;
  ground_coeffs.resize(4);
  std::vector<int> clicked_points_indices;
  for (unsigned int i = 0; i < clicked_points_3d->points.size(); i++)
    clicked_points_indices.push_back(i);
  pcl::SampleConsensusModelPlane<PointT> model_plane(clicked_points_3d);
  model_plane.computeModelCoefficients(clicked_points_indices,ground_coeffs);
  std::cout << "Ground plane: " << ground_coeffs(0) << " " << ground_coeffs(1) << " " << ground_coeffs(2) << " " << ground_coeffs(3) << std::endl;

  // Initialize new viewer:
  pcl::visualization::PCLVisualizer viewer("PCL Viewer");          // viewer initialization
  viewer.setCameraPosition(0,0,-2,0,-1,0,0);

  // Create classifier for people detection:  
  pcl::people::PersonClassifier<pcl::RGB> person_classifier;
  person_classifier.loadSVMFromFile(svm_filename);   // load trained SVM

  // People detection app initialization:
  pcl::people::GroundBasedPeopleDetectionApp<PointT> people_detector;    // people detection object
  people_detector.setVoxelSize(voxel_size);                        // set the voxel size
  people_detector.setIntrinsics(rgb_intrinsics_matrix);            // set RGB camera intrinsic parameters
  people_detector.setClassifier(person_classifier);                // set person classifier
  people_detector.setHeightLimits(min_height, max_height);         // set person classifier
//  people_detector.setSensorPortraitOrientation(true);             // set sensor orientation to vertical

  // For timing:
  static unsigned count = 0;
  static double last = pcl::getTime ();
  
  int people_count = 0;
  histogram* first_hist;

  int max_people_num = (int)fs_->getFirstTopLevelNode()["max_people_num"];
  // Main loop:
  while (!viewer.wasStopped() && ros::ok() )
  {
    if ( current_state == 1 )
    {
      if ( cc_->ready_xyzrgb_ )    // if a new cloud is available
      {
    //    std::cout << "In state 1!!!!!!!!!!" << std::endl;

        std::vector<float> x;
        std::vector<float> y;
        std::vector<float> depth;

        cloud = cc_->msg_xyzrgb_;
        PointCloudT::Ptr cloud_new(new PointCloudT(*cloud));
        cc_->ready_xyzrgb_ = false;

        // Perform people detection on the new cloud:
        std::vector<pcl::people::PersonCluster<PointT> > clusters;   // vector containing persons clusters
        people_detector.setInputCloud(cloud_new);
        people_detector.setGround(ground_coeffs);                    // set floor coefficients
        people_detector.compute(clusters);                           // perform people detection

        ground_coeffs = people_detector.getGround();                 // get updated floor coefficients

        // Draw cloud and people bounding boxes in the viewer:
        viewer.removeAllPointClouds();
        viewer.removeAllShapes();
        pcl::visualization::PointCloudColorHandlerRGBField<PointT> rgb(cloud);
        viewer.addPointCloud<PointT> (cloud, rgb, "input_cloud");
        unsigned int k = 0;
        std::vector<pcl::people::PersonCluster<PointT> >::iterator it;
        std::vector<pcl::people::PersonCluster<PointT> >::iterator it_min;

        float min_z = 10.0;

        float histo_dist_min = 2.0;
        int id = -1;

        for(it = clusters.begin(); it != clusters.end(); ++it)
        {
          if(it->getPersonConfidence() > min_confidence)             // draw only people with confidence above a threshold
          {

            x.push_back((it->getTCenter())[0]);
            y.push_back((it->getTCenter())[1]);
            depth.push_back(it->getDistance());
            // draw theoretical person bounding box in the PCL viewer:
            /*pcl::copyPointCloud( *cloud, it->getIndices(), *cloud_people);
            if ( people_count == 0 )
            {
              first_hist = calc_histogram_a( cloud_people );
              people_count++;
              it->drawTBoundingBox(viewer, k);
            }
            else if ( people_count <= 10 )
            {
              histogram* hist_tmp = calc_histogram_a( cloud_people );
              add_hist( first_hist, hist_tmp );
              it->drawTBoundingBox(viewer, k);
              people_count++;
            }*/
            pcl::copyPointCloud( *cloud, it->getIndices(), *cloud_people);
            if ( people_count < 11 )
            {
              if ( it->getDistance() < min_z )
              {
                it_min = it;
                min_z = it->getDistance();
              }
            }
            else if ( people_count == 11 )
            {
              normalize_histogram( first_hist );
              people_count++;
              histogram* hist_tmp = calc_histogram( cloud_people );
              float tmp = histo_dist_sq( first_hist, hist_tmp );
              std::cout << "The histogram distance is " << tmp << std::endl;
              histo_dist_min = tmp;
              it_min = it;
              id = k;
            }
            else
            {
              histogram* hist_tmp = calc_histogram( cloud_people );
              float tmp = histo_dist_sq( first_hist, hist_tmp );
              std::cout << "The histogram distance is " << tmp << std::endl;
              if ( tmp < histo_dist_min )
              {
                histo_dist_min = tmp;
                it_min = it;
                id = k;
              }
            }
            k++;
            //std::cout << "The data of the center is " << cloud->points [(cloud->width >> 1) * (cloud->height + 1)].x << "  " << cloud->points [(cloud->width >> 1) * (cloud->height + 1)].y << " " << cloud->points [(cloud->width >> 1) * (cloud->height + 1)].z << std::endl;
            //std::cout << "The size of the people cloud is " <<  cloud_people->points.size() << std::endl;
            std::cout << "The " << k << " person's distance is " << it->getDistance() << std::endl;
          }
        }
        pub_people_.x = x;
        pub_people_.y = y;
        pub_people_.depth = depth;
        if ( k > 0 )
        {
          if ( people_count <= 11 )
          {
            pcl::copyPointCloud( *cloud, it_min->getIndices(), *cloud_people);
            if ( people_count == 0)
            {
              first_hist = calc_histogram_a( cloud_people );
              people_count++;
              it_min->drawTBoundingBox(viewer, 1);
            }
            else if ( people_count < 11 )
            {
              histogram* hist_tmp = calc_histogram_a( cloud_people );
              add_hist( first_hist, hist_tmp );
              it_min->drawTBoundingBox(viewer, 1);
              people_count++;
            }
          }
          else
          {
            pub_people_.id = k-1;
            if ( histo_dist_min < 1.3 )
            {
              it_min->drawTBoundingBox(viewer, 1);
              std::cout << "The minimum distance of the histogram is " << histo_dist_min << std::endl;
              std::cout << "The vector is " << it_min->getTCenter() << std::endl << "while the elements are " << (it_min->getTCenter())[0] << " " << (it_min->getTCenter())[1] << std::endl;
            }
            else
            {
              pub_people_.id = -1;
            }
          }
        }
        else
        {
          pub_people_.id = -1;
        }
        pub_people_.header.stamp = ros::Time::now();
        people_pub.publish(pub_people_);
        
        std::cout << k << " people found" << std::endl;
        viewer.spinOnce();
        ros::spinOnce();

        // Display average framerate:
        if (++count == 30)
        {
          double now = pcl::getTime ();
          std::cout << "Average framerate: " << double(count)/double(now - last) << " Hz" <<  std::endl;
          count = 0;
          last = now;
        }
        //cloud_mutex.unlock ();
      }
    }
    else
    {
      viewer.spinOnce();
      ros::spinOnce();
      // std::cout << "In state 0!!!!!!!!!" << std::endl;
    }
  }

  return 0;
}
示例#7
0
std::vector<double> create_normalized_histogram( iterator begin, iterator end, size_t num_buckets )
{
    return normalize_histogram( create_histogram( begin, end, num_buckets ) );
}
示例#8
0
std::vector<double> create_normalized_histogram( iterator begin, iterator end, size_t num_buckets, typename std::iterator_traits<iterator>::value_type min, typename std::iterator_traits<iterator>::value_type max )
{
    return normalize_histogram( create_histogram( begin, end, num_buckets, min, max ) );
}