コード例 #1
0
ファイル: extract_indices.hpp プロジェクト: nttputus/PCL
template <typename PointT> void
pcl::ExtractIndices<PointT>::applyFilter (PointCloud &output)
{
  if (indices_->empty () || input_->points.empty ())
  {
    output.width = output.height = 0;
    output.points.clear ();
    // If negative, copy all the data
    if (negative_)
      output = *input_;
    return;
  }

  // If the set of indices equals the number of points in the input dataset
  if (indices_->size () == (input_->width * input_->height) ||
      indices_->size () == input_->points.size ())
  {
    // If negative, then return an empty cloud
    if (negative_)
    {
      output.width = output.height = 0;
      output.points.clear ();
    }
    // else, we need to return all points
    else
      output = *input_;
    return;
  }

  // We have a set of indices which is a subset of cloud, so let's start processing
  if (negative_)
  {
    // Prepare a vector holding all indices
    std::vector<int> all_indices (input_->points.size ());
    for (size_t i = 0; i < all_indices.size (); ++i)
      all_indices[i] = i;

    std::vector<int> indices = *indices_;
    std::sort (indices.begin (), indices.end ());

    // Get the diference
    std::vector<int> remaining_indices;
    set_difference (all_indices.begin (), all_indices.end (), indices.begin (), indices.end (),
                    inserter (remaining_indices, remaining_indices.begin ()));

    copyPointCloud (*input_, remaining_indices, output);
  }
  else
    copyPointCloud (*input_, *indices_, output);
}
コード例 #2
0
ファイル: io.hpp プロジェクト: 87west/pcl
template <typename PointInT, typename PointOutT> void
pcl::copyPointCloud (const pcl::PointCloud<PointInT> &cloud_in,
                     const pcl::PointIndices &indices,
                     pcl::PointCloud<PointOutT> &cloud_out)
{
  copyPointCloud (cloud_in, indices.indices, cloud_out);
}
コード例 #3
0
 void Planar_filter::setInputCloud(PointCloudT::Ptr cloud)
 {
     copyPointCloud(*cloud,*_cloud);
     //this->Downsample_cloud();
     
       Downsample_cloud();
 }
