void OctreeVoxelGrid::generateVoxelCloudImpl(const sensor_msgs::PointCloud2ConstPtr& input_msg)
  {

    typename pcl::PointCloud<PointT>::Ptr cloud (new pcl::PointCloud<PointT> ());
    typename pcl::PointCloud<PointT>::Ptr cloud_voxeled (new pcl::PointCloud<PointT> ());
    pcl::fromROSMsg(*input_msg, *cloud);

    // generate octree
    typename pcl::octree::OctreePointCloud<PointT> octree(resolution_);
    // add point cloud to octree
    octree.setInputCloud(cloud);
    octree.addPointsFromInputCloud();
    // get points where grid is occupied
    typename pcl::octree::OctreePointCloud<PointT>::AlignedPointTVector point_vec;
    octree.getOccupiedVoxelCenters(point_vec);
    // put points into point cloud
    cloud_voxeled->width = point_vec.size();
    cloud_voxeled->height = 1;
    for (int i = 0; i < point_vec.size(); i++) {
      cloud_voxeled->push_back(point_vec[i]);
    }

    // publish point cloud
    sensor_msgs::PointCloud2 output_msg;
    toROSMsg(*cloud_voxeled, output_msg);
    output_msg.header = input_msg->header;
    pub_cloud_.publish(output_msg);

    // publish marker
    if (publish_marker_flag_) {
      visualization_msgs::Marker marker_msg;
      marker_msg.type = visualization_msgs::Marker::CUBE_LIST;
      marker_msg.scale.x = resolution_;
      marker_msg.scale.y = resolution_;
      marker_msg.scale.z = resolution_;
      marker_msg.header = input_msg->header;
      marker_msg.pose.orientation.w = 1.0;
      if (marker_color_ == "flat") {
        marker_msg.color = jsk_topic_tools::colorCategory20(0);
        marker_msg.color.a = marker_color_alpha_;
      }
      
      // compute min and max
      Eigen::Vector4f minpt, maxpt;
      pcl::getMinMax3D<PointT>(*cloud_voxeled, minpt, maxpt);
      PointT p;
      for (size_t i = 0; i < cloud_voxeled->size(); i++) {
        p = cloud_voxeled->at(i);
        geometry_msgs::Point point_ros;
        point_ros.x = p.x;
        point_ros.y = p.y;
        point_ros.z = p.z;
        marker_msg.points.push_back(point_ros);
        if (marker_color_ == "flat") {
          marker_msg.colors.push_back(jsk_topic_tools::colorCategory20(0));
        }
        else if (marker_color_ == "z") {
          marker_msg.colors.push_back(jsk_topic_tools::heatColor((p.z - minpt[2]) / (maxpt[2] - minpt[2])));
        }
        else if (marker_color_ == "x") {
          marker_msg.colors.push_back(jsk_topic_tools::heatColor((p.x - minpt[0]) / (maxpt[0] - minpt[0])));
        }
        else if (marker_color_ == "y") {
          marker_msg.colors.push_back(jsk_topic_tools::heatColor((p.y - minpt[1]) / (maxpt[1] - minpt[1])));
        }
        marker_msg.colors[marker_msg.colors.size() - 1].a = marker_color_alpha_;
      }
      pub_marker_.publish(marker_msg);
    }
  }
