Esempio n. 1
0
void FeatureCalculate::pca(CloudPtr cloud,PlanSegment &plane)
{    
    Eigen::MatrixXf X(3,cloud->size());
    double avr_x=0.0,avr_y=0.0,avr_z=0.0;
    for (int i = 0; i <cloud->size(); i++) {
        avr_x = avr_x  * double(i) / (i + 1) + cloud->points[i].x / double(i + 1);
        avr_y = avr_y  * double(i) / (i + 1) + cloud->points[i].y / double(i + 1);
        avr_z = avr_z  * double(i) / (i + 1) + cloud->points[i].z / double(i + 1);
    }
    for (int i=0;i<cloud->size();i++) 
    {
        X(0,i)=cloud->points[i].x-avr_x;
        X(1,i)=cloud->points[i].y-avr_y;
        X(2,i)=cloud->points[i].z-avr_z;
    }
    Eigen::MatrixXf XT(cloud->size(),3); 
    XT=X.transpose();
    Eigen::Matrix3f XXT; 
    XXT=X*XT;
    EIGEN_ALIGN16 Eigen::Vector3f::Scalar eigen_value;
    EIGEN_ALIGN16 Eigen::Vector3f eigen_vector;
    pcl::eigen33 (XXT, eigen_value, eigen_vector);
    plane.normal.normal_x = eigen_vector [0];
    plane.normal.normal_y = eigen_vector [1];
    plane.normal.normal_z = eigen_vector [2];
    plane.distance = - (plane.normal.normal_x * avr_x + plane.normal.normal_y * avr_y + plane.normal.normal_z * avr_z);

    float eig_sum = XXT.coeff (0) + XXT.coeff (4) + XXT.coeff (8);
    plane.min_value=eigen_value;
    if (eig_sum != 0)
    plane.normal.curvature = fabsf (eigen_value / eig_sum);
    else
        plane.normal.curvature = 0.0;
}
Esempio n. 2
0
void transform(string frame)
{    
    tf_listener->waitForTransform(frame.data(), pCloud_input->header.frame_id, ros::Time::now(), ros::Duration(5.0));
    pcl_ros::transformPointCloud(frame.data(), *(pCloud_input), *pCloud, *tf_listener);

    pCloud_input.reset(new Cloud);
    pCloud_input = pCloud;
    pCloud.reset(new Cloud);
}
Esempio n. 3
0
int
main (int argc, char **argv)
{
  double dist = 0.1;
  pcl::console::parse_argument (argc, argv, "-d", dist);

  double rans = 0.1;
  pcl::console::parse_argument (argc, argv, "-r", rans);

  int iter = 100;
  pcl::console::parse_argument (argc, argv, "-i", iter);

  pcl::registration::ELCH<PointType> elch;
  pcl::IterativeClosestPoint<PointType, PointType>::Ptr icp (new pcl::IterativeClosestPoint<PointType, PointType>);
  icp->setMaximumIterations (iter);
  icp->setMaxCorrespondenceDistance (dist);
  icp->setRANSACOutlierRejectionThreshold (rans);
  elch.setReg (icp);

  std::vector<int> pcd_indices;
  pcd_indices = pcl::console::parse_file_extension_argument (argc, argv, ".pcd");

  CloudVector clouds;
  for (size_t i = 0; i < pcd_indices.size (); i++)
  {
    CloudPtr pc (new Cloud);
    pcl::io::loadPCDFile (argv[pcd_indices[i]], *pc);
    clouds.push_back (CloudPair (argv[pcd_indices[i]], pc));
    std::cout << "loading file: " << argv[pcd_indices[i]] << " size: " << pc->size () << std::endl;
    elch.addPointCloud (clouds[i].second);
  }

  int first = 0, last = 0;

  for (size_t i = 0; i < clouds.size (); i++)
  {

    if (loopDetection (int (i), clouds, 3.0, first, last))
    {
      std::cout << "Loop between " << first << " (" << clouds[first].first << ") and " << last << " (" << clouds[last].first << ")" << std::endl;
      elch.setLoopStart (first);
      elch.setLoopEnd (last);
      elch.compute ();
    }
  }

  for (const auto &cloud : clouds)
  {
    std::string result_filename (cloud.first);
    result_filename = result_filename.substr (result_filename.rfind ('/') + 1);
    pcl::io::savePCDFileBinary (result_filename.c_str (), *(cloud.second));
    std::cout << "saving result to " << result_filename << std::endl;
  }

  return 0;
}
Esempio n. 4
0
void cuttingRange(double range)
{
    for(int i=0;i<pCloud_input->points.size();i++){
        double dist = sqrt(pow(pCloud_input->points.at(i).x,2)+pow(pCloud_input->points.at(i).y,2)+pow(pCloud_input->points.at(i).z,2));
        if(dist<range){
            pCloud->points.push_back(pCloud_input->points.at(i));
        }
    }
    pCloud_input.reset(new Cloud);
    pCloud_input = pCloud;
    pCloud.reset(new Cloud);
}
Esempio n. 5
0
void downsampling(double scale)
{
    CloudConstPtr pCloudConst = (CloudConstPtr)pCloud_input;

    pcl::VoxelGrid<PointT> vg;
    vg.setInputCloud(pCloudConst);
    vg.setLeafSize(scale, scale, scale); // down sampling using a leaf size of 'scale'
    vg.filter(*pCloud);

    pCloud_input.reset(new Cloud);
    pCloud_input = pCloud;
    pCloud.reset(new Cloud);
}
Esempio n. 6
0
/* ---[ */
int
main (int argc, char** argv)
{
  // Check whether we want to enable debug mode
  bool debug = false;
  parse_argument (argc, argv, "-debug", debug);
  if (debug)
    setVerbosityLevel (L_DEBUG);

  parse_argument (argc, argv, "-rejection", rejection);
  parse_argument (argc, argv, "-visualization", visualize);
  if (visualize)
    vis.reset (new PCLVisualizer ("Registration example"));

  // Parse the command line arguments for .pcd and .transform files
  std::vector<int> p_pcd_file_indices, p_tr_file_indices;
  p_pcd_file_indices = parse_file_extension_argument (argc, argv, ".pcd");
  if (p_pcd_file_indices.size () != 2)
  {
    print_error ("Need one input source PCD file and one input target PCD file to continue.\n");
    print_error ("Example: %s source.pcd target.pcd output.transform\n", argv[0]);
    return (-1);
  }
  p_tr_file_indices = parse_file_extension_argument (argc, argv, ".transform");
  if (p_tr_file_indices.size () != 1)
  {
    print_error ("Need one output transform file to continue.\n");
    print_error ("Example: %s source.pcd target.pcd output.transform\n", argv[0]);
    return (-1);
  }
  
  // Load the files
  print_info ("Loading %s as source and %s as target...\n", argv[p_pcd_file_indices[0]], argv[p_pcd_file_indices[1]]);
  src.reset (new PointCloud<PointT>);
  tgt.reset (new PointCloud<PointT>);
  if (loadPCDFile (argv[p_pcd_file_indices[0]], *src) == -1 || loadPCDFile (argv[p_pcd_file_indices[1]], *tgt) == -1)
  {
    print_error ("Error reading the input files!\n");
    return (-1);
  }

  // Compute the best transformtion
  Eigen::Matrix4d transform;
  icp (src, tgt, transform);

  saveTransform (argv[p_tr_file_indices[0]], transform);

  cerr.precision (15);
  std::cerr << transform << std::endl;
}
  void
      run ()
      {
        CloudPtr filecloud;
        pcl::Grabber* interface;
        if(filename_.empty()) {
          interface = new pcl::OpenNIGrabber (device_id_);

          boost::function<void (const CloudConstPtr&)> f = boost::bind (&PclPyramid::cloud_cb_, this, _1);
          boost::signals2::connection c = interface->registerCallback (f);

          interface->start ();
        }
        else 
        {
          pcd_cloud.reset (new pcl::PCLPointCloud2);
          if(pcd.read (filename_, *pcd_cloud, origin, orientation, version) < 0)
            cout << "file read failed" << endl;
          filecloud.reset (new Cloud);
          pcl::fromPCLPointCloud2(*pcd_cloud, *filecloud);
          cloud_  = filecloud;
        }


#ifdef NOVIEWER        
        if(!filename_.empty()) 
          get();
        else 
          while(TRUE)
          {
            if (cloud_)
              get();
            boost::this_thread::sleep (boost::posix_time::microseconds (10000));
          }
#else
        while ( !viewer.wasStopped () )
        {
          if (cloud_)
          {
            //the call to get() sets the cloud_ to null;
            viewer.showCloud (get ());
          }
          boost::this_thread::sleep (boost::posix_time::microseconds (10000));
        }
#endif

        if(filename_.empty()) {
          interface->stop ();
        }
      }