コード例 #4
0
ファイル: extract_indices.hpp プロジェクト: 2php/pcl
template <typename PointT> void
pcl::ExtractIndices<PointT>::applyFilter (PointCloud &output)
{
  std::vector<int> indices;
  if (keep_organized_)
  {
    bool temp = extract_removed_indices_;
    extract_removed_indices_ = true;
    applyFilterIndices (indices);
    extract_removed_indices_ = temp;

    output = *input_;
    std::vector<pcl::PCLPointField> fields;
    pcl::for_each_type<FieldList> (pcl::detail::FieldAdder<PointT> (fields));
    for (int rii = 0; rii < static_cast<int> (removed_indices_->size ()); ++rii)  // rii = removed indices iterator
    {
      uint8_t* pt_data = reinterpret_cast<uint8_t*> (&output.points[(*removed_indices_)[rii]]);
      for (int fi = 0; fi < static_cast<int> (fields.size ()); ++fi)  // fi = field iterator
        memcpy (pt_data + fields[fi].offset, &user_filter_value_, sizeof (float));
    }
    if (!pcl_isfinite (user_filter_value_))
      output.is_dense = false;
  }
  else
  {
    applyFilterIndices (indices);
    copyPointCloud (*input_, indices, output);
  }
}
コード例 #5
0
ファイル: supervoxel_clustering.hpp プロジェクト: 4ker/pcl
template <typename PointT> typename pcl::PointCloud<PointT>::Ptr
pcl::SupervoxelClustering<PointT>::getVoxelCentroidCloud () const
{
  typename PointCloudT::Ptr centroid_copy (new PointCloudT);
  copyPointCloud (*voxel_centroid_cloud_, *centroid_copy);
  return centroid_copy;
}
コード例 #6
0
void 
RGBID_SLAM::VisualizationManager::getPointCloud(pcl::PointCloud<pcl::PointXYZRGB>::Ptr whole_point_cloud)
{  
  IdToPoseMap::iterator id2pose_map_it;        
  IdToPointCloudMap::const_iterator id2pointcloud_map_it;
  
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr aux_point_cloud;  
  aux_point_cloud.reset(new pcl::PointCloud<pcl::PointXYZRGB>);
  
  for (id2pose_map_it = id2pose_map_.begin(); 
       id2pose_map_it != id2pose_map_.end();
       id2pose_map_it++)
  {
    int kf_id = (*id2pose_map_it).first;
    Eigen::Affine3d pose = (*id2pose_map_it).second;
    
    id2pointcloud_map_it = id2pointcloud_map_.find(kf_id);
    
    if (id2pointcloud_map_it != id2pointcloud_map_.end())
    {
      aux_point_cloud->clear();
      copyPointCloud((*id2pointcloud_map_it).second, aux_point_cloud);  
      (*id2pointcloud_map_it).second->clear();
      alignPointCloud(aux_point_cloud, pose.linear(), pose.translation());   
      *whole_point_cloud += *aux_point_cloud;
    }          
  }  
  
  
}
コード例 #7
0
void laserCloudSub(const sensor_msgs::PointCloud2ConstPtr &msg)
{
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr color_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr laser_cloud(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PCLPointCloud2 pcl_pc2;
    pcl_conversions::toPCL(*msg, pcl_pc2);
    pcl::fromPCLPointCloud2(pcl_pc2, *laser_cloud);
    copyPointCloud(*laser_cloud, *color_cloud);
	addCloud(color_cloud);
}
コード例 #8
0
ファイル: random_sample.hpp プロジェクト: KanzhiWu/pcl
template<typename PointT> void
pcl::RandomSample<PointT>::applyFilter (PointCloud &output)
{
  std::vector<int> indices;
  if (keep_organized_)
  {
    bool temp = extract_removed_indices_;
    extract_removed_indices_ = true;
    applyFilter (indices);
    extract_removed_indices_ = temp;
    copyPointCloud (*input_, output);
    // Get X, Y, Z fields
    std::vector<sensor_msgs::PointField> fields;
    pcl::getFields (*input_, fields);
    std::vector<size_t> offsets;
    for (size_t i = 0; i < fields.size (); ++i)
    {
      if (fields[i].name == "x" ||
          fields[i].name == "y" ||
          fields[i].name == "z")
        offsets.push_back (fields[i].offset);
    }
    // For every "removed" point, set the x,y,z fields to user_filter_value_
    const static float user_filter_value = user_filter_value_;
    for (size_t rii = 0; rii < removed_indices_->size (); ++rii)
    {
      uint8_t* pt_data = reinterpret_cast<uint8_t*> (&output[(*removed_indices_)[rii]]);
      for (size_t i = 0; i < offsets.size (); ++i)
      {
        memcpy (pt_data + offsets[i], &user_filter_value, sizeof (float));
      }
      if (!pcl_isfinite (user_filter_value_))
        output.is_dense = false;
    }
  }
  else
  {
    output.is_dense = true;
    applyFilter (indices);
    copyPointCloud (*input_, indices, output);
  }
}
コード例 #9
0
ファイル: main.cpp プロジェクト: S3BASTI3N/ComputerVision2PCL
pcl::PointCloud<pcl::PointNormal>::Ptr computeNormals(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud) {
    pcl::PointCloud<pcl::PointNormal>::Ptr cloud_normals (new pcl::PointCloud<pcl::PointNormal>); // Output datasets
    pcl::IntegralImageNormalEstimation<pcl::PointXYZ, pcl::PointNormal> ne;
    ne.setNormalEstimationMethod( ne.AVERAGE_3D_GRADIENT);
    ne.setMaxDepthChangeFactor(0.02f);
    ne.setNormalSmoothingSize(10.0f);
    ne.setInputCloud(cloud);
    ne.compute(*cloud_normals);
    copyPointCloud(*cloud, *cloud_normals);
    return cloud_normals;
}
コード例 #10
0
ファイル: PassThrough.cpp プロジェクト: mlepicka/PCL
void PassThrough::applyFilter (pcl::PointCloud<PointXYZSIFT>::Ptr input, pcl::PointCloud<PointXYZSIFT> &output, std::string filter_field_name, float min, float max, bool negative)
{
    CLOG(LTRACE) << "applyFilter() " << filter_field_name;
    output.header = input->header;
    output.sensor_origin_ = input->sensor_origin_;
    output.sensor_orientation_ = input->sensor_orientation_;

    std::vector<int> indices;

    output.is_dense = true;
    applyFilterIndices (indices, input, filter_field_name, min, max, negative);
    CLOG(LTRACE)<< "Number of indices: "<< indices.size() <<endl;
    copyPointCloud (*input, indices, output);

}
コード例 #11
0
    void
    UniformSamplingWrapper::compute(PointCloudInPtr input, PointCloudOut &output)
    {
      float keypoint_separation = 0.01f;

      IndicesPtr indices (new std::vector<int>());
      PointCloud<int> leaves;
      UniformSampling<PointNormal> us;
      us.setRadiusSearch(keypoint_separation);
      us.setInputCloud(input);
      us.compute(leaves);

      // Copy point cloud and return
      indices->assign(leaves.points.begin(), leaves.points.end()); // can't use operator=, probably because of different allocators
      copyPointCloud(*input, *indices, output);
    }
コード例 #12
0
ファイル: supervoxel_clustering.hpp プロジェクト: 4ker/pcl
template <typename PointT> pcl::PointCloud<pcl::PointXYZL>::Ptr
pcl::SupervoxelClustering<PointT>::getLabeledVoxelCloud () const
{
  pcl::PointCloud<pcl::PointXYZL>::Ptr labeled_voxel_cloud (new pcl::PointCloud<pcl::PointXYZL>);
  for (typename HelperListT::const_iterator sv_itr = supervoxel_helpers_.cbegin (); sv_itr != supervoxel_helpers_.cend (); ++sv_itr)
  {
    typename PointCloudT::Ptr voxels;
    sv_itr->getVoxels (voxels);
    pcl::PointCloud<pcl::PointXYZL> xyzl_copy;
    copyPointCloud (*voxels, xyzl_copy);
    
    pcl::PointCloud<pcl::PointXYZL>::iterator xyzl_copy_itr = xyzl_copy.begin ();
    for ( ; xyzl_copy_itr != xyzl_copy.end (); ++xyzl_copy_itr) 
      xyzl_copy_itr->label = sv_itr->getLabel ();
    
    *labeled_voxel_cloud += xyzl_copy;
  }
  
  return labeled_voxel_cloud;  
}
コード例 #13
0
template <typename PointT> pcl::PointCloud<pcl::PointXYZRGBA>::Ptr
pcl::SupervoxelClustering<PointT>::getColoredVoxelCloud () const
{
  pcl::PointCloud<pcl::PointXYZRGBA>::Ptr colored_cloud = boost::make_shared< pcl::PointCloud<pcl::PointXYZRGBA> > ();
  for (typename HelperListT::const_iterator sv_itr = supervoxel_helpers_.cbegin (); sv_itr != supervoxel_helpers_.cend (); ++sv_itr)
  {
    typename PointCloudT::Ptr voxels;
    sv_itr->getVoxels (voxels);
    pcl::PointCloud<pcl::PointXYZRGBA> rgb_copy;
    copyPointCloud (*voxels, rgb_copy);
    
    pcl::PointCloud<pcl::PointXYZRGBA>::iterator rgb_copy_itr = rgb_copy.begin ();
    for ( ; rgb_copy_itr != rgb_copy.end (); ++rgb_copy_itr) 
      rgb_copy_itr->rgba = label_colors_ [sv_itr->getLabel ()];
    
    *colored_cloud += rgb_copy;
  }
  
  return colored_cloud;
}
コード例 #14
0
int main (int argc, char** argv)
{
  PointCloudType::Ptr cloud(new PointCloudType);
  PointCloudType::PointType p0(1,2,3);
  cloud->push_back(p0);

  // Call without normals
  {
  ComputeFeatures(cloud);
  }

  // Call with normals
  {
  PointCloudWithNormalsType::Ptr cloudWithNormals(new PointCloudWithNormalsType);
  copyPointCloud(*cloud, *cloudWithNormals);
  ComputeFeatures(cloudWithNormals);
  }

  return 0;
}
コード例 #15
0
void ComputeFeatures(PointCloudType::Ptr cloud)
{
  // Compute the normals
  pcl::NormalEstimation<PointCloudType::PointType, PointCloudWithNormalsType::PointType> normalEstimation;
  normalEstimation.setInputCloud (cloud);

  typedef pcl::search::KdTree<PointCloudType::PointType> TreeType;
  TreeType::Ptr tree (new TreeType);
  normalEstimation.setSearchMethod (tree);

  PointCloudWithNormalsType::Ptr cloudWithNormals (new PointCloudWithNormalsType);

  normalEstimation.setRadiusSearch (0.03);

  normalEstimation.compute (*cloudWithNormals);

  copyPointCloud(*cloud, *cloudWithNormals);
  std::cout << cloudWithNormals->points[0].x << std::endl;

  ComputeFeatures(cloudWithNormals);
}
コード例 #16
0
	/**
	 * Implements the Median Filter.
	 * Gets the largest value one dexel is allowed to move as argument (floating point), and the window size (integer).
	 * Returns a pointer to the filtered cloud.
	 * Relies on the PCL 1.7 implementation. [not exists in 1.6]
	 */
	PointCloud<pointType>::Ptr FilterHandler::medianFilter(float maxAllowedMovement, int windowSize)
	{
		PointCloud<pointType>::Ptr original = _cloud->makeShared(); 		// getting the cloud from pointer
		PointCloud<pointType>::Ptr filtered (new PointCloud<pointType>);	// will hold the filtered cloud

		// Copy everything from the original cloud to a new cloud (will be the filtered)
		copyPointCloud (*original, *filtered);

		for (int y = 0; y < filtered->height; ++y)
		for (int x = 0; x < filtered->width; ++x)
		  if (pcl::isFinite ((*original)(x, y)))
		  {
			std::vector<float> vals;
			vals.reserve (windowSize * windowSize);
			// Fill in the vector of values with the depths around the interest point
			for (int y_dev = -windowSize/2; y_dev <= windowSize/2; ++y_dev)
			  for (int x_dev = -windowSize/2; x_dev <= windowSize/2; ++x_dev)
			  {
				if (x + x_dev >= 0 && x + x_dev < filtered->width &&
					y + y_dev >= 0 && y + y_dev < filtered->height &&
					pcl::isFinite ((*original)(x+x_dev, y+y_dev)))
				  vals.push_back ((*original)(x+x_dev, y+y_dev).z);
			  }

			if (vals.size () == 0)
			  continue;

			// The output depth will be the median of all the depths in the window
			partial_sort (vals.begin (), vals.begin () + vals.size () / 2 + 1, vals.end ());
			float new_depth = vals[vals.size () / 2];
			// Do not allow points to move more than the set max_allowed_movement_
			if (fabs (new_depth - (*original)(x, y).z) < maxAllowedMovement)
			  filtered->operator ()(x, y).z = new_depth;
			else
			  filtered->operator ()(x, y).z = (*original)(x, y).z +
								 maxAllowedMovement * (new_depth - (*original)(x, y).z) / fabsf (new_depth - (*original)(x, y).z);
		  }
		io::savePCDFile(_output, *filtered, true);
		return filtered;
	}
コード例 #17
0
template <typename PointT> void
pcl::RadiusOutlierRemoval<PointT>::applyFilter (PointCloud &output)
{
  std::vector<int> indices;
  if (keep_organized_)
  {
    bool temp = extract_removed_indices_;
    extract_removed_indices_ = true;
    applyFilterIndices (indices);
    extract_removed_indices_ = temp;

    output = *input_;
    for (int rii = 0; rii < static_cast<int> (removed_indices_->size ()); ++rii)  // rii = removed indices iterator
      output.points[(*removed_indices_)[rii]].x = output.points[(*removed_indices_)[rii]].y = output.points[(*removed_indices_)[rii]].z = user_filter_value_;
    if (!pcl_isfinite (user_filter_value_))
      output.is_dense = false;
  }
  else
  {
    applyFilterIndices (indices);
    copyPointCloud (*input_, indices, output);
  }
}
コード例 #18
0
template <typename PointT> void
pcl::getPointCloudDifference (
    const pcl::PointCloud<PointT> &src,
    double threshold,
    const typename pcl::search::Search<PointT>::Ptr &tree,
    pcl::PointCloud<PointT> &output)
{
  // We're interested in a single nearest neighbor only
  std::vector<int> nn_indices (1);
  std::vector<float> nn_distances (1);

  // The input cloud indices that do not have a neighbor in the target cloud
  std::vector<int> src_indices;

  // Iterate through the source data set
  for (int i = 0; i < static_cast<int> (src.points.size ()); ++i)
  {
    // Ignore invalid points in the inpout cloud
    if (!isFinite (src.points[i]))
      continue;
    // Search for the closest point in the target data set (number of neighbors to find = 1)
    if (!tree->nearestKSearch (src.points[i], 1, nn_indices, nn_distances))
    {
      PCL_WARN ("No neighbor found for point %lu (%f %f %f)!\n", i, src.points[i].x, src.points[i].y, src.points[i].z);
      continue;
    }
    // Add points without a corresponding point in the target cloud to the output cloud
    if (nn_distances[0] > threshold)
      src_indices.push_back (i);
  }

  // Copy all the data fields from the input cloud to the output one
  copyPointCloud (src, src_indices, output);

  // Output is always dense, as invalid points in the input cloud are ignored
  output.is_dense = true;
}
コード例 #19
0
void 
RGBID_SLAM::VisualizationManager::refresh()
{
  if (visodo_ptr_->camera_pose_has_changed_ == true)
  {
    new_pose_ = visodo_ptr_->getSharedCameraPose();
    redraw_camera_ = true;
  }
  
  if (visodo_ptr_->scene_view_has_changed_ == true)
  {
    boost::mutex::scoped_lock lock(visodo_ptr_->mutex_scene_view_);
    
    scene_view_ = visodo_ptr_->scene_view_;
    intensity_view_ = visodo_ptr_->intensity_view_;
    depthinv_view_ = visodo_ptr_->depthinv_view_;
    
    redraw_scene_view_ = true;
    visodo_ptr_->scene_view_has_changed_ = false;
  }
  
  if (keyframe_manager_ptr_->trajectory_has_changed_ == true)
  {
    boost::mutex::scoped_lock lock(keyframe_manager_ptr_->mutex_odometry_);
    
    copyTrajectory(keyframe_manager_ptr_->poses_);
    
    keyframe_manager_ptr_->trajectory_has_changed_ = false;
    //redraw_trajectory_ = true;
  }
  
  if (keyframe_manager_ptr_->new_keyframe_has_changed_ == true)
  {
    boost::mutex::scoped_lock lock(keyframe_manager_ptr_->mutex_new_keyframe_);
    
    kf_id_ = keyframe_manager_ptr_->new_kf_id_;
    last_KF_pose_ = keyframe_manager_ptr_->new_keyframe_pose_;  
    
    std::pair<IdToPoseMap::iterator,bool> is_new_id;
    is_new_id = id2pose_map_.insert(std::make_pair(kf_id_, last_KF_pose_));  
    
    //segmentation_view_ = keyframe_manager_ptr_->rgb_image_with_keypoints_;
    memcpy ((PixelRGB*)&(segmentation_view_[0]), &(keyframe_manager_ptr_->rgb_image_with_keypoints_.data[0]), 
            keyframe_manager_ptr_->rgb_image_with_keypoints_.cols * keyframe_manager_ptr_->rgb_image_with_keypoints_.rows*sizeof(PixelRGB));
    negentropy_view_ = keyframe_manager_ptr_->negentropy_view_;
    
    keyframe_manager_ptr_->new_keyframe_has_changed_ = false;
    redraw_keyframe_ = true;
  }
  
  if (keyframe_manager_ptr_->new_pointcloud_has_changed_ == true)
  {
    boost::mutex::scoped_lock lock(keyframe_manager_ptr_->mutex_new_pointcloud_);
    
    new_pc_id_ = keyframe_manager_ptr_->new_pointcloud_id_;   
    
    std::pair<IdToPointCloudMap::iterator,bool> is_new_id;
        
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr aux_ptr;
    aux_ptr.reset(new pcl::PointCloud<pcl::PointXYZRGB>) ;
    
    copyPointCloud(keyframe_manager_ptr_->new_pointcloud_, aux_ptr);
    
    is_new_id = id2pointcloud_map_.insert(std::make_pair(new_pc_id_, aux_ptr));
    
    keyframe_manager_ptr_->new_pointcloud_has_changed_ = false;
    redraw_pointcloud_ = true;
  }
  
  if (keyframe_manager_ptr_->pose_graph_has_changed_ == true)
  {
    IdToPoseMap::iterator id2pose_map_it;
  
    for (int i=0; i<keyframe_manager_ptr_->keyframes_list_.size(); i++)
    {
      id2pose_map_it = id2pose_map_.find(keyframe_manager_ptr_->keyframes_list_[i]->kf_id_);
      if (id2pose_map_it != id2pose_map_.end())
      {
        id2pose_map_[keyframe_manager_ptr_->keyframes_list_[i]->kf_id_] = keyframe_manager_ptr_->keyframes_list_[i]->getPose();
      } 
    }
    
    keyframe_manager_ptr_->pose_graph_has_changed_ = false;
    redraw_all_pointclouds_ = true;
  }
  
  if (redraw_scene_view_)
  {
    scene_viewer_.viewerRGB_.showRGBImage (reinterpret_cast<unsigned char*> (&scene_view_[0]), cols_, rows_);
    intensity_viewer_.viewerFloat_.showFloatImage(&intensity_view_[0], cols_, rows_, intensity_viewer_.min_val_, intensity_viewer_.max_val_,true);
    redraw_scene_view_ = false;
  }
    
  if (redraw_camera_)
  {
    camera_viewer_.updateCamera(new_pose_);
    redraw_camera_ = false;
  }
  
  if (redraw_keyframe_)
  {
    negentropy_viewer_.viewerRGB_.showRGBImage (reinterpret_cast<unsigned char*> (&negentropy_view_[0]), cols_, rows_);
    //segmentation_viewer_.viewerRGB_.showRGBImage (reinterpret_cast<unsigned char*> (&segmentation_view_[0]), cols_, rows_);
    redraw_keyframe_ = false;
  }
  
  if (redraw_pointcloud_)
  {
    IdToPoseMap::iterator id2pose_map_it;
    
    id2pose_map_it = id2pose_map_.find(new_pc_id_);
        
    if (id2pose_map_it != id2pose_map_.end())
    {
      camera_viewer_.updatePointCloud(id2pointcloud_map_[new_pc_id_],(*id2pose_map_it).first, (*id2pose_map_it).second);
    }
    
    redraw_pointcloud_ = false;
  }
  
  if (redraw_trajectory_)
  {
    camera_viewer_.updateTrajectory(trajectory_);
    redraw_trajectory_ = false;
  }
  
  if (redraw_all_pointclouds_)
  {
    IdToPoseMap::iterator id2pose_map_it;        
    IdToPointCloudMap::const_iterator id2pointcloud_map_it;
    
    for (id2pose_map_it = id2pose_map_.begin(); 
         id2pose_map_it != id2pose_map_.end();
         id2pose_map_it++)
    {
      int kf_id = (*id2pose_map_it).first;
      Eigen::Affine3d pose = (*id2pose_map_it).second;
      
      //camera_viewer_.updateKeyframe(pose, kf_id);    
      
      id2pointcloud_map_it = id2pointcloud_map_.find(kf_id);
      
      if (id2pointcloud_map_it != id2pointcloud_map_.end())
      {
        camera_viewer_.updatePointCloud((*id2pointcloud_map_it).second, kf_id, pose);
      }          
    }  
    
    redraw_all_pointclouds_ = false;
  }
  
  camera_viewer_.spinOnce(1);
  //boost::this_thread::sleep (boost::posix_time::millisec (20)); 
}
コード例 #20
0
ファイル: fast_bilateral.hpp プロジェクト: hitsjt/StanfordPCL
template <typename PointT> void
pcl::FastBilateralFilter<PointT>::applyFilter (PointCloud &output)
{
  if (!input_->isOrganized ())
  {
    PCL_ERROR ("[pcl::FastBilateralFilter] Input cloud needs to be organized.\n");
    return;
  }

  copyPointCloud (*input_, output);
  float base_max = std::numeric_limits<float>::min (),
        base_min = std::numeric_limits<float>::max ();
  for (size_t x = 0; x < output.width; ++x)
    for (size_t y = 0; y < output.height; ++y)
      if (pcl_isfinite (output (x, y).z))
      {
        if (base_max < output (x, y).z)
          base_max = output (x, y).z;
        if (base_min > output (x, y).z)
          base_min = output (x, y).z;
      }

  for (size_t x = 0; x < output.width; ++x)
      for (size_t y = 0; y < output.height; ++y)
        if (!pcl_isfinite (output (x, y).z))
          output (x, y).z = base_max;

  const float base_delta = base_max - base_min;

  const size_t padding_xy = 2;
  const size_t padding_z  = 2;

  const size_t small_width  = static_cast<size_t> (static_cast<float> (input_->width  - 1) / sigma_s_) + 1 + 2 * padding_xy;
  const size_t small_height = static_cast<size_t> (static_cast<float> (input_->height - 1) / sigma_s_) + 1 + 2 * padding_xy;
  const size_t small_depth  = static_cast<size_t> (base_delta / sigma_r_)   + 1 + 2 * padding_z;


  Array3D data (small_width, small_height, small_depth);
  for (size_t x = 0; x < input_->width; ++x)
  {
    const size_t small_x = static_cast<size_t> (static_cast<float> (x) / sigma_s_ + 0.5f) + padding_xy;
    for (size_t y = 0; y < input_->height; ++y)
    {
      const float z = output (x,y).z - base_min;

      const size_t small_y = static_cast<size_t> (static_cast<float> (y) / sigma_s_ + 0.5f) + padding_xy;
      const size_t small_z = static_cast<size_t> (static_cast<float> (z) / sigma_r_ + 0.5f) + padding_z;

      Eigen::Vector2f& d = data (small_x, small_y, small_z);
      d[0] += output (x,y).z;
      d[1] += 1.0f;
    }
  }


  std::vector<long int> offset (3);
  offset[0] = &(data (1,0,0)) - &(data (0,0,0));
  offset[1] = &(data (0,1,0)) - &(data (0,0,0));
  offset[2] = &(data (0,0,1)) - &(data (0,0,0));

  Array3D buffer (small_width, small_height, small_depth);

  for (size_t dim = 0; dim < 3; ++dim)
  {
    const long int off = offset[dim];
    for (size_t n_iter = 0; n_iter < 2; ++n_iter)
    {
      std::swap (buffer, data);
      for(size_t x = 1; x < small_width - 1; ++x)
        for(size_t y = 1; y < small_height - 1; ++y)
        {
          Eigen::Vector2f* d_ptr = &(data (x,y,1));
          Eigen::Vector2f* b_ptr = &(buffer (x,y,1));

          for(size_t z = 1; z < small_depth - 1; ++z, ++d_ptr, ++b_ptr)
            *d_ptr = (*(b_ptr - off) + *(b_ptr + off) + 2.0 * (*b_ptr)) / 4.0;
        }
    }
  }

  if (early_division_)
  {
    for (std::vector<Eigen::Vector2f >::iterator d = data.begin (); d != data.end (); ++d)
      *d /= ((*d)[0] != 0) ? (*d)[1] : 1;

    for (size_t x = 0; x < input_->width; x++)
      for (size_t y = 0; y < input_->height; y++)
      {
        const float z = output (x,y).z - base_min;
        const Eigen::Vector2f D = data.trilinear_interpolation (static_cast<float> (x) / sigma_s_ + padding_xy,
                                                                static_cast<float> (y) / sigma_s_ + padding_xy,
                                                                z / sigma_r_ + padding_z);
        output(x,y).z = D[0];
      }
  }
  else
  {
    for (size_t x = 0; x < input_->width; ++x)
      for (size_t y = 0; y < input_->height; ++y)
      {
        const float z = output (x,y).z - base_min;
        const Eigen::Vector2f D = data.trilinear_interpolation (static_cast<float> (x) / sigma_s_ + padding_xy,
                                                                static_cast<float> (y) / sigma_s_ + padding_xy,
                                                                z / sigma_r_ + padding_z);
        output (x,y).z = D[0] / D[1];
      }
  }
}
コード例 #21
0
ファイル: Track.cpp プロジェクト: biotracking/biotrack
/**
 * Given a vector of data points and a vector of model
 * points, use ICP to register the model to the data.  Add registration transformation
 * to transforms vector and registration transformation from
 * the track's birth frame to absoluteTransforms vector.
 */
pcl::PointCloud<pcl::PointXYZRGB> Track::updatePosition(pcl::PointCloud<pcl::PointXYZRGB> dataPTS_cloud,vector<Model> modelPTS_clouds,
                                             int modelTODataThreshold, int separateThreshold, int matchDThresh,
                                             int ICP_ITERATIONS, double ICP_TRANSEPSILON, double ICP_EUCLIDEANDIST, double ICP_MAXFIT)
{

    matchDistanceThreshold=matchDThresh;
    nukeDistanceThreshold = separateThreshold;

    icp_maxIter=ICP_ITERATIONS;
    icp_transformationEpsilon=ICP_TRANSEPSILON;
    icp_euclideanDistance=ICP_EUCLIDEANDIST;

    icp_maxfitness = ICP_MAXFIT;




    pcl::PointCloud<pcl::PointXYZRGB> modelToProcess_cloud;

    Eigen::Matrix4f T;

    T.setIdentity();


//Figure out the identity of the model for this region
    if (frameIndex == birthFrameIndex)
    {

        isBirthFrame=true; // Try out Doing extra work aligning the objects if it is the very first frame
        //Determine what model this creature should use
        if(modelPTS_clouds.size()<2){// If there is only one model, just use that one
            modelIndex = 0;
        }
        else{
        modelIndex= identify(dataPTS_cloud,modelPTS_clouds);
        modelRotated = modelPTS_clouds[modelIndex].rotated;
}

    }
    else
    {
        isBirthFrame=false;


    }


    //    double birthAssociationsThreshold = modelPTS_clouds[0].cloud.size() * .01 *modelTODataThreshold; //TODO Shouldn't hardcode this to the first!
        double birthAssociationsThreshold = modelPTS_clouds[modelIndex].cloud.size() * .01 *modelTODataThreshold; //TODO Shouldn't hardcode this to the first!

        double healthyAssociationsThreshold = birthAssociationsThreshold*.4; //Still healthy with 40 percent of a whole model


    modelToProcess_cloud = modelPTS_clouds[modelIndex].cloud;

    //STRIP 3D data
    /**/
    PointCloud<PointXY> dataPTS_cloud2D;
    copyPointCloud(dataPTS_cloud,dataPTS_cloud2D);
    PointCloud<PointXY> modelPTS_cloud2D;
    copyPointCloud(modelToProcess_cloud,modelPTS_cloud2D);

             PointCloud<PointXYZRGB> dataPTS_cloudStripped;
             copyPointCloud(dataPTS_cloud2D,dataPTS_cloudStripped);

             PointCloud<PointXYZRGB> modelPTS_cloudStripped;
             copyPointCloud(modelPTS_cloud2D,modelPTS_cloudStripped);
            /* */


    //leave open for other possible ICP methods
    bool doPCL_3DICP=true;
    double fitnessin;

    //PCL implementation of ICP
    if(doPCL_3DICP){

        if (dataPTS_cloud.size() > 1) //TODO FIX the math will throw an error if there are not enough data points
        {
            // Find transformation from the orgin that will optimize a match between model and target
            T=   calcTransformPCLRGB(dataPTS_cloud, modelToProcess_cloud,&fitnessin); //Use 3D color

            //Apply the Transformation
            pcl::transformPointCloud(modelToProcess_cloud,modelToProcess_cloud, T);
//            pcl::transformPointCloud(modelPTS_cloudStripped,modelPTS_cloudStripped, T);

        }

    }

    else{



        //Use the 2D stripped models and data and align them
                    T=   calcTransformPCLRGB(dataPTS_cloudStripped, modelPTS_cloudStripped, &fitnessin); // Just 2D
                    pcl::transformPointCloud(modelToProcess_cloud,modelToProcess_cloud, T);

    }


    int totalpointsBeforeRemoval=dataPTS_cloud.size();
    int totalRemovedPoints = 0;

    /** Remove old data points associated with the model
      * delete the points under where the model was placed. The amount of points destroyed in this process gives us a metric for how healthy the track is.
      *
      **/
    pcl::PointCloud<pcl::PointXYZRGB> dataPTSreduced_cloud;

    //Removeclosestdatacloudpoints strips the 3D data and nukes points in the proximitiy of where our model has lined up
    pcl::copyPointCloud(removeClosestDataCloudPoints(dataPTS_cloud, modelToProcess_cloud, nukeDistanceThreshold ),dataPTSreduced_cloud);


    totalRemovedPoints = totalpointsBeforeRemoval - dataPTSreduced_cloud.size();


    qDebug()<<"Removed Points from Track  "<<totalRemovedPoints;

    /// For Debugging we can visualize the Pointcloud
             /**
                pcl:: PointCloud<PointXYZRGB>::Ptr dataPTS_cloud_ptr (new pcl::PointCloud<PointXYZRGB> (dataPTS_cloud));
              //  copyPointCloud(modelPTS_cloud,)
                transformPointCloud(modelPTS_clouds[modelIndex].cloud,modelPTS_clouds[modelIndex].cloud,T);

                pcl:: PointCloud<PointXYZRGB>::Ptr model_cloud_ptrTempTrans (new pcl::PointCloud<PointXYZRGB> (modelPTS_clouds[modelIndex].cloud));

                pcl::visualization::CloudViewer viewer("Simple Cloud Viewer");
                viewer.showCloud(dataPTS_cloud_ptr);

            int sw=0;
                        while (!viewer.wasStopped())
                        {
                            if(sw==0){
                            viewer.showCloud(model_cloud_ptrTempTrans);
                            sw=1;
                            }
                            else{
                                viewer.showCloud(dataPTS_cloud_ptr);
            sw=0;
                            }

                        }
            /**/



    //////
    ///Determine Health of latest part of track
    ////////

    // Just born tracks go here: //Should get rid of this, doesn't make sense
    if (frameIndex == birthFrameIndex)
    {

        absoluteTransforms.push_back(T);

        //Check number of data points associated with this track, if it is greater than
        // the birth threshold then set birth flag to true.
        //If it doesn't hit this, it never gets born ever

        if ( totalRemovedPoints>birthAssociationsThreshold ) //matchScore >birthAssociationsThreshold && //Need new check for birthAssociationsthresh//  closestToModel.size() >= birthAssociationsThreshold)
        {
            isBirthable=true;


        }

    }
    // Tracks that are older than 1 frame get determined here
    else
    {
        //Check the number of data points associated with this track, if it is less than
        //the zombie/death threshold, then copy previous transform, and add this frame
        //index to zombieFrames

        //Is the track unhealthy? if so make it a zombie
        if ( totalRemovedPoints<healthyAssociationsThreshold )// !didConverge)// No zombies for now! eternal life! !didConverge) //closestToModel.size() < healthyAssociationsThreshold && absoluteTransforms.size() > 2)
        {

            absoluteTransforms.push_back(T);

            zombieIndicesIntoAbsTransforms.push_back(absoluteTransforms.size()-1);
            numberOfContinuousZombieFrames++;
        }
        else // Not zombie frame, keep using new transforms
        {

            // add to transforms
            absoluteTransforms.push_back(T);
            numberOfContinuousZombieFrames = 0;

        } //not zombie frame
    }// Not first frame



    // increment frame counter
    frameIndex++;

    return dataPTSreduced_cloud;

}
コード例 #22
0
ファイル: Track.cpp プロジェクト: biotracking/biotrack
/**
 * Given a cloud of datapts (an in/out variable), a vector of dataPts, an index to a specific
 * point in datapts and a distanceThreshold, add the list of indices into dataPts which are
 * within the distanceThreshold  to dirtyPts.
 *
 * @param dataPTSreduced_cloud a reference to a vector of Points
 * @param point_cloud_for_reduction   the collection of points that we want to delete some points from
  * @param distanceThreshold an integer Euclidean distance
 */
pcl::PointCloud<pcl::PointXYZRGB> Track::removeClosestDataCloudPoints(pcl::PointCloud<pcl::PointXYZRGB> point_cloud_for_reduction,pcl::PointCloud<pcl::PointXYZRGB> removal_Cloud, int distanceThreshold){

    //TODO, MAKE THIS PARALLEL

        //NOTE: you cannot feed a KNN searcher clouds with 1 or fewer datapoints!

        //KDTREE SEARCH

        pcl::KdTreeFLANN<pcl::PointXYZRGB> kdtree;
        // Neighbors within radius search

        std::vector<int> pointIdxRadiusSearch;
        std::vector<float> pointRadiusSquaredDistance;

        double point_radius = distanceThreshold;

        PointCloud<pcl::PointXYZRGB> point_cloud_flattened;//Point cloud with extra Hue Dimension crushed to 0

        PointCloud<pcl::PointXYZRGB> point_cloud_for_return;
        point_cloud_for_return.reserve(point_cloud_for_reduction.size());


        if(point_cloud_for_reduction.size()>1){

            bool *marked= new bool[point_cloud_for_reduction.size()];
            memset(marked,false,sizeof(bool)*point_cloud_for_reduction.size());

            bool *parmarked= new bool[point_cloud_for_reduction.size()];
            memset(parmarked,false,sizeof(bool)*point_cloud_for_reduction.size());
            //Make a version of the original data cloud that is flattened to z=0 but with the same indice
            copyPointCloud( point_cloud_for_reduction,point_cloud_flattened);


            for(int q=0; q<point_cloud_flattened.size();q++){

                point_cloud_flattened.at(q).z=0;

            }

           pcl:: PointCloud<PointXYZRGB>::Ptr point_cloud_for_reduction_ptr (new pcl::PointCloud<PointXYZRGB> (point_cloud_flattened));

           // K nearest neighbor search with Radius

            kdtree.setInputCloud (point_cloud_for_reduction_ptr); //Needs to have more than 1 data pt or segfault




            double t = (double)getTickCount();


//The below has been parallelized,and runs about 2X faster
                /**
            // iterate over points in model and remove those points within a certain distance
            for (unsigned int c=0; c < removal_Cloud.size(); c++)
            {

                if(point_cloud_for_reduction.size()<2){
                    break;
                }


                pcl::PointXYZRGB searchPoint;

                searchPoint.x = removal_Cloud.points[c].x;
                searchPoint.y = removal_Cloud.points[c].y;
               //Need to take z as zero when using a flattened data point cloud
               searchPoint.z = 0;

                // qDebug() <<"Datapts before incremental remove"<< point_cloud_for_reduction.size();
                if ( kdtree.radiusSearch (searchPoint, point_radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0 )
                {
                    for (size_t i = 0; i < pointIdxRadiusSearch.size (); ++i){
                        if(point_cloud_for_reduction.size()>0) ///NOTE CHANGED FROM > 1

                        marked[pointIdxRadiusSearch[i]]=true;
    //                        point_cloud_for_reduction.erase(point_cloud_for_reduction.begin()+pointIdxRadiusSearch[i]);// point_cloud_for_reduction.points[ pointIdxRadiusSearch[i] ]
                    }
                }


            }

            //DESTROY ALL MARKED POINTS
            for(uint q=0; q< point_cloud_for_reduction.size(); q++){
                if(!marked[q]){
                    point_cloud_for_return.push_back(point_cloud_for_reduction.at(q));
    //                point_cloud_for_return.at(q) = point_cloud_for_reduction.at(q);

                }

            }
            t = ((double)getTickCount() - t)/getTickFrequency();
            cout << "::: time serial remove " << t << endl;
/**/

            //PARALLEL VERSION



            /// Timing
             t = (double)getTickCount();

                cv::parallel_for_(Range(0, removal_Cloud.size()),Remove_Parallel(&kdtree, removal_Cloud,point_cloud_for_reduction, point_radius, &parmarked) );



                //DESTROY ALL MARKED POINTS
                for(uint q=0; q< point_cloud_for_reduction.size(); q++){
                    if(!parmarked[q]){
                        point_cloud_for_return.push_back(point_cloud_for_reduction.at(q));
        //                point_cloud_for_return.at(q) = point_cloud_for_reduction.at(q);

                    }

                }

                t = ((double)getTickCount() - t)/getTickFrequency();
                cout << "::: time parallel remove " << t << endl;


            delete[] marked;
                delete[] parmarked;
        }

        //point_cloud_for_reduction.resize();
        return point_cloud_for_return;

    }