Esempio n. 2
0
void WorldDownloadManager::cropMesh(const kinfu_msgs::KinfuCloudPoint & min,
  const kinfu_msgs::KinfuCloudPoint & max,typename pcl::PointCloud<PointT>::ConstPtr cloud,
  TrianglesConstPtr triangles,typename pcl::PointCloud<PointT>::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);

  uint offset;

  // check the points
  for (uint i = 0; i < cloud_size; i++)
  {
    const PointT & 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);
}
    bool
    segmentCloud (pointing_object_evaluation::ObjectSegmentationRequest::Request &req, pointing_object_evaluation::ObjectSegmentationRequest::Response &res)
    {
      CloudPtr cloud (new Cloud ());
      {
        //boost::lock_guard<boost::mutex> lock (cloud_mutex_);
        *cloud = *cloud_;
      }
      
      unsigned char red [6] = {255,   0,   0, 255, 255,   0};
      unsigned char grn [6] = {  0, 255,   0, 255,   0, 255};
      unsigned char blu [6] = {  0,   0, 255,   0, 255, 255};
      pcl::PointCloud<pcl::PointXYZRGB> aggregate_cloud;

      // Estimate Normals
      double ne_start = pcl::getTime ();
      pcl::PointCloud<pcl::Normal>::Ptr normal_cloud (new pcl::PointCloud<pcl::Normal>);
      ne_.setInputCloud (cloud);
      ne_.compute (*normal_cloud);
      double ne_end = pcl::getTime ();
      //std::cout << "NE took: " << double(ne_end - ne_start) << std::endl;
      
      // Segment Planes
      double mps_start = pcl::getTime ();
      std::vector<pcl::PlanarRegion<PointT>, Eigen::aligned_allocator<pcl::PlanarRegion<PointT> > > regions;
      std::vector<pcl::ModelCoefficients> model_coefficients;
      std::vector<pcl::PointIndices> inlier_indices;  
      pcl::PointCloud<pcl::Label>::Ptr labels (new pcl::PointCloud<pcl::Label>);
      std::vector<pcl::PointIndices> label_indices;
      std::vector<pcl::PointIndices> boundary_indices;
      mps_.setInputNormals (normal_cloud);
      mps_.setInputCloud (cloud);
      //mps.segment (regions);
      mps_.segmentAndRefine (regions, model_coefficients, inlier_indices, labels, label_indices, boundary_indices);      
      
      //Segment Objects
      typename pcl::PointCloud<PointT>::CloudVectorType clusters;

      if (regions.size () > 0)
      {
        //printf ("got some planes!\n");
        std::vector<bool> plane_labels;
        plane_labels.resize (label_indices.size (), false);
        for (size_t i = 0; i < label_indices.size (); i++)
        {
          if (label_indices[i].indices.size () > 10000)
          {
            plane_labels[i] = true;
          }
        }  
        //printf ("made label vec\n");
        
        typename pcl::EuclideanClusterComparator<PointT, pcl::Normal, pcl::Label>::Ptr euclidean_cluster_comparator_ (new typename pcl::EuclideanClusterComparator<PointT, pcl::Normal, pcl::Label>());
        euclidean_cluster_comparator_->setInputCloud (cloud);
        euclidean_cluster_comparator_->setLabels (labels);
        euclidean_cluster_comparator_->setExcludeLabels (plane_labels);
        euclidean_cluster_comparator_->setDistanceThreshold (0.01f, false);
        

        pcl::PointCloud<pcl::Label> euclidean_labels;
        std::vector<pcl::PointIndices> euclidean_label_indices;
        pcl::OrganizedConnectedComponentSegmentation<PointT,pcl::Label> euclidean_segmentation (euclidean_cluster_comparator_);
        euclidean_segmentation.setInputCloud (cloud);
        euclidean_segmentation.segment (euclidean_labels, euclidean_label_indices);
        
        //printf ("done segmenting the clusters\n");
        
        std::vector<Eigen::Vector4f> centroids;
        
        for (size_t i = 0; i < euclidean_label_indices.size (); i++)
        {
          if (euclidean_label_indices[i].indices.size () > 1000)
          {
            pcl::PointCloud<PointT> cluster;
            pcl::copyPointCloud (*cloud,euclidean_label_indices[i].indices,cluster);	    
            clusters.push_back (cluster);
            
	    /*Eigen::Vector4f centroid;
            pcl::compute3DCentroid (cluster, centroid);
	    centroids.push_back (centroid);*/
            
	    //pcl::PointXYZ centroid_pt (centroid[0], centroid[1], centroid[2]);
            
            
            //double dist = sqrt (centroid[0]*centroid[0] + centroid[1]*centroid[1] + centroid[2]*centroid[2]);
            //object_points_.push_back (centroid_pt);

	    
            // Send to RViz
            pcl::PointCloud<pcl::PointXYZRGB> color_cluster;
            pcl::copyPointCloud (cluster, color_cluster);
            for (size_t j = 0; j < color_cluster.size (); j++)
            {
              color_cluster.points[j].r = (color_cluster.points[j].r + red[i%6]) / 2;
              color_cluster.points[j].g = (color_cluster.points[j].g + grn[i%6]) / 2;
              color_cluster.points[j].b = (color_cluster.points[j].b + blu[i%6]) / 2;
            }
            aggregate_cloud += color_cluster;
	    
          }    
        }
        
        PCL_INFO ("Got %d euclidean clusters!\n", clusters.size ());
	//PCL_INFO ("Got %d centroids!\n", centroids.size ());

        aggregate_cloud.header = cloud->header;
        sensor_msgs::PointCloud2 cloud_msg;
        pcl::toROSMsg (aggregate_cloud, cloud_msg);
        object_cloud_pub_.publish (cloud_msg);

        // TODO: mutex
        
	/*clusters_ = clusters;
	  centroids_= centroids;*/
      }
      
    }
