コード例 #1
0
template <typename PointT> bool
open_ptrack::detection::GroundplaneEstimation<PointT>::tooManyNaN(PointCloudConstPtr cloud, float max_ratio)
{
    int nan_counter = 0;
    for(unsigned int i = 0; i < cloud->size(); i++)
    {
        // If the point has a non-valid coordinate:
        if(isnan(cloud->points[i].x) || isnan(cloud->points[i].y) || isnan(cloud->points[i].z) )
        {   // update the counter:
            nan_counter++;
        }
    }

    // If the nan ratio is over max_ratio:
    if( (float) nan_counter/cloud->size() > max_ratio )
        return true;    // too many NaNs, frame invalid
    else
        return false;
}
コード例 #2
0
template<typename Point> void
TableObjectCluster<Point>::extractTableRoi2(const PointCloudConstPtr& pc_in,
                                    PointCloudPtr& hull,
                                    Eigen::Vector4f& plane_coeffs,
                                    pcl::PointCloud<Point>& pc_roi)
{
  pcl::PointIndices indices;
  for(unsigned int i=0; i<pc_in->size(); i++)
    indices.indices.push_back(i);
  // Project all points
  pcl::PointCloud<Point> projected_points;
  pcl::SampleConsensusModelPlane<Point> sacmodel (pc_in);
  //Eigen::Vector4f e_plane_coeffs(plane_coeffs.values[0],plane_coeffs.values[1],plane_coeffs.values[2],plane_coeffs.values[3]);
  sacmodel.projectPoints (indices.indices, plane_coeffs, projected_points, false);
  pcl::io::savePCDFileASCII ("/home/goa/tmp/proj.pcd", projected_points);
  pcl::io::savePCDFileASCII ("/home/goa/tmp/hull.pcd", *hull);
  pcl::PointIndices inliers;
  std::cout << "Coeffs:" << plane_coeffs << std::endl;
  int ctr=0, ctr2=0;
  for(unsigned int i=0; i<pc_in->size(); i++)
  {
    double distance = pcl::pointToPlaneDistanceSigned (pc_in->points[i], plane_coeffs);
    if (distance < height_min_ || distance > height_max_)
         continue;
    ctr++;
    if (!pcl::isXYPointIn2DXYPolygon (projected_points.points[i], *hull))
      continue;
    ctr2++;
    ROS_INFO("Point is in polygon");
    inliers.indices.push_back(i);
  }
  ROS_INFO("Pts in height: %d", ctr);
  ROS_INFO("Pts in poly: %d", ctr2);

  pcl::ExtractIndices<Point> extract_roi;
  extract_roi.setInputCloud (pc_in);
  extract_roi.setIndices (boost::make_shared<const pcl::PointIndices> (inliers));
  extract_roi.filter (pc_roi);
}
コード例 #3
0
ファイル: extract_pieces.cpp プロジェクト: rqou/prlite-pc
void
pcl_ros::PieceExtraction::input_indices_callback (
      const PointCloudConstPtr &cloud_transformed, const PointIndicesConstPtr &indices)
{
  // No subscribers, no work
  if (pub_output_.getNumSubscribers () <= 0)
    return;

  // If cloud is given, check if it's valid
  if (!isValid (cloud_transformed))
  {
    NODELET_ERROR ("[%s::input_indices_callback] Invalid input!", getName ().c_str ());
    return;
  }
  // If indices are given, check if they are valid
  if (indices && !isValid (indices))
  {
    NODELET_ERROR ("[%s::input_indices_callback] Invalid indices!", getName ().c_str ());
    return;
  }

  IndicesPtr indices_ptr;
  if (indices)
    indices_ptr = boost::make_shared <std::vector<int> > (indices->indices);

  
//  PointCloud cloud_transformed;
//  if (!pcl_ros::transformPointCloud (std::string("chess_board"), *cloud, cloud_transformed, tf_listener_))
//  {
//    NODELET_ERROR ("[%s::computePublish] Error converting output dataset from to chess_board", getName().c_str () );
//    return;
//  }

  impl_.setInputCloud (cloud_transformed->makeShared());
  impl_.setIndices (indices_ptr);

  std::vector<PointIndices> clusters;
  impl_.extract (clusters);

  // output ChessBoard
  chess_msgs::ChessBoard cb;
  for (size_t c = 0; c < clusters.size (); ++c)
  {
    chess_msgs::ChessPiece p;
    p.header.frame_id = cloud_transformed->header.frame_id;
    p.header.stamp = ros::Time::now();
    
    // find cluster centroid/color
    float x = 0; float y = 0; float z = 0; int color = 0;
    for (size_t i = 0; i < clusters[c].indices.size(); i++)
    {
        int j = clusters[c].indices[i];
        x += cloud_transformed->points[j].x;
        y += cloud_transformed->points[j].y;
        z += cloud_transformed->points[j].z;
        unsigned char * rgb = (unsigned char *) &(cloud_transformed->points[j].rgb);
        color += (rgb[0] + rgb[1] + rgb[2])/3;
    }
    x = x/clusters[c].indices.size();
    y = y/clusters[c].indices.size();
    z = z/clusters[c].indices.size();
    // set centroid
    p.pose.position.x = x;
    p.pose.position.y = y;
    p.pose.position.z = z;
    // set color
    color = color/clusters[c].indices.size();
    NODELET_DEBUG ("extract_pieces color %d\n", color);
    //printf ("extract_pieces color %d\n", color);
    // if(color > 100){
    // if(color > 30){
    if(color > 100){
        p.type = chess_msgs::ChessPiece::WHITE_UNKNOWN;   
    }else{
        p.type = chess_msgs::ChessPiece::BLACK_UNKNOWN;
    }

    bool new_ = true;
    // check if cluster overlaps any other
    for (size_t j = 0; j < cb.pieces.size(); j++)
    {
        if( (fabs(p.pose.position.x-cb.pieces[j].pose.position.x) < 0.012) &&
            (fabs(p.pose.position.y-cb.pieces[j].pose.position.y) < 0.012) )
        {
            cb.pieces[j].pose.position.x = (cb.pieces[j].pose.position.x + p.pose.position.x)/2;
            cb.pieces[j].pose.position.y = (cb.pieces[j].pose.position.y + p.pose.position.y)/2;
            cb.pieces[j].pose.position.z = (cb.pieces[j].pose.position.z + p.pose.position.z)/2;  
            new_ = false;
            break;
        }
    }  
    
    // add cluster if not overlapping with another
    if (new_){
        // TODO: add check for outside board, or too large to be a chess piece
        cb.pieces.push_back(p);
    }

  }
  pub_output_.publish(cb);
}
コード例 #4
0
bool RelativePoseEstimatorICPWithNormals<PointT> ::
computeRegistration(Pose3D& relative_pose,
                    PointCloudConstPtr source_cloud,
                    PointCloudConstPtr target_cloud,
                    PointCloudType& aligned_cloud)
{
#ifndef HAVE_PCL_GREATER_THAN_1_2_0
    return super::computeRegistration(relative_pose, source_cloud, target_cloud, aligned_cloud);
#else
    pcl::IterativeClosestPoint<PointT, PointT> reg;
    typedef pcl::registration::TransformationEstimationPointToPlaneLLS<PointT, PointT> PointToPlane;
    boost::shared_ptr<PointToPlane> point_to_plane (new PointToPlane);
    reg.setTransformationEstimation (point_to_plane);

    // typedef TransformationEstimationRGBD<PointT, PointT> TransformRGBD;
    // boost::shared_ptr<TransformRGBD> transform_rgbd (new TransformRGBD);
    // reg.setTransformationEstimation (transform_rgbd);

#ifdef HAVE_PCL_GREATER_THAN_1_6_0 // rejectors are not well supported before 1.7

    boost::shared_ptr<pcl::registration::CorrespondenceRejectorDistance> rejector_distance (new pcl::registration::CorrespondenceRejectorDistance);
    rejector_distance->setInputSource<PointT>(source_cloud);
    rejector_distance->setInputTarget<PointT>(target_cloud);
    rejector_distance->setMaximumDistance(m_distance_threshold);
    reg.addCorrespondenceRejector(rejector_distance);

    boost::shared_ptr<pcl::registration::CorrespondenceRejectorSurfaceNormal> rejector_normal (new pcl::registration::CorrespondenceRejectorSurfaceNormal);
    rejector_normal->setThreshold(cos(M_PI/4.f));
    rejector_normal->initializeDataContainer<PointT, PointT>();
    rejector_normal->setInputSource<PointT>(source_cloud);
    rejector_normal->setInputTarget<PointT>(target_cloud);
    rejector_normal->setInputNormals<PointT, PointT>(source_cloud);
    rejector_normal->setTargetNormals<PointT, PointT>(target_cloud);
    reg.addCorrespondenceRejector(rejector_normal);

    boost::shared_ptr<pcl::registration::CorrespondenceRejectorOneToOne> rejector_one_to_one (new pcl::registration::CorrespondenceRejectorOneToOne);
    reg.addCorrespondenceRejector(rejector_one_to_one);

    typedef pcl::registration::CorrespondenceRejectorSampleConsensus<PointT> RejectorConsensusT;
    boost::shared_ptr<RejectorConsensusT> rejector_ransac (new RejectorConsensusT());
    rejector_ransac->setInputSource(source_cloud);
    rejector_ransac->setInputTarget(target_cloud);
    rejector_ransac->setInlierThreshold(m_ransac_outlier_threshold);
    rejector_ransac->setMaxIterations(100);
    reg.addCorrespondenceRejector(rejector_ransac);

    boost::shared_ptr<pcl::registration::CorrespondenceRejectorVarTrimmed> rejector_var_trimmed (new pcl::registration::CorrespondenceRejectorVarTrimmed());
    rejector_var_trimmed->setInputSource<PointT>(source_cloud);
    rejector_var_trimmed->setInputTarget<PointT>(target_cloud);
    rejector_var_trimmed->setMinRatio(0.1f);
    rejector_var_trimmed->setMaxRatio(0.75f);
    reg.addCorrespondenceRejector(rejector_var_trimmed);

    boost::shared_ptr<pcl::registration::CorrespondenceRejectorTrimmed> rejector_trimmed (new pcl::registration::CorrespondenceRejectorTrimmed());
    rejector_trimmed->setMinCorrespondences(static_cast<int>(0.1f * source_cloud->size()));
    rejector_trimmed->setOverlapRatio(0.5f);
    reg.addCorrespondenceRejector(rejector_trimmed);
#endif

#if 0
    ntk::Mesh target_mesh;
    pointCloudToMesh(target_mesh, *target_cloud);
    target_mesh.saveToPlyFile("debug_target.ply");

    ntk::Mesh source_mesh;
    pointCloudToMesh(source_mesh, *source_cloud);
    source_mesh.saveToPlyFile("debug_source.ply");
#endif

    reg.setMaximumIterations (m_max_iterations);
    reg.setTransformationEpsilon (1e-10);
    reg.setMaxCorrespondenceDistance (m_distance_threshold);
    reg.setRANSACOutlierRejectionThreshold(m_ransac_outlier_threshold);
#ifdef HAVE_PCL_GREATER_THAN_1_6_0
    reg.setInputSource (source_cloud);
#else
    reg.setInputCloud (source_cloud);
#endif
    reg.setInputTarget (target_cloud);
    reg.align (aligned_cloud);

    if (!reg.hasConverged())
    {
      ntk_dbg(1) << "ICP did not converge, ignoring.";
      return false;
    }

    if (0)
    {
        ntk::Mesh mesh1, mesh2;
        pointCloudToMesh(mesh1, aligned_cloud);
        pointCloudToMesh(mesh2, *target_cloud);
        mesh1.saveToPlyFile("debug_icp_1.ply");
        mesh2.saveToPlyFile("debug_icp_2.ply");
    }

#if 0
    ntk::Mesh debug_mesh;
    pointCloudToMesh(debug_mesh, aligned_cloud);
    debug_mesh.saveToPlyFile("debug_aligned.ply");
#endif

    ntk_dbg_print(reg.getFitnessScore(), 1);

    Eigen::Matrix4f t = reg.getFinalTransformation ();
    cv::Mat1f T(4,4);
    //toOpencv(t,T);
    for (int r = 0; r < 4; ++r)
        for (int c = 0; c < 4; ++c)
            T(r,c) = t(r,c);

    relative_pose.setCameraTransform(T);
#endif
    return true;
}