Esempio n. 8
0
void planeExtraction(int nPlane)
{
    CloudConstPtr pCloudConst = (CloudConstPtr)pCloud_input;

    // Create the segmentation object for the planar model and set all the parameters
    pcl::SACSegmentation<PointT> seg;
    pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
    pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
    //    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane (new pcl::PointCloud<pcl::PointXYZ> ());
    seg.setOptimizeCoefficients (true);
    seg.setModelType (pcl::SACMODEL_PLANE);
    seg.setMethodType (pcl::SAC_RANSAC);
    seg.setMaxIterations (100);
    seg.setDistanceThreshold (0.02);

    // plane segmentation
    for(int i=0;i<nPlane;i++)
    {
        // Segment the largest planar component from the remaining cloud
        seg.setInputCloud(pCloud_input);
        seg.segment (*inliers, *coefficients); //*
        if (inliers->indices.size () == 0)
        {
            std::cout << "Could not estimate a planar model for the given dataset." << std::endl;
            break;
        }

        // Extract the planar inliers from the input cloud
        pcl::ExtractIndices<PointT> extract;
        extract.setInputCloud (pCloudConst);
        extract.setIndices (inliers);
        extract.setNegative (false);

        // Write the planar inliers to disk
        CloudPtr cloud_plane;
        cloud_plane.reset(new Cloud);
        extract.filter (*cloud_plane); //*
//        clouds_plane.push_back(cloud_plane);
        //      std::cout << "PointCloud representing the planar component: " << cloud_plane->points.size () << " data points." << std::endl;

        // Remove the planar inliers, extract the rest
        extract.setNegative (true);
        extract.filter (*pCloud); //*
    }

    pCloud_input.reset(new Cloud);
    pCloud_input = pCloud;
    pCloud.reset(new Cloud);
}
void detection::GraspPointDetector::detect(const CloudPtr &input_object)
{
  clock_t beginCallback = clock();

  // If no cloud or no new images since last time, do nothing.
  if (input_object->size() == 0) return;

  world_obj_ = input_object->makeShared();
  pub_cluster_.publish(world_obj_);

  // Check if computation succeeded
  if (doProcessing())
    ROS_INFO("[%g ms] Detection Success", durationMillis(beginCallback));
  else
    ROS_ERROR("[%g ms] Detection Failed", durationMillis(beginCallback));
}
Esempio n. 10
0
void ObjectDetector::processCloud(const CloudConstPtr& cloud)
{
	cloud_pass_through_.reset(new Cloud);
	// Computation goes here
	pass_through_.setInputCloud(cloud);
	pass.filter(*cloud_pass_through_);
	
	// Estimate 3d convex hull
	pcl::ConvexHull<PointType> hr;
	hr.setInputCloud(cloud_pass_through_);
	cloud_hull_.reset(new Cloud);
	hr.reconstruct(*cloud_hull_, vertices_);
	
	cloud_ = cloud;
	new_cloud_ = true;
}
Esempio n. 11
0
//OpenNI Grabber's cloud Callback function
void
cloud_cb (const CloudConstPtr &cloud)
{
  boost::mutex::scoped_lock lock (mtx_);
  cloud_pass_.reset (new Cloud);
  cloud_pass_downsampled_.reset (new Cloud);
  filterPassThrough (cloud, *cloud_pass_);
  gridSampleApprox (cloud_pass_, *cloud_pass_downsampled_, downsampling_grid_size_);

  if(counter < 10){
	counter++;
  }else{
  	//Track the object
	tracker_->setInputCloud (cloud_pass_downsampled_);
	tracker_->compute ();
	new_cloud_ = true;
  }
}
Esempio n. 12
0
File: elch.cpp Progetto: Bardo91/pcl
int
main (int argc, char **argv)
{
  pcl::registration::ELCH<PointType> elch;
  pcl::IterativeClosestPoint<PointType, PointType>::Ptr icp (new pcl::IterativeClosestPoint<PointType, PointType>);
  icp->setMaximumIterations (100);
  icp->setMaxCorrespondenceDistance (0.1);
  icp->setRANSACOutlierRejectionThreshold (0.1);
  elch.setReg (icp);

  CloudVector clouds;
  for (int i = 1; i < argc; i++)
  {
    CloudPtr pc (new Cloud);
    pcl::io::loadPCDFile (argv[i], *pc);
    clouds.push_back (CloudPair (argv[i], pc));
    std::cout << "loading file: " << argv[i] << " size: " << pc->size () << std::endl;
    elch.addPointCloud (clouds[i-1].second);
  }

  int first = 0, last = 0;

  for (size_t i = 0; i < clouds.size (); i++)
  {

    if (loopDetection (int (i), clouds, 3.0, first, last))
    {
      std::cout << "Loop between " << first << " (" << clouds[first].first << ") and " << last << " (" << clouds[last].first << ")" << std::endl;
      elch.setLoopStart (first);
      elch.setLoopEnd (last);
      elch.compute ();
    }
  }

  for (size_t i = 0; i < clouds.size (); i++)
  {
    std::string result_filename (clouds[i].first);
    result_filename = result_filename.substr (result_filename.rfind ("/") + 1);
    pcl::io::savePCDFileBinary (result_filename.c_str (), *(clouds[i].second));
    std::cout << "saving result to " << result_filename << std::endl;
  }

  return 0;
}
Esempio n. 13
0
int main (int argc, char **argv)
{
  std::vector<int> pcd_indices;
  pcd_indices = pcl::console::parse_file_extension_argument (argc, argv, ".pcd");
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr sum_clouds (new pcl::PointCloud<pcl::PointXYZRGB>);

  CloudVector clouds;
  for (size_t i = 0; i < pcd_indices.size (); i++)
  {
    CloudPtr pc (new Cloud);
    pcl::io::loadPCDFile (argv[pcd_indices[i]], *pc);
    std::cout << "loading file: " << argv[pcd_indices[i]] << " size: " << pc->size () << std::endl;
    *sum_clouds += *pc;
  }
  std::string result_filename ("merged");
  result_filename += getTimeAsString();
  result_filename += ".pcd";
  pcl::io::savePCDFileASCII (result_filename.c_str (), *sum_clouds);
  std::cout << "saving result to " << result_filename << std::endl;

  return 0;
}
Esempio n. 14
0
void workingspace()
{
    double top = param_workspace_y + param_workspace_height/2;
    double bottom = param_workspace_y - param_workspace_height/2;
    double left = param_workspace_x - param_workspace_width/2;
    double right = param_workspace_x + param_workspace_width/2;
    double zbottom = param_workspace_z;
    double ztop = param_workspace_z + param_workspace_zheight;

    for(int i=0;i<pCloud_input->points.size();i++){
        PointT temp_point = pCloud_input->points[i];
        // cut off
        if(temp_point.x<right && temp_point.x>left && temp_point.y<top && temp_point.y>bottom && temp_point.z>zbottom && temp_point.z<ztop)
                pCloud->points.push_back(temp_point);
    }
    pCloud_input.reset(new Cloud);
    pCloud_input = pCloud;
    pCloud.reset(new Cloud);

    workspace.header.frame_id = param_frame_id.data();
    workspace.header.stamp = ros::Time::now();
    pub_workspace.publish (workspace);
}
Esempio n. 15
0
int Utils::subtractField(const NormalCloudPtr &cloud_in, const CloudPtr &cloud_out)
{
    cloud_out->clear();
    for (int i = 0; i < cloud_in->points.size(); i++)
    {
        Point pt;
        pt.x = cloud_in->points[i].x;
        pt.y = cloud_in->points[i].y;
        pt.z = cloud_in->points[i].z;
        pt.r = cloud_in->points[i].r;
        pt.g = cloud_in->points[i].g;
        pt.b = cloud_in->points[i].b;
        pt.a = cloud_in->points[i].a;
        cloud_out->points.push_back(pt);
    }
    return 0;
}
Esempio n. 16
0
/* ---[ */
int
main (int argc, char** argv)
{
  if (argc < 3)
  {
    std::cerr << "No test file given. Please download `bun0.pcd` and `milk.pcd` pass its path to the test." << std::endl;
    return (-1);
  }

  if (loadPCDFile<PointXYZ> (argv[1], cloud) < 0)
  {
    std::cerr << "Failed to read test file. Please download `bun0.pcd` and pass its path to the test." << std::endl;
    return (-1);
  }

  CloudPtr milk_loaded(new PointCloud<PointXYZ>());
  if (loadPCDFile<PointXYZ> (argv[2], *milk_loaded) < 0)
  {
    std::cerr << "Failed to read test file. Please download `milk.pcd` and pass its path to the test." << std::endl;
    return (-1);
  }

  indices.resize (cloud.points.size ());
  for (size_t i = 0; i < indices.size (); ++i)
  {
    indices[i] = static_cast<int>(i);
  }

  tree.reset (new search::KdTree<PointXYZ> (false));
  tree->setInputCloud (cloud.makeShared ());

  cloud_milk.reset(new PointCloud<PointXYZ>());
  CloudPtr grid;
  pcl::VoxelGrid < pcl::PointXYZ > grid_;
  grid_.setInputCloud (milk_loaded);
  grid_.setLeafSize (leaf_size_, leaf_size_, leaf_size_);
  grid_.filter (*cloud_milk);

  tree_milk.reset (new search::KdTree<PointXYZ> (false));
  tree_milk->setInputCloud (cloud_milk);

  testing::InitGoogleTest (&argc, argv);
  return (RUN_ALL_TESTS ());
}
Esempio n. 17
0
void FeatureCalculate::rpca_plane(CloudPtr cloud, PlanSegment &plane_rpca, float Pr, float epi,float reliability)
{
    CloudPtr cloud_h(new Cloud);
    CloudPtr cloud_final(new Cloud);
    int iteration_number;
    int h_free;
    int point_num=cloud->size();
    vector<PlanePoint> plane_points;
    plane_points.resize(point_num);
    h_free = int(reliability * point_num);
    iteration_number = compute_iteration_number(Pr, epi, h_free);
    vector<PlanSegment> planes;
    for (int i = 0; i < iteration_number; i++) {
        CloudPtr points(new Cloud);
        int num[3];
        for (int n = 0; n < 3; n++) {
            num[n] = rand() % point_num;
            points->push_back(cloud->points[num[n]]);
        }
        if (num[0] == num[1] || num[0] == num[2] || num[1] == num[2])
            continue;
        PlanSegment a_plane;
        pca(points,a_plane);
        for (int n = 0; n < point_num; n++) {
            plane_points[n].dis =compute_distance_from_point_to_plane(cloud->points[n],a_plane);
            plane_points[n].point=cloud->points[n];
        }
        std::sort(plane_points.begin(), plane_points.end(), CmpDis);
        for (int n = 0; n < h_free; n++) {
            CloudItem temp_point=plane_points[n].point;
            cloud_h->points.push_back(temp_point);
        }
        pca(cloud_h,a_plane);
        cloud_h->points.clear();
        planes.push_back(a_plane);
    }
    sort(planes.begin(), planes.end(), Cmpmin_value);
    PlanSegment final_plane;
    final_plane = planes[0];
    planes.clear();
    final_plane.points_id=plane_rpca.points_id;
    plane_rpca=final_plane;
}
Esempio n. 18
0
void cb_rgb(const pcl::PCLPointCloud2ConstPtr& input)
{
    static int cnt = 0;

    // input
    pCloud_input.reset(new Cloud);
    pCloud.reset(new Cloud);
    pcl::fromPCLPointCloud2(*input, *pCloud_input);
    if(isdebug) cout<<"I heard RGB, # of points: "<<pCloud_input->points.size()<<endl;

    // downsampling
    lastT = pcl::getTime ();
    downsampling(cont_sampling);
    if(isdebug) cout<<"downsampling computation time : "<<pcl::getTime()-lastT<<endl;
    if(isdebug) cout<<"downsampling end, # of points: "<<pCloud_input->points.size()<<endl;

    // transform to the given frame
    lastT = pcl::getTime ();
    transform(param_frame_id);
    if(isdebug) cout<<"transform computation time : "<<pcl::getTime()-lastT<<endl;

    // workspace
    lastT = pcl::getTime ();
    workingspace();
    if(isdebug) cout<<"workingspace computation time : "<<pcl::getTime()-lastT<<endl;
    if(isdebug) cout<<"workingspace end, # of points: "<<pCloud_input->points.size()<<endl;

//    // planeExtraction
//    lastT = pcl::getTime ();
//    planeExtraction(1);
//    if(isdebug) cout<<"planeExtraction computation time : "<<pcl::getTime()-lastT<<endl;
//    if(isdebug) cout<<"planeExtraction end, # of points: "<<pCloud_input->points.size()<<endl;

    // tracking
    lastT = pcl::getTime ();
    pctracking->run(pCloud_input);
    nowT = pcl::getTime ();
    if(isdebug) cout<<"total tracking computation time : "<<nowT-lastT<<endl;

    // publish filtered pointcloud
    pcl::PCLPointCloud2 output_filtered;
    pcl::toPCLPointCloud2(*pctracking->getFilteredPC(), output_filtered);
    output_filtered.header.frame_id=param_frame_id.data();    
    pub_filtered.publish (output_filtered);

    // publish segmented pointcloud
    pcl::PCLPointCloud2 output_segmented;
    pcl::toPCLPointCloud2(*pctracking->getSegmentedPC(), output_segmented);
    output_segmented.header.frame_id=param_frame_id.data();
    pub_segmented.publish (output_segmented);

    // publish model pointcloud
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr pCloudOut_model;
    pCloudOut_model.reset(new pcl::PointCloud<pcl::PointXYZRGB>);
    pctracking->object_tracks->toPointCloudXYZI_model(*pCloudOut_model);
    pcl::PCLPointCloud2 output_model;
    pcl::toPCLPointCloud2(*pCloudOut_model, output_model);
    output_model.header.frame_id=param_frame_id.data();
    pub_objectmodel.publish (output_model);

    // output tracks pointcloud
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr pCloudOut;
    pCloudOut.reset(new pcl::PointCloud<pcl::PointXYZRGB>);
    pctracking->object_tracks->toPointCloudXYZI(*pCloudOut);
    //    ROS_INFO("# of track points: %d", pCloudOut->points.size());

    // publish pointcloud(currentT) of tracks
    pcl::PCLPointCloud2 output;
    pcl::toPCLPointCloud2(*pCloudOut, output);
    output.header.frame_id=param_frame_id.data();
    pub_track.publish (output);


    //    // publish lastmodel
    //    pcl::PCLPointCloud2 output_lastmodel;
    //    pcl::toPCLPointCloud2(*pctracking->getLastModel(), output_lastmodel);
    //    output_lastmodel.header.frame_id=param_frame_id.data();
    //    pub_lastmodel.publish (output_lastmodel);

    //    // publish particles
    //    pcl::PCLPointCloud2 output_particles;
    //    pcl::toPCLPointCloud2(*pctracking->getParticles(), output_particles);
    //    output_particles.header.frame_id=param_frame_id.data();
    //    pub_particles.publish (output_particles);


    // publish gaussians
    pub_gaussians.publish(pctracking->object_tracks->oldGaussians());
    pub_gaussians.publish(pctracking->object_tracks->toMarkerGaussians());

    // publish gmms
//    pub_gmms.publish(pctracking->object_tracks->toMarkerGMMs());
//    pub_edges.publish(pctracking->object_tracks->toMarkerEdges());

    // publish delete id marker of deleted tracks
    pub_trackID.publish(pctracking->object_tracks->oldMarkerIDs());
    pub_trackID.publish(pctracking->object_tracks->toMarkerIDs());

    pCloudOut.reset();
    cnt++;

}
Esempio n. 19
0
float get_rot_icp_internal(CloudPtr cloud_src, CloudPtr cloud_temp, Matrix4f& mat_rot){
    int verbose = 0;
    bool do_scale = false;
    bool do_affine = false;
    bool bulkmode = false;

    TriMesh::set_verbose(verbose);
    TriMesh* mesh1 = new TriMesh();
    TriMesh* mesh2 = new TriMesh();
    mesh1->vertices.resize(cloud_src->size());
    mesh2->vertices.resize(cloud_temp->size());
    for (int i = 0; i < cloud_src->size(); i++) {
        mesh1->vertices[i] = point(cloud_src->points[i].x, cloud_src->points[i].y,
            cloud_src->points[i].z);
    }
    for (int i = 0; i < cloud_temp->size(); i++) {
        mesh2->vertices[i] = point(cloud_temp->points[i].x, cloud_temp->points[i].y,
            cloud_temp->points[i].z);
    }


    xform xf1;
    xform xf2;

    KDtree* kd1 = new KDtree(mesh1->vertices);
    KDtree* kd2 = new KDtree(mesh2->vertices);
    vector< float> weights1, weights2;

    if (bulkmode) {
        float area1 = mesh1->stat(TriMesh::STAT_TOTAL, TriMesh::STAT_FACEAREA);
        float area2 = mesh2->stat(TriMesh::STAT_TOTAL, TriMesh::STAT_FACEAREA);
        float overlap_area, overlap_dist;
        find_overlap(mesh1, mesh2, xf1, xf2, kd1, kd2, overlap_area, overlap_dist);
        float frac_overlap = overlap_area / min(area1, area2);
        if (frac_overlap < 0.1f) {
            TriMesh::eprintf("Insufficient overlap\n");
            exit(1);
        } else {
            TriMesh::dprintf("%.1f%% overlap\n", frac_overlap * 100.0);
        }
    }
    float err = ICP(mesh1, mesh2, xf1, xf2, kd1, kd2, weights1, weights2, 0, verbose,
        do_scale, do_affine);
    if (err >= 0.0f)
      err = ICP(mesh1, mesh2, xf1, xf2, kd1, kd2, weights1, weights2,0,
      verbose, do_scale, do_affine);

    if (err < 0.0f) {
        TriMesh::eprintf("ICP failed\n");
        //exit(1);
    }

    //TriMesh::eprintf("ICP succeeded - distance = %f\n", err);
    delete kd1;
    delete kd2;
    delete mesh1;
    delete mesh2;

    if (bulkmode) {
        xform xf12 = inv(xf2) * xf1;
        for (int i = 0; i < 4; i++) {
            for (int j = 0; j < 4; j++) {
                mat_rot(i, j) = xf12[i + 4 * j];
            }
        }
    } else {
        for (int i = 0; i < 4; i++) {
            for (int j = 0; j < 4; j++) {
                mat_rot(i, j) = xf2[i + 4 * j];
            }
        }
    }
    return err;
}
Esempio n. 20
0
int
main (int argc, char** argv)
{
  if (argc < 3)
    {
      PCL_WARN("Please set device_id pcd_filename(e.g. $ %s '#1' sample.pcd)\n", argv[0]);
      exit (1);
    }

  //read pcd file
  target_cloud.reset(new Cloud());
  if(pcl::io::loadPCDFile (argv[2], *target_cloud) == -1){
    std::cout << "pcd file not found" << std::endl;
    exit(-1);
  }

  std::string device_id = std::string (argv[1]);  

  counter = 0;

  //Set parameters
  new_cloud_  = false;
  downsampling_grid_size_ =  0.002;

  std::vector<double> default_step_covariance = std::vector<double> (6, 0.015 * 0.015);
  default_step_covariance[3] *= 40.0;
  default_step_covariance[4] *= 40.0;
  default_step_covariance[5] *= 40.0;

  std::vector<double> initial_noise_covariance = std::vector<double> (6, 0.00001);
  std::vector<double> default_initial_mean = std::vector<double> (6, 0.0);

  KLDAdaptiveParticleFilterOMPTracker<RefPointType, ParticleT>::Ptr tracker
    (new KLDAdaptiveParticleFilterOMPTracker<RefPointType, ParticleT> (8));

  ParticleT bin_size;
  bin_size.x = 0.1f;
  bin_size.y = 0.1f;
  bin_size.z = 0.1f;
  bin_size.roll = 0.1f;
  bin_size.pitch = 0.1f;
  bin_size.yaw = 0.1f;


  //Set all parameters for  KLDAdaptiveParticleFilterOMPTracker
  tracker->setMaximumParticleNum (1000);
  tracker->setDelta (0.99);
  tracker->setEpsilon (0.2);
  tracker->setBinSize (bin_size);

  //Set all parameters for  ParticleFilter
  tracker_ = tracker;
  tracker_->setTrans (Eigen::Affine3f::Identity ());
  tracker_->setStepNoiseCovariance (default_step_covariance);
  tracker_->setInitialNoiseCovariance (initial_noise_covariance);
  tracker_->setInitialNoiseMean (default_initial_mean);
  tracker_->setIterationNum (1);
  tracker_->setParticleNum (600);
  tracker_->setResampleLikelihoodThr(0.00);
  tracker_->setUseNormal (false);


  //Setup coherence object for tracking
  ApproxNearestPairPointCloudCoherence<RefPointType>::Ptr coherence
    (new ApproxNearestPairPointCloudCoherence<RefPointType>);

  DistanceCoherence<RefPointType>::Ptr distance_coherence
    (new DistanceCoherence<RefPointType>);
  coherence->addPointCoherence (distance_coherence);

  pcl::search::Octree<RefPointType>::Ptr search (new pcl::search::Octree<RefPointType> (0.01));
  coherence->setSearchMethod (search);
  coherence->setMaximumDistance (0.01);

  tracker_->setCloudCoherence (coherence);

  //prepare the model of tracker's target
  Eigen::Vector4f c;
  Eigen::Affine3f trans = Eigen::Affine3f::Identity ();
  CloudPtr transed_ref (new Cloud);
  CloudPtr transed_ref_downsampled (new Cloud);

  pcl::compute3DCentroid<RefPointType> (*target_cloud, c);
  trans.translation ().matrix () = Eigen::Vector3f (c[0], c[1], c[2]);
  pcl::transformPointCloud<RefPointType> (*target_cloud, *transed_ref, trans.inverse());
  gridSampleApprox (transed_ref, *transed_ref_downsampled, downsampling_grid_size_);

  //set reference model and trans
  tracker_->setReferenceCloud (transed_ref_downsampled);
  tracker_->setTrans (trans);

  //Setup OpenNIGrabber and viewer
  pcl::visualization::CloudViewer* viewer_ = new pcl::visualization::CloudViewer("PCL OpenNI Tracking Viewer");
  pcl::Grabber* interface = new pcl::OpenNIGrabber (device_id);
  boost::function<void (const CloudConstPtr&)> f =
    boost::bind (&cloud_cb, _1);
  interface->registerCallback (f);
    
  viewer_->runOnVisualizationThread (boost::bind(&viz_cb, _1), "viz_cb");

  //Start viewer and object tracking
  interface->start();
  while (!viewer_->wasStopped ())
    std::this_thread::sleep_for(1s);
  interface->stop();
}
Esempio n. 21
0
    void
    segment (const PointT &picked_point, 
             int picked_idx,
             PlanarRegion<PointT> &region,
             PointIndices &indices,
             CloudPtr &object)
    {
      // First frame is segmented using an organized multi plane segmentation approach from points and their normals
      if (!first_frame_)
        return;

      // Estimate normals in the cloud
      PointCloud<Normal>::Ptr normal_cloud (new PointCloud<Normal>);
      ne_.setInputCloud (search_.getInputCloud ());
      ne_.compute (*normal_cloud);

      plane_comparator_->setDistanceMap (ne_.getDistanceMap ());

      // Segment out all planes
      mps_.setInputNormals (normal_cloud);
      mps_.setInputCloud (search_.getInputCloud ());

      // Use one of the overloaded segmentAndRefine calls to get all the information that we want out
      vector<PlanarRegion<PointT> > regions;
      vector<ModelCoefficients> model_coefficients;
      vector<PointIndices> inlier_indices;  
      PointCloud<Label>::Ptr labels (new PointCloud<Label>);
      vector<PointIndices> label_indices;
      vector<PointIndices> boundary_indices;
      mps_.segmentAndRefine (regions, model_coefficients, inlier_indices, labels, label_indices, boundary_indices);
      PCL_DEBUG ("Number of planar regions detected: %zu for a cloud of %zu points and %zu normals.\n", regions.size (), search_.getInputCloud ()->points.size (), normal_cloud->points.size ());

      double max_dist = numeric_limits<double>::max ();
      // Compute the distances from all the planar regions to the picked point, and select the closest region
      int idx = -1;
      for (size_t i = 0; i < regions.size (); ++i)
      {
        double dist = pointToPlaneDistance (picked_point, regions[i].getCoefficients ()); 
        if (dist < max_dist)
        {
          max_dist = dist;
          idx = static_cast<int> (i);
        }
      }

      PointIndices::Ptr plane_boundary_indices;
      // Get the plane that holds the object of interest
      if (idx != -1)
      {
        region = regions[idx]; 
        plane_indices_.reset (new PointIndices (inlier_indices[idx]));
        plane_boundary_indices.reset (new PointIndices (boundary_indices[idx]));
      }

      // Segment the object of interest
      if (plane_boundary_indices && !plane_boundary_indices->indices.empty ())
      {
        object.reset (new Cloud);
        segmentObject (picked_idx, search_.getInputCloud (), plane_indices_, plane_boundary_indices, *object);

        // Save to disk
        //pcl::io::saveTARPointCloud ("output.ltm", *object, "object.pcd");
      }
      first_frame_ = false;
    }