Esempio n. 4
0
template <typename PointT> void
pcl::approximatePolygon2D (const typename pcl::PointCloud<PointT>::VectorType &polygon,
                           typename pcl::PointCloud<PointT>::VectorType &approx_polygon,
                           float threshold, bool refine, bool closed)
{
    approx_polygon.clear ();
    if (polygon.size () < 3)
        return;

    std::vector<std::pair<unsigned, unsigned> > intervals;
    std::pair<unsigned,unsigned> interval (0, 0);

    if (closed)
    {
        float max_distance = .0f;
        for (unsigned idx = 1; idx < polygon.size (); ++idx)
        {
            float distance = (polygon [0].x - polygon [idx].x) * (polygon [0].x - polygon [idx].x) +
                             (polygon [0].y - polygon [idx].y) * (polygon [0].y - polygon [idx].y);

            if (distance > max_distance)
            {
                max_distance = distance;
                interval.second = idx;
            }
        }

        for (unsigned idx = 1; idx < polygon.size (); ++idx)
        {
            float distance = (polygon [interval.second].x - polygon [idx].x) * (polygon [interval.second].x - polygon [idx].x) +
                             (polygon [interval.second].y - polygon [idx].y) * (polygon [interval.second].y - polygon [idx].y);

            if (distance > max_distance)
            {
                max_distance = distance;
                interval.first = idx;
            }
        }

        if (max_distance < threshold * threshold)
            return;

        intervals.push_back (interval);
        std::swap (interval.first, interval.second);
        intervals.push_back (interval);
    }
    else
    {
        interval.first = 0;
        interval.second = static_cast<unsigned int> (polygon.size ()) - 1;
        intervals.push_back (interval);
    }

    std::vector<unsigned> result;
    // recursively refine
    while (!intervals.empty ())
    {
        std::pair<unsigned, unsigned>& currentInterval = intervals.back ();
        float line_x = polygon [currentInterval.first].y - polygon [currentInterval.second].y;
        float line_y = polygon [currentInterval.second].x - polygon [currentInterval.first].x;
        float line_d = polygon [currentInterval.first].x * polygon [currentInterval.second].y - polygon [currentInterval.first].y * polygon [currentInterval.second].x;

        float linelen = 1.0f / sqrt (line_x * line_x + line_y * line_y);

        line_x *= linelen;
        line_y *= linelen;
        line_d *= linelen;

        float max_distance = 0.0;
        unsigned first_index = currentInterval.first + 1;
        unsigned max_index = 0;

        // => 0-crossing
        if (currentInterval.first > currentInterval.second)
        {
            for (unsigned idx = first_index; idx < polygon.size(); idx++)
            {
                float distance = fabsf (line_x * polygon[idx].x + line_y * polygon[idx].y + line_d);
                if (distance > max_distance)
                {
                    max_distance = distance;
                    max_index  = idx;
                }
            }
            first_index = 0;
        }

        for (unsigned int idx = first_index; idx < currentInterval.second; idx++)
        {
            float distance = fabsf (line_x * polygon[idx].x + line_y * polygon[idx].y + line_d);
            if (distance > max_distance)
            {
                max_distance = distance;
                max_index  = idx;
            }
        }

        if (max_distance > threshold)
        {
            std::pair<unsigned, unsigned> interval (max_index, currentInterval.second);
            currentInterval.second = max_index;
            intervals.push_back (interval);
        }
        else
        {
            result.push_back (currentInterval.second);
            intervals.pop_back ();
        }
    }

    approx_polygon.reserve (result.size ());
    if (refine)
    {
        std::vector<Eigen::Vector3f> lines (result.size ());
        std::reverse (result.begin (), result.end ());
        for (unsigned rIdx = 0; rIdx < result.size (); ++rIdx)
        {
            unsigned nIdx = rIdx + 1;
            if (nIdx == result.size ())
                nIdx = 0;

            Eigen::Vector2f centroid = Eigen::Vector2f::Zero ();
            Eigen::Matrix2f covariance = Eigen::Matrix2f::Zero ();
            unsigned pIdx = result[rIdx];
            unsigned num_points = 0;
            if (pIdx > result[nIdx])
            {
                num_points = static_cast<unsigned> (polygon.size ()) - pIdx;
                for (; pIdx < polygon.size (); ++pIdx)
                {
                    covariance.coeffRef (0) += polygon [pIdx].x * polygon [pIdx].x;
                    covariance.coeffRef (1) += polygon [pIdx].x * polygon [pIdx].y;
                    covariance.coeffRef (3) += polygon [pIdx].y * polygon [pIdx].y;
                    centroid [0] += polygon [pIdx].x;
                    centroid [1] += polygon [pIdx].y;
                }
                pIdx = 0;
            }

            num_points += result[nIdx] - pIdx;
            for (; pIdx < result[nIdx]; ++pIdx)
            {
                covariance.coeffRef (0) += polygon [pIdx].x * polygon [pIdx].x;
                covariance.coeffRef (1) += polygon [pIdx].x * polygon [pIdx].y;
                covariance.coeffRef (3) += polygon [pIdx].y * polygon [pIdx].y;
                centroid [0] += polygon [pIdx].x;
                centroid [1] += polygon [pIdx].y;
            }

            covariance.coeffRef (2) = covariance.coeff (1);

            float norm = 1.0f / float (num_points);
            centroid *= norm;
            covariance *= norm;
            covariance.coeffRef (0) -= centroid [0] * centroid [0];
            covariance.coeffRef (1) -= centroid [0] * centroid [1];
            covariance.coeffRef (3) -= centroid [1] * centroid [1];

            float eval;
            Eigen::Vector2f normal;
            eigen22 (covariance, eval, normal);

            // select the one which is more "parallel" to the original line
            Eigen::Vector2f direction;
            direction [0] = polygon[result[nIdx]].x - polygon[result[rIdx]].x;
            direction [1] = polygon[result[nIdx]].y - polygon[result[rIdx]].y;
            direction.normalize ();

            if (fabs (direction.dot (normal)) > float(M_SQRT1_2))
            {
                std::swap (normal [0], normal [1]);
                normal [0] = -normal [0];
            }

            // needs to be on the left side of the edge
            if (direction [0] * normal [1] < direction [1] * normal [0])
                normal *= -1.0;

            lines [rIdx].head<2> () = normal;
            lines [rIdx] [2] = -normal.dot (centroid);
        }

        float threshold2 = threshold * threshold;
        for (unsigned rIdx = 0; rIdx < lines.size (); ++rIdx)
        {
            unsigned nIdx = rIdx + 1;
            if (nIdx == result.size ())
                nIdx = 0;

            Eigen::Vector3f vertex = lines [rIdx].cross (lines [nIdx]);
            vertex /= vertex [2];
            vertex [2] = 0.0;

            PointT point;
            // test whether we need another edge since the intersection point is too far away from the original vertex
            Eigen::Vector3f pq = polygon [result[nIdx]].getVector3fMap () - vertex;
            pq [2] = 0.0;

            float distance = pq.squaredNorm ();
            if (distance > threshold2)
            {
                // test whether the old point is inside the new polygon or outside
                if ((pq [0] * lines [rIdx] [0] + pq [1] * lines [rIdx] [1] < 0.0) &&
                        (pq [0] * lines [nIdx] [0] + pq [1] * lines [nIdx] [1] < 0.0) )
                {
                    float distance1 = lines [rIdx] [0] * polygon[result[nIdx]].x + lines [rIdx] [1] * polygon[result[nIdx]].y + lines [rIdx] [2];
                    float distance2 = lines [nIdx] [0] * polygon[result[nIdx]].x + lines [nIdx] [1] * polygon[result[nIdx]].y + lines [nIdx] [2];

                    point.x = polygon[result[nIdx]].x - distance1 * lines [rIdx] [0];
                    point.y = polygon[result[nIdx]].y - distance1 * lines [rIdx] [1];

                    approx_polygon.push_back (point);

                    vertex [0] = polygon[result[nIdx]].x - distance2 * lines [nIdx] [0];
                    vertex [1] = polygon[result[nIdx]].y - distance2 * lines [nIdx] [1];
                }
            }
            point.getVector3fMap () = vertex;
            approx_polygon.push_back (point);
        }
    }
    else
    {
        // we have a new polygon in results, but inverted (clockwise <-> counter-clockwise)
        for (std::vector<unsigned>::reverse_iterator it = result.rbegin (); it != result.rend (); ++it)
            approx_polygon.push_back (polygon [*it]);
    }
}