Esempio n. 22
0
File: lum.cpp Progetto: 2php/pcl
int
main (int argc, char **argv)
{
  double dist = 2.5;
  pcl::console::parse_argument (argc, argv, "-d", dist);

  int iter = 10;
  pcl::console::parse_argument (argc, argv, "-i", iter);

  int lumIter = 1;
  pcl::console::parse_argument (argc, argv, "-l", lumIter);

  double loopDist = 5.0;
  pcl::console::parse_argument (argc, argv, "-D", loopDist);

  int loopCount = 20;
  pcl::console::parse_argument (argc, argv, "-c", loopCount);

  pcl::registration::LUM<PointType> lum;
  lum.setMaxIterations (lumIter);
  lum.setConvergenceThreshold (0.001f);

  std::vector<int> pcd_indices;
  pcd_indices = pcl::console::parse_file_extension_argument (argc, argv, ".pcd");

  CloudVector clouds;
  for (size_t i = 0; i < pcd_indices.size (); i++)
  {
    CloudPtr pc (new Cloud);
    pcl::io::loadPCDFile (argv[pcd_indices[i]], *pc);
    clouds.push_back (CloudPair (argv[pcd_indices[i]], pc));
    std::cout << "loading file: " << argv[pcd_indices[i]] << " size: " << pc->size () << std::endl;
    lum.addPointCloud (clouds[i].second);
  }

  for (int i = 0; i < iter; i++)
  {
    for (size_t i = 1; i < clouds.size (); i++)
      for (size_t j = 0; j < i; j++)
      {
        Eigen::Vector4f ci, cj;
        pcl::compute3DCentroid (*(clouds[i].second), ci);
        pcl::compute3DCentroid (*(clouds[j].second), cj);
        Eigen::Vector4f diff = ci - cj;

        //std::cout << i << " " << j << " " << diff.norm () << std::endl;

        if(diff.norm () < loopDist && (i - j == 1 || i - j > loopCount))
        {
          if(i - j > loopCount)
            std::cout << "add connection between " << i << " (" << clouds[i].first << ") and " << j << " (" << clouds[j].first << ")" << std::endl;
          pcl::registration::CorrespondenceEstimation<PointType, PointType> ce;
          ce.setInputTarget (clouds[i].second);
          ce.setInputSource (clouds[j].second);
          pcl::CorrespondencesPtr corr (new pcl::Correspondences);
          ce.determineCorrespondences (*corr, dist);
          if (corr->size () > 2)
            lum.setCorrespondences (j, i, corr);
        }
      }

    lum.compute ();

    for(size_t i = 0; i < lum.getNumVertices (); i++)
    {
      //std::cout << i << ": " << lum.getTransformation (i) (0, 3) << " " << lum.getTransformation (i) (1, 3) << " " << lum.getTransformation (i) (2, 3) << std::endl;
      clouds[i].second = lum.getTransformedCloud (i);
    }
  }

  for(size_t i = 0; i < lum.getNumVertices (); i++)
  {
    std::string result_filename (clouds[i].first);
    result_filename = result_filename.substr (result_filename.rfind ("/") + 1);
    pcl::io::savePCDFileBinary (result_filename.c_str (), *(clouds[i].second));
    //std::cout << "saving result to " << result_filename << std::endl;
  }

  return 0;
}
Esempio n. 23
0
void VisMotCoord::getPCScene(CloudPtr &pc_out)
{
    pc_out.reset(new Cloud);
    *pc_out = *scene.getPCScene();
}
Esempio n. 24
0
int main(int argc, char *argv[]){
    if(argc>1){
        CloudPtr cloud (new Cloud);
        ColorCloudPtr color_cloud (new ColorCloud);
        if (pcl::io::loadPCDFile(argv[1], *cloud) == -1) //* load the file
        {
            PCL_ERROR ("Couldn't read file.\n");
            return -1;
        }
        for(int i = 0; i<cloud->size(); i++){
            Point p1 = cloud->points[i];
            ColorPoint p2;
            p2.x = p1.x;
            p2.y = p1.y;
            p2.z = p1.z;
            p2.r = 0;
            p2.g = 0;
            p2.b = 0;
            color_cloud->push_back(p2);
        }
        
        float centroid[3];
        calculateCentroid(cloud,centroid);


        pcl::PolygonMesh pm;
        pcl::ConvexHull<ColorPoint> chull;
        chull.setInputCloud(color_cloud);
        chull.reconstruct(pm);

        pcl::fromROSMsg(pm.cloud,*color_cloud);
        vector<float> distances(color_cloud->size(),999999);

        for(int i = 0; i<pm.polygons.size(); i++){
            int i0 = pm.polygons[i].vertices[0];
            int i1 = pm.polygons[i].vertices[1];
            int i2 = pm.polygons[i].vertices[2];
            ColorPoint p0 = color_cloud->points[i0];
            ColorPoint p1 = color_cloud->points[i1];
            ColorPoint p2 = color_cloud->points[i2];

            Eigen::Vector3f v0;
            Eigen::Vector3f v1;
            Eigen::Vector3f v2;
            v0 << p0.x,p0.y,p0.z;
            v1 << p1.x,p1.y,p1.z;
            v2 << p2.x,p2.y,p2.z;
            Eigen::Vector3f normal = (v1-v0).cross(v2-v0).normalized();

            Eigen::Vector3f p;
            p << centroid[0], centroid[1], centroid[2];

            Eigen::Vector3f to_p = p-v0;
            Eigen::Vector3f projected_p = p-(normal* normal.dot(to_p));

            float dist0 = (v1-v0).dot(projected_p-v0)/(v1-v0).norm();
            float dist1 = (v2-v0).dot(projected_p-v0)/(v2-v0).norm();
            distances[i0] = min(distances[i0],(dist0+dist1));
            distances[i1] = min(distances[i1],(dist0+dist1));
            distances[i2] = min(distances[i2],(dist0+dist1));
        }
        float max_dist = *max_element(distances.begin(),distances.end());

        float thresh = 500;
        for(int i = 0; i<color_cloud->size(); i++){
            ColorPoint* p1 = &color_cloud->points[i];
            float color = 255 - distances[i]*(255/max_dist);
            if(distances[i]>thresh)
                color = 0;
            p1->r = color;
            p1->g = 0;
            p1->b = 0;
        }

        chull.setInputCloud(color_cloud);
        chull.reconstruct(pm);

        pcl::io::saveVTKFile("hull.vtk",pm);
    }
}
void detection::GraspPointDetector::setTable(const CloudPtr &table_cloud, const pcl::ModelCoefficientsPtr table_plane)
{
  table_plane_ = boost::make_shared<ModelCoefficients>(*table_plane);
  table_cloud_ = table_cloud->makeShared();
}
Esempio n. 26
0
void FeatureCalculate::rpca(CloudPtr cloud,NormalPtr normals, int num_of_neighbors, float Pr, float epi,float reliability)
{
    pcl::KdTreeFLANN<CloudItem> kdtree;
    kdtree.setInputCloud(cloud);
    normals->resize(cloud->size());
    #pragma omp parallel for
    for (int j = 0; j < cloud->size(); j++) {
        CloudPtr cloud_h(new Cloud);
        CloudPtr cloud_final(new Cloud);
        vector<int> point_id_search;
        vector<float> point_distance;
        int point_num = kdtree.nearestKSearch(j,num_of_neighbors,point_id_search,point_distance);
        point_distance.clear();
        vector<PlanePoint> plane_points;
        plane_points.resize(point_num);
        if (point_num > 3) {
            int iteration_number;
            int h_free;
            h_free = int(reliability * point_num);
            iteration_number = compute_iteration_number(Pr, epi, h_free);
            vector<PlanSegment> planes;
            for (int i = 0; i < iteration_number; i++) {
                CloudPtr points(new Cloud);
                int num[3];
                for (int n = 0; n < 3; n++) {
                    num[n] = rand() % point_num;
                    points->push_back(cloud->points[point_id_search[num[n]]]);
                }
                if (num[0] == num[1] || num[0] == num[2] || num[1] == num[2])
                    continue;
                PlanSegment a_plane;
                pca(points,a_plane);
                for (int n = 0; n < point_num; n++) {
                    plane_points[n].dis =compute_distance_from_point_to_plane(cloud->points[point_id_search[n]],a_plane);
                    plane_points[n].point=cloud->points[point_id_search[n]];
                }
                std::sort(plane_points.begin(), plane_points.end(), CmpDis);
                for (int n = 0; n < h_free; n++) {
                    CloudItem temp_point=plane_points[n].point;
                    cloud_h->points.push_back(temp_point);
                }
                pca(cloud_h,a_plane);
                cloud_h->points.clear();
                planes.push_back(a_plane);
            }
            sort(planes.begin(), planes.end(), Cmpmin_value);
            PlanSegment final_plane;
            final_plane = planes[0];
            planes.clear();
            vector<float> distance;
            vector<float> distance_sort;
            for (int n = 0; n < point_num; n++) {
                float dis_from_point_plane;
                dis_from_point_plane =compute_distance_from_point_to_plane(cloud->points[point_id_search[n]],final_plane);
                distance.push_back(dis_from_point_plane);
                distance_sort.push_back(dis_from_point_plane);
            }
            sort(distance_sort.begin(), distance_sort.end());
            float distance_median;
            distance_median = distance_sort[point_num / 2];
            distance_sort.clear();
            float MAD;
            vector<float> temp_MAD;
            for (int n = 0; n < point_num; n++) {
                temp_MAD.push_back(abs(distance[n] - distance_median));
            }
            sort(temp_MAD.begin(), temp_MAD.end());
            MAD = 1.4826 * temp_MAD[point_num / 2];
            if (MAD == 0) {
                for (int n = 0; n < point_num; n++) {
                    CloudItem temp_point=cloud->points[point_id_search[n]];
                    cloud_final->points.push_back(temp_point);
                }
            } else {
                for (int n = 0; n < point_num; n++) {
                    float Rz;
                    Rz = (abs(distance[n] - distance_median)) / MAD;
                    if (Rz < 2.5) {
                        CloudItem temp_point=cloud->points[point_id_search[n]];
                        cloud_final->points.push_back(temp_point);
                    }
                }
            }
            point_id_search.clear();
            if (cloud_final->points.size() > 3)
            {
                pca(cloud_final,final_plane);
                normals->points[j]= final_plane.normal;
            } else {
                normals->points[j].normal_x = 0.0;
                normals->points[j].normal_y = 0.0;
                normals->points[j].normal_z = 0.0;
                normals->points[j].curvature = 1.0;
            }
            cloud_final->clear();
        } else {
            normals->points[j].normal_x = 0.0;
            normals->points[j].normal_y = 0.0;
            normals->points[j].normal_z = 0.0;
            normals->points[j].curvature = 1.0;
        }    
    